视觉slam14讲——第7讲 视觉里程计1
程序员文章站
2022-04-17 17:25:54
...
本系列文章是记录学习高翔所著《视觉slam14讲》的内容总结，文中的主要文字和代码、图片都是引用自课本和高翔博士的博客。代码运行效果是在自己电脑上实际运行得出。手动记录主要是为了深入理解
第7讲 视觉里程计1
这一讲笔记记录的主要下面几条内容
- 1 特征点如何提取并且匹配
- 2 利用对极约束方法从
2D−2D 匹配好的特征点估计相机运动 - 3 使用三角测量方法从
2D−2D 匹配估计一个点的空间位置 - 4
3D−2D 的PnP 问题的线性解法和BoundAdjustment 解法 - 5
3D−3D 的ICP 问题的线性解法和BoundAdjustment 解法
做了一个简单的流程图概括上述内容
1 特征点法
1.1 人工设计的特征点有四个特征
- 可重复性Repeatability
- 可区别性Distinctiveness
- 高效率Efficiency
- 本地性Locality
1.2 特征点组成：
- 关键点Key-Point
- 描述子Descriptor
外观相似的特征具有相似的描述子
1.3 常见的特征点
- SIFT 尺度不变特征变换
- SURF
- ORB 具有代表性的实时图像特征
2 ORB特征点
提取ORB特征的两个步骤：
- FAST角点提取
- BRIEF描述子
2.1 改进的FAST 关键点(Oriented FAST)
FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的一个思想是：如果一个像素与邻域像素的差别较大（过亮或过暗），那么它更可能是角点。
改进的FAST 关键点添加了方向和尺度信息。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转由灰度质心法来表示，步骤如下
(1) 定义图像块的矩
(2) 图像块的质心
(3) 特征点方向
2.2 BRIEF描述子
BRIEF(Binary Robust Independent Elementary Feature)是二进制描述子，它的描述向量是由许多个0和1组成，这里的0和1编码了关键点附近的两个像素
大体上按照某种概率分布，随机的挑选
p 和q 的位置，最终选取128对这样的p 和q 构成128维向量。
这段话看的一知半解，高博士说阅读BRIEF论文或者OPENCV源码可以查看具体实现。
这是使用opencv算法，提取出的ORB特征点，
作者高翔博士关于特征提取的代码如下
//-- 读取图像
Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );
//-- 初始化
std::vector<KeyPoint> keypoints_1, keypoints_2;
Mat descriptors_1, descriptors_2;
#ifndef __OPENCV_2__
// used in OpenCV3
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
#else
// use this if you are in OpenCV2
Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
#endif
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" );
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect ( img_1, keypoints_1 );
detector->detect ( img_2, keypoints_2 );
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute ( img_1, keypoints_1, descriptors_1 );
descriptor->compute ( img_2, keypoints_2, descriptors_2 );
Mat outimg1;
drawKeypoints( img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
imshow("ORB特征点", outimg1);2.3 特征匹配
特征匹配解决了SLAM中的数据关联问题(data association)，即确定当前帧的图像特征与前一帧的图像对应关系，通过描述子的差异判断哪些特征为同一个点。
2.3.1 存在的问题
由于场景图像中存在大量重复的纹理，使得描述特征非常相似，导致错误匹配广泛存在。
2.3.2 匹配方法
在图像
对每一个特征点
xmt 和所有的xnt+1 测量描述子的距离，然后排序，取最近的一个点作为匹配点。
描述子 距离表示两个特征之间的相似程度，在实际应用中取不同的距离度量范数。
- 浮点类型的描述子使用欧式距离度量
- 二进制类型描述子
(eg:BRIEF) 使用汉明距离(HammingDistance) 作为度量。比如两个二进制串的汉明距离表示不同位的个数。
特征点数量过大时，暴力匹配算法不能满足slam实时性的要求，它的改进算法是快速最近邻算法
所有匹配点对如下图
优化后匹配的点对
特征匹配的代码如下，这个部分是紧接这上面的特征点提取部分的代码的。
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> matches;
//BFMatcher matcher ( NORM_HAMMING );
matcher->match ( descriptors_1, descriptors_2, matches );
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for ( int i = 0; i < descriptors_1.rows; i++ )
{
double dist = matches[i].distance;
if ( dist < min_dist ) min_dist = dist;
if ( dist > max_dist ) max_dist = dist;
}
// 仅供娱乐的写法
min_dist = min_element( matches.begin(), matches.end(), [](const DMatch & m1, const DMatch & m2) {return m1.distance < m2.distance;} )->distance;
max_dist = max_element( matches.begin(), matches.end(), [](const DMatch & m1, const DMatch & m2) {return m1.distance < m2.distance;} )->distance;
printf ( "-- Max dist : %f \n", max_dist );
printf ( "-- Min dist : %f \n", min_dist );
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
std::vector< DMatch > good_matches;
for ( int i = 0; i < descriptors_1.rows; i++ )
{
if ( matches[i].distance <= max ( 2 * min_dist, 30.0 ) )
{
good_matches.push_back ( matches[i] );
}
}
//-- 第五步:绘制匹配结果
Mat img_match;
Mat img_goodmatch;
drawMatches ( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
drawMatches ( img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch );
imshow ( "所有匹配点对", img_match );
imshow ( "优化后匹配点对", img_goodmatch );2.4 特征提取和匹配代码实践的步骤分析
(1)读取图像
(2)初始化 std::vector< KeyPoint > 类型的特征点和 Mat 类型的描述子
(3)定义Ptr< FeatureDetector >类型的检测对象,使用这个detect去检测两个图像，然后返回值存储在刚才定义的 std::vector< KeyPoint> 类型的特征点。
定义Ptr< DescriptorExtractor >类型的特征点提取对象，使用这个descriptor，根据计算出的关键点，计算BRIEF描述子(4)使用OpenCV中的huigui模块，将特征点画出来
(5) 定义Ptr< DescriptorMatcher > 类型的匹配对象，用这个matcher，输入刚刚计算出的描述子descriptors_1，descriptors_2，对两幅图片的BRIEF描述子进行匹配，返回为vector< DMatch > 类型的匹配对象matches.
(6)对匹配点进行筛选，找出匹配点之间最小距离和最大距离，也就是找到最相似和最不相似的两组点之间的距离。当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限。
(7) 绘制匹配结果
最后高翔博士的输出结果是如下
可以看到最近的两个量的匹配的距离是4，最远的匹配距离是95。当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限。
3 2D-2D对极几何
根据得到的若干对匹配点，就可以通过这些二位图像的对应关系，恢复出两帧图像摄像机的运动。
对极几何约束如下图
定义中间部分的基础矩阵(Fundamental Matrix)
也就得到
对极约束给出了两个匹配点之间的空间关系，于是，问题简化为：
- 由匹配点的像素位置计算
E 或者F - 由
E 或F 求R,t
3.1 本质矩阵(Essential Matrix)E
关于
- 本质矩阵由对极约束定义，由于对极约束是等式为零的约束，所以对
E 乘以乘任意非零常数依然满足，称E 在不同尺度下是等价的。 -
E=t^R ,E 的奇异值(Singular Value)必定是[σ,σ,0]T 的形式，这称为本质矩阵E 的内在性质 - 由于平移旋转共6个*度，所以
t^R 有6个*度。但是由于尺度等价性，E 实际上共五个*度
使用经典的八点法求解
八点法的总结：
- 用于单目视觉的初始化
- 尺度不确定性：归一化 t 或特征点的平均深度
- 纯旋转问题：t=0 时无法求解
- 多于八对点时：最小二乘或随机采样一致性（RANSAC）
3.2 单应矩阵H(Homography)
它描述的是两个平面之间的映射关系，如果场景中的特征点都落在同一平面上(比如情面、地面)，则可以通过单应性进行运动估计。
假设特征点落在平面
也就是
代入最开始式子
得到一个从图像坐标
记作
它的定义与平移、旋转和平面的参数相关。
求解单应矩阵使用线性变换法(Direct Linear Transform), 与求解本质矩阵类似，求出来后需要对其进行分解才能得到
3.3 对极约束求相机运动代码实践
void pose_estimation_2d2d ( std::vector<KeyPoint> keypoints_1,
std::vector<KeyPoint> keypoints_2,
std::vector< DMatch > matches,
Mat& R, Mat& t )
{
// 相机内参,TUM Freiburg2
Mat K = ( Mat_<double> ( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
//-- 把匹配点转换为vector<Point2f>的形式
vector<Point2f> points1;
vector<Point2f> points2;
for ( int i = 0; i < ( int ) matches.size(); i++ )
{
points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
}
//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;
//-- 计算本质矩阵
Point2d principal_point ( 325.1, 249.7 ); //相机光心, TUM dataset标定值
double focal_length = 521; //相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
cout << "essential_matrix is " << endl << essential_matrix << endl;
//-- 计算单应矩阵
Mat homography_matrix;
homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
cout << "homography_matrix is " << endl << homography_matrix << endl;
//-- 从本质矩阵中恢复旋转和平移信息.
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
cout << "R is " << endl << R << endl;
cout << "t is " << endl << t << endl;
}演示了如何使用2D-2D的特征匹配估计相机运动代码如下
//-- 读取图像
Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
cout << "一共找到了" << matches.size() << "组匹配点" << endl;
//-- 估计两张图像间运动
Mat R, t;
pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );
//-- 验证E=t^R*scale
Mat t_x = ( Mat_<double> ( 3, 3 ) <<
0, -t.at<double> ( 2, 0 ), t.at<double> ( 1, 0 ),
t.at<double> ( 2, 0 ), 0, -t.at<double> ( 0, 0 ),
-t.at<double> ( 1.0 ), t.at<double> ( 0, 0 ), 0 );
cout << "t^R=" << endl << t_x*R << endl;
//-- 验证对极约束
Mat K = ( Mat_<double> ( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
for ( DMatch m : matches )
{
Point2d pt1 = pixel2cam ( keypoints_1[ m.queryIdx ].pt, K );
Mat y1 = ( Mat_<double> ( 3, 1 ) << pt1.x, pt1.y, 1 );
Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
Mat y2 = ( Mat_<double> ( 3, 1 ) << pt2.x, pt2.y, 1 );
Mat d = y2.t() * t_x * R * y1;
cout << "epipolar constraint = " << d << endl;
}最后程序执行结果如下
观察结果可以看到验证对极约束的精度在小数点后三位
3.4 总结
尺度不确定性
对t 长度的归一化，直接导致单目视觉的尺度不确定性(Scale Ambiguity),在单目视觉中对两张图像的t 归一化相当于固定了制度，我们不知道长度是多少，但是我们这是t 的单位是1，计算相机运动和特征点的3D 的位置，这被称为单目视觉SLAM的初始化。初始化的纯旋转问题
单目视觉初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，从E 分解到R,t 的过程中，导致t 为零，那么得到的E 也为零，这将导致无法求解R 。不过此时可以依靠H 求取旋转，但是仅仅有旋转时，无法使用三角测量估计特征点的空间位置。-
对于8对点的情况
当给定匹配点多余8个的时候，比如算出79对匹配点，使用一个最小二乘计算, 不过在存在错误匹配的情况下使用随机抽样一致算法(Random Sample Concensus,RANSAC)，可以处理带有错误匹配的数据。mine||Ae||22=mineeTATAe
4 三角测量(Triangulation)
4.1 三角测量理论介绍
上一步得到运动之后，下一步需要用相机的运动估计特征点的空间位置。
三角测量指的是通过在两处观察同一个点的夹角，从而确定改点的距离。最早是由高斯提出的。由于噪声的影响，图中的两条直线无法相交，因此可以通过最小二乘求解。
按照对极几何的定义，设
现在
由此算出
4.2 三角测量代码实践
void triangulation (
const vector< KeyPoint >& keypoint_1,
const vector< KeyPoint >& keypoint_2,
const std::vector< DMatch >& matches,
const Mat& R, const Mat& t,
vector< Point3d >& points )
{
Mat T1 = (Mat_<float> (3, 4) <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0);
Mat T2 = (Mat_<float> (3, 4) <<
R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
);
Mat K = ( Mat_<double> ( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point2f> pts_1, pts_2;
for ( DMatch m : matches )
{
// 将像素坐标转换至相机坐标
pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) );
pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) );
}
Mat pts_4d;
cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );
// 转换成非齐次坐标
for ( int i = 0; i < pts_4d.cols; i++ )
{
Mat x = pts_4d.col(i);
x /= x.at<float>(3, 0); // 归一化
Point3d p (
x.at<float>(0, 0),
x.at<float>(1, 0),
x.at<float>(2, 0)
);
points.push_back( p );
}
}验证三角化点与特征点的重投影关系代码如下，
//-- 验证三角化点与特征点的重投影关系
Mat K = ( Mat_<double> ( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
for ( int i = 0; i < matches.size(); i++ )
{
Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
Point2d pt1_cam_3d(
points[i].x / points[i].z,
points[i].y / points[i].z
);
cout << "point in the first camera frame: " << pt1_cam << endl;
cout << "point projected from 3D " << pt1_cam_3d << ", d=" << points[i].z << endl;
// 第二个图
Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
Mat pt2_trans = R * ( Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z ) + t;
pt2_trans /= pt2_trans.at<double>(2, 0);
cout << "point in the second camera frame: " << pt2_cam << endl;
cout << "point reprojected from second frame: " << pt2_trans.t() << endl;
cout << endl;
}执行结果如下图
结果显示了每个空间点在两个相机坐标系下的投影坐标和像素坐标，相当于
point in the first camera frame: [-0.0136303, -0.302687]
point projected from 3D [-0.0161972, -0.318825], d=19.9868比如这一对结果显示三角化特征点距离为19.9868，但是并不知道它代表多少米。
4.3 三角测量的矛盾
5 3D-2D : PnP
5.1 PnP 简介
其中，特征点的3D位置可以由三角化或者RGB-D相机深度图获得。
单目双目使用
* 双目相机或者RGB-D相机的视觉里程计中可以使用
* 单目视觉需要先进性初始化，然后才能使用
解决
- 3对点估计位姿的
P3P - 直接线性变换
DLT -
EPnP(EfficientPnP) -
UPnP - 非线性优化构建最小二乘并迭代求解(Bundle Adjustment)
5.2 PnP 求解——直接线性变换(DLT )
最少通过6对匹配点实现矩阵
T 的线性求解，当匹配点多于6对时，可以使用SVD 对超定方程求最小二乘解。在
DLT 中，将T 看做12个未知数，忽略之间的联系，求解出的R 不一定满足S(O3) 约束。对于旋转矩阵，必须针对DLT 估计的T 左边3×3 的矩阵块，寻找一个最好的旋转矩阵进行近似，可以使用QR 分解完成。相当于把结果从矩阵空间投影到SE(3) 流形上。
5.3 PnP 求解——P3P
5.3.1 P3P 原理
输出数据是得到
这样就得到了
5.3.2 P3P 存在问题
-
P3P 只利用3个点的信息，当给定匹配点多余3组时，难以利用更多信息 - 如果
3D 点或者2D 点受噪声影响，或者存在误匹配，算法失效
后续人们提出的
在
SLAM 中，通常先使用P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment) 。
5.4 PnP 求解——Bundle Adjustment
除了使用线性方法之外，还可以把
- 线性方法：先求相机位姿，再求空间点位置
- 非线性优化方法：把他们都看成优化变量，放在一起进行优化。这是一种非常通用的求解方法。
在
像素与空间点位置关系是
把误差求和构建最小二乘问题，然后寻找最好的相机位姿，使其最小化
该问题的误差项是将像素坐标(观测到的投影位置)与
5.5 PnP 求解代码实践
5.5.1 使用EPnP 求解位姿
利用
下面一段代码是在得到配对特征点后，在第一个图的深度图中寻找他们的深度，并求出空间位置。以此空间位置为3D点，在以第二个图的像素位置为2D点，调用
// 建立3D点
Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数，单通道图像
Mat K = ( Mat_<double> ( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m : matches )
{
ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 ) // bad depth
continue;
float dd = d / 1000.0;
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x * dd, p1.y * dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}
cout << "3d-2d pairs: " << pts_3d.size() << endl;
Mat r, t;
solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
cout << "R=" << endl << R << endl;
cout << "t=" << endl << t << endl;
程序执行结果如下图
可以看到一共迭代了10次求出了最优解。
5.5.2 使用BundleAdjustment 优化求解位姿
求解过程首先使用
使用
bundleAdjustment代码如下
void bundleAdjustment (
const vector< Point3f > points_3d,
const vector< Point2f > points_2d,
const Mat& K,
Mat& R, Mat& t )
{
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Block; // pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block ( linearSolver ); // 矩阵块求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
// vertex
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
R.at<double> ( 0, 0 ), R.at<double> ( 0, 1 ), R.at<double> ( 0, 2 ),
R.at<double> ( 1, 0 ), R.at<double> ( 1, 1 ), R.at<double> ( 1, 2 ),
R.at<double> ( 2, 0 ), R.at<double> ( 2, 1 ), R.at<double> ( 2, 2 );
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (
R_mat,
Eigen::Vector3d ( t.at<double> ( 0, 0 ), t.at<double> ( 1, 0 ), t.at<double> ( 2, 0 ) )
) );
optimizer.addVertex ( pose );
int index = 1;
for ( const Point3f p : points_3d ) // landmarks
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
optimizer.addVertex ( point );
}
// parameter: camera intrinsics
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0, 0 ), Eigen::Vector2d ( K.at<double> ( 0, 2 ), K.at<double> ( 1, 2 ) ), 0
);
camera->setId ( 0 );
optimizer.addParameter ( camera );
// edges
index = 1;
for ( const Point2f p : points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0, 0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
optimizer.optimize ( 100 );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2 - t1 );
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << endl << "after optimization:" << endl;
cout << "T=" << endl << Eigen::Isometry3d ( pose->estimate() ).matrix() << endl;
}计算结果如上图。
6 3D-3D：ICP估计相机位姿
最后解决3D-3D位姿估计问题，也就是根据一组已经匹配好的3D点，
从中找到一个欧氏变换的
这个问题是用迭代最近点(ICP)解决。和PnP类似，ICP有两种解法
- 线性代数求解(SVD方法)
- 非线性优化方法(类似于Bundle Adjustment)
6.1 求解ICP——SVD方法
目标函数建模如下
求解步骤如下
- 1 计算两组点的质心位置
p,p′ ，然后计算每个点的去质心坐标qi=pi=p, q′i=p′i−p′ - 2 根据一下优化问题计算旋转矩阵
R∗=arg minR=12∑i=1n||qi−Rq′i||2 - 3 根据第二步结果计算
t t∗=p−Rp′
6.2 求解ICP——非线性优化方法
目标函数建模如下
可以证明ICP问题岑霭唯一解或无穷多解得情况。唯一解情况下，只要能找到极小值解，那么就是全局最优值——因此不会遇到局部极小值非全局最小值的情况。
这意味着ICP求解可以任意选定初始值，这是已经匹配点求解ICP问题的一个好处。
6.3 代码实践
6.3.1 求解ICP——SVD方法代码实践
void pose_estimation_3d3d (
const vector<Point3f>& pts1,
const vector<Point3f>& pts2,
Mat& R, Mat& t
)
{
Point3f p1, p2; // center of mass
int N = pts1.size();
for ( int i = 0; i < N; i++ )
{
p1 += pts1[i];
p2 += pts2[i];
}
p1 = Point3f( Vec3f(p1) / N);
p2 = Point3f( Vec3f(p2) / N);
vector<Point3f> q1 ( N ), q2 ( N ); // remove the center
for ( int i = 0; i < N; i++ )
{
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
}
// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for ( int i = 0; i < N; i++ )
{
W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
}
cout << "W=" << W << endl;
// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU | Eigen::ComputeFullV );
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
cout << "U=" << U << endl;
cout << "V=" << V << endl;
Eigen::Matrix3d R_ = U * ( V.transpose() );
Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );
// convert to cv::Mat
R = ( Mat_<double> ( 3, 3 ) <<
R_ ( 0, 0 ), R_ ( 0, 1 ), R_ ( 0, 2 ),
R_ ( 1, 0 ), R_ ( 1, 1 ), R_ ( 1, 2 ),
R_ ( 2, 0 ), R_ ( 2, 1 ), R_ ( 2, 2 )
);
t = ( Mat_<double> ( 3, 1 ) << t_ ( 0, 0 ), t_ ( 1, 0 ), t_ ( 2, 0 ) );
}6.3.2 求解ICP——非线性方法代码实践
Eigen::Matrix3d::Identity(),
Eigen::Vector3d( 0, 0, 0 )
) );
optimizer.addVertex( pose );
// edges
int index = 1;
vector<EdgeProjectXYZRGBDPoseOnly*> edges;
for ( size_t i = 0; i < pts1.size(); i++ )
{
EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(
Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z) );
edge->setId( index );
edge->setVertex( 0, dynamic_cast<g2o::VertexSE3Expmap*> (pose) );
edge->setMeasurement( Eigen::Vector3d(
pts1[i].x, pts1[i].y, pts1[i].z) );
edge->setInformation( Eigen::Matrix3d::Identity() * 1e4 );
optimizer.addEdge(edge);
index++;
edges.push_back(edge);
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose( true );
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << endl << "after optimization:" << endl;
cout << "T=" << endl << Eigen::Isometry3d( pose->estimate() ).matrix() << endl;
}程序执行结果如下