视觉slam14讲——第7讲 视觉里程计1

本系列文章是记录学习高翔所著《视觉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) 定义图像块的矩

mpq=∑xpyqI(x,y),p,q=0,1

(2) 图像块的质心

C=(m10m00,m01m00)=(∑xI(x,y)∑I(x,y),∑yI(x,y)∑I(x,y))

(3) 特征点方向

θ=arctan(m01m10)=arctan(∑yI(x,y)∑xI(x,y))

2.2 BRIEF描述子

BRIEF(Binary Robust Independent Elementary Feature)是二进制描述子，它的描述向量是由许多个0和1组成，这里的0和1编码了关键点附近的两个像素p和q的大小关系，如果p>q,则取1；反之取0。

大体上按照某种概率分布，随机的挑选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 匹配方法

在图像It中提取到特征点xmt,m=1,2,...M，在图像It+1中提取到特征点xnt+1,n=1,2,...N，寻找这两个集合之间的对应关系就是使用暴力匹配方法(Brute-Force Matcher)。

对每一个特征点xmt和所有的xnt+1测量描述子的距离，然后排序，取最近的一个点作为匹配点。

描述子 距离表示两个特征之间的相似程度，在实际应用中取不同的距离度量范数。

• 浮点类型的描述子使用欧式距离度量
• 二进制类型描述子(eg:BRIEF)使用汉明距离(HammingDistance)作为度量。比如两个二进制串的汉明距离表示不同位的个数。

特征点数量过大时，暴力匹配算法不能满足slam实时性的要求，它的改进算法是快速最近邻算法(FLANN)，更加适合匹配点数量极多的情况。这些算法已经成熟，都已经集成到OpenCV中。

所有匹配点对如下图

优化后匹配的点对

特征匹配的代码如下，这个部分是紧接这上面的特征点提取部分的代码的。

    //-- 第三步:对两幅图像中的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对极几何

根据得到的若干对匹配点，就可以通过这些二位图像的对应关系，恢复出两帧图像摄像机的运动。

对极几何约束如下图

P为摄像机观察的空间的某一坐标点，p1,p2为P在图像中的投影点，并且两幅图像较好的匹配到了, 最终推导出对极约束公式：

pT2K−Tt^RK−1p1=0

定义中间部分的基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E

E=t^R,F=K−Tt^RK−1

也就得到

xT2Ex1=pT2Fp1

对极约束给出了两个匹配点之间的空间关系，于是，问题简化为：

• 由匹配点的像素位置计算E或者F
• 由E或F求R,t

3.1 本质矩阵(Essential Matrix)E

关于E有三个特点

• 本质矩阵由对极约束定义，由于对极约束是等式为零的约束，所以对E乘以乘任意非零常数依然满足，称E在不同尺度下是等价的。
• E=t^R, E的奇异值(Singular Value)必定是[σ,σ,0]T的形式，这称为本质矩阵E的内在性质
• 由于平移旋转共6个自由度，所以t^R 有6个自由度。但是由于尺度等价性，E实际上共五个自由度

使用经典的八点法求解E, 八点法构成的方程组可解出E, 还有一点需要注意的是下图，

八点法的总结：

• 用于单目视觉的初始化
• 尺度不确定性：归一化 t 或特征点的平均深度
• 纯旋转问题：t=0 时无法求解
• 多于八对点时：最小二乘或随机采样一致性（RANSAC）

3.2 单应矩阵H(Homography)

它描述的是两个平面之间的映射关系，如果场景中的特征点都落在同一平面上(比如情面、地面)，则可以通过单应性进行运动估计。

假设特征点落在平面P上，

nTP+d=0

也就是

−nTPd=1

代入最开始式子

p2=K(RP+t)

得到一个从图像坐标p1到p2之间的变换

p2=K(R−tnTd)K−1p1

记作

p2=Hp1

它的定义与平移、旋转和平面的参数相关。
求解单应矩阵使用线性变换法(Direct Linear Transform), 与求解本质矩阵类似，求出来后需要对其进行分解才能得到R,t,分解方法有数值法和解析法。分解同样得到四组解，最后根据先验信息进行排除得到唯一的一组R,t解。

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;    }

最后程序执行结果如下

观察结果可以看到验证对极约束的精度在小数点后三位10−3。

3.4 总结

• 尺度不确定性
  对t长度的归一化，直接导致单目视觉的尺度不确定性(Scale Ambiguity),在单目视觉中对两张图像的t归一化相当于固定了制度，我们不知道长度是多少，但是我们这是t的单位是1，计算相机运动和特征点的3D的位置，这被称为单目视觉SLAM的初始化。

• 初始化的纯旋转问题
  单目视觉初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，从E分解到R,t的过程中，导致t为零，那么得到的E也为零，这将导致无法求解R。不过此时可以依靠H求取旋转，但是仅仅有旋转时，无法使用三角测量估计特征点的空间位置。

• 对于8对点的情况
  当给定匹配点多余8个的时候，比如算出79对匹配点，使用一个最小二乘计算

  mine||Ae||22=mineeTATAe
  , 不过在存在错误匹配的情况下使用随机抽样一致算法(Random Sample Concensus,RANSAC)，可以处理带有错误匹配的数据。

4 三角测量(Triangulation)

4.1 三角测量理论介绍

上一步得到运动之后，下一步需要用相机的运动估计特征点的空间位置。
三角测量指的是通过在两处观察同一个点的夹角，从而确定改点的距离。最早是由高斯提出的。由于噪声的影响，图中的两条直线无法相交，因此可以通过最小二乘求解。

按照对极几何的定义，设x1,x2为两个特征点归一化坐标，满足

s1x1=s2Rx2+t

现在R,t是已知的，需要求解深度信息s1,s2，先对上个式子两侧左乘x^1，得到

s1x^1x1=0=s2x^1Rx2+t

由此算出s2，然后代入求出s1

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;    }

执行结果如下图

结果显示了每个空间点在两个相机坐标系下的投影坐标和像素坐标，相当于P的投影位置与看到的特征点的位置。由于误差存在，有微小差异。

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简介

PnP(Perspective-n-Point)是求解3D到2D点对运动的方法。如果两张图像中其中一张特征点的3D位置已知，那么只需要3个点对便可以估计相机运动。

其中，特征点的3D位置可以由三角化或者RGB-D相机深度图获得。

单目双目使用PnP的区别如下：
* 双目相机或者RGB-D相机的视觉里程计中可以使用PnP估计相机运动
* 单目视觉需要先进性初始化，然后才能使用PnP

解决PnP问题的方法：

• 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原理

P3P利用3个点的几何关系(三角形的相似性)，输入数据为3对3D−2D匹配点，3D点是A,B,C(世界坐标系中坐标)，2D点是a,b,c(成像平面像素坐标)。此外P3P还需要一对验证点，从可能解中选出正确的哪一个(类似于对极几何)。记验证点对为D−d。
输出数据是得到A,B,C在相机坐标系下的3D坐标。
这样就得到了3D−3D对应点，此时转换成了ICP问题。

5.3.2 P3P 存在问题

• P3P只利用3个点的信息，当给定匹配点多余3组时，难以利用更多信息
• 如果3D点或者2D点受噪声影响，或者存在误匹配，算法失效

后续人们提出的 EPnP(EfficientPnP)，UPnP就是利用更多的信息，而且使用迭代的方式对相机位姿进行优化，尽可能消除噪声的影响。

在SLAM中，通常先使用 P3P/EPnP等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整(Bundle Adjustment) 。

5.4 PnP求解——Bundle Adjustment

除了使用线性方法之外，还可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。

• 线性方法：先求相机位姿，再求空间点位置
• 非线性优化方法：把他们都看成优化变量，放在一起进行优化。这是一种非常通用的求解方法。

在PnP中这个Bundle Adjustment问题是一个最小化重投影误差(Reprojection error)的问题。

像素与空间点位置关系是

siui=Kexp(ξ^)Pi

把误差求和构建最小二乘问题，然后寻找最好的相机位姿，使其最小化

ξ∗=arg minξ12∑i=1n||ui−1siKexp(ξ^)Pi||22

该问题的误差项是将像素坐标(观测到的投影位置)与 3D点按照当前估计的相机位姿进行投影得到的位置相比较得到的误差，称为重投影误差。

5.5 PnP求解代码实践

5.5.1 使用EPnP求解位姿

利用OpenCV提供的EPnP求解PnP问题，然后通过g2o对结果优化。

下面一段代码是在得到配对特征点后，在第一个图的深度图中寻找他们的深度，并求出空间位置。以此空间位置为3D点，在以第二个图的像素位置为2D点，调用EPnP求解PnP问题。

    // 建立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优化求解位姿

求解过程首先使用Ceres或者g2o构建一个最小二乘问题。
使用g2o建模的图优化节点和边，图示如下

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点，

P={p1,..,pn}，P′={p′1,..,p′n}

从中找到一个欧氏变换的R，t，使得

任意i满足pi=Rp′i+t

这个问题是用迭代最近点(ICP)解决。和PnP类似，ICP有两种解法

• 线性代数求解(SVD方法)
• 非线性优化方法(类似于Bundle Adjustment)

6.1 求解ICP——SVD方法

目标函数建模如下

minR,t J=12∑i=1n||pi−p−R(p′i−p′)||22+||p−Rpi−t)||22

求解步骤如下

• 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——非线性优化方法

目标函数建模如下

minξ=12∑i=1n||pi−Kexp(ξ^)P′i||22

可以证明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;}

程序执行结果如下