2D-2D相机位姿估计

本文将介绍如何根据相机在不同位置拍摄的两张图片恢复出相机的运动。在多视图几何学中，这被称为对极几何。

一、对极几何

如下图所示，相机在两个不同的位置（蓝色位置和绿色位置）同时观测到了同一个点X。根据上一篇文章《OpenCV提取ORB特征并匹配》，我们可以将X在两个相机平面上的投影x点和x'点的像素坐标匹配起来。此时，x和x'便满足一个约束，称为对极约束。

对极约束的数学推导并不复杂，本文为求简明和实用，对此不做细究。更多内容可以参考文末列出的参考资料。这里只给出直观的解释。

相机在两个位置的光心分别为O点和O'点，连线OO'与两个成像平面的交点分别为e点和e'点。我们把线段xe和x'e'称为极线。下面我们来分析为什么利用两张图片中特征点对的像素坐标可以恢复相机运动和地图点X的深度（X点的深度估计将在下篇文章中介绍）。

假设左侧相机固定，右侧相机位姿待估计。那么OX射线方向确定，同时，OX在右侧相机平面上的投影也随之确定，即直线l'。这一投影关系便称为对极约束。不过，单个对极约束并不能完全确定右侧相机的位置，大家可以想象右侧相机平面如何在保持x'坐标不变的情况下调整自己的姿态。换句话说，对极约束是有多个自由度的，一对匹配点并不能唯一确定两个相机的位姿。那到底需要多少对点呢？数学上可以证明，至少需要5对。而实际应用中通常使用8对以简化计算过程，称为“八点法”。下面就进入实际代码环节，不在理论上过多纠缠了。

二、2D-2D相机位姿估计

从两张2D图像估计相机位姿的流程如下图所示。

其中有一处分叉点，分别适用于不同的情况。左侧“计算基础矩阵或本质矩阵”适用于特征点不共面的情况；右侧“计算单应矩阵”适用于特征点共面的情况（比如墙壁、地面、航拍等场合）。

下面只给出2D-2D相机位姿估计函数的代码，特征点匹配部分的代码在上一篇文章中可以找到。

完整代码的下载地址：https://github.com/jingedawang/FeatureMethod

void pose_estimation_2d2d ( std::vector keypoints_1,
                            std::vector keypoints_2,
                            std::vector< DMatch > matches,
                            Mat& R, Mat& t )
{
    // 相机内参,TUM Freiburg2
    Mat K = ( Mat_ ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    //-- 把匹配点转换为vector的形式
    vector points1;
    vector 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 "<

这里需要解释一下，基础矩阵和本质矩阵都是3×3的矩阵，它们之间不过是差了个相机内参，因此使用时效果完全一样。上边的代码使用了本质矩阵来恢复相机运动，而没有用单应矩阵，这是因为示例图片中的特征点并不共面。在实际应用中，如果事先无法知道特征点是否共面，则应当同时计算本质矩阵和单应矩阵，选择重投影误差比较小的那个作为最终的运动估计矩阵，具体操作敬请期待后续系列文章。

三、进一步讨论

2D-2D相机位姿估计是单目SLAM初始化时的关键技术。初始化成功之后，后续的视频帧就可以采用3D-2D匹配来简化计算过程。因此初始化成功与否对SLAM至关重要。

从单目SLAM的角度考虑，2D-2D相机位姿估计存在以下三个敏感的问题：

1. 尺度不确定性
   用上面的方法估计出的相机平移向量t的值并没有单位，也就是说相机移动的距离只有相对值，没有绝对值。这是单目相机固有的尺度不确定性问题，无法从根本上解决。因此单目SLAM中一般把初始化后的t归一化，即把初始化时移动的距离默认为1，此后的距离都以这个1为单位。

2. 初始化的纯旋转问题
   单目初始化不能只有旋转，必须要有一定程度的平移，否则由于t趋近于0，导致无从求解R或者误差非常大。

3. 多于8对点的情况
   如果匹配的点对数多于8（大多数情况都是这样），可以考虑充分利用这些点，而不是只从中选择8对用于计算。推荐的算法是随机采样一致性（Random Sample Consensus，RANSAC），该算法可以有效地避免错误数据对整体结果的影响。在代码中，只需要将findFundamentalMat函数的第三个参数从CV_FM_8POINT换成CV_FM_RANSAC就可以了。

四、参考资料

《视觉SLAM十四讲》第7讲 视觉里程计1 高翔
本质矩阵和基础矩阵的区别是什么？ 知乎