视觉里程计基础学习：对极约束求解估计相机运动、显示出筛选特征点后的图像

对极约束原理

求取两帧图像 I1, I2 之间的运动，设第一帧到第二帧的运动为R, t。两个相机中心分别为 O1, O2。现在，考虑 I1 中有一个特征点 p1，它在 I2 中对应着特征点 p2。我们晓得这俩是通过特征匹配得到的。如果匹配正确，说明它们确实是同一个空间点在两个成像平面上的投影。这里我们需要一些术语来描述它们之间的几何关系。首先，连线 −−−→O1p1 和连线 −−−→O2p2 在三维空间中会相交于点 P。这时候点 O1, O2, P 三个点可以确定一个平面，称为极平面（Epipolar plane）。O1O2 连线与像平面 I1, I2 的交点分别为 e1, e2。e1, e2，称为极（Epipoles），O1O2 被称为基线（Baseline）。称极平面与
两个像平面 I1, I2 之间的相交线 l1, l2 为极线（Epipolar line）。

直观上讲，从第一帧的角度上看，射线 −−−→O1p1 是某个像素可能出现的空间位置——因
为该射线上的所有点都会投影到同一个像素点。同时，如果不知道 P 的位置，那么当我们
在第二个图像上看时，连线 −−→e2p2（也就是第二个图像中的极线）就是 P 可能出现的投影的位置，也就是射线 −−−→O1p1 在第二个相机中的投影。现在，由于我们通过特征点匹配，确定了 p2 的像素位置，所以能够推断 P 的空间位置，以及相机的运动。
在第一帧的坐标系下，设 P 的空间位置为：P = [X, Y, Z]T .
根据针孔相机模型

我们知道两个像素点 p1, p2 的像素位置为：s1p1 = KP , s2p2 = K (RP + t)
这里 K 为相机内参矩阵，R, t 为两个坐标系的相机运动如果使用齐次坐标，我们也可以把上式写成在乘以非零常数下成立的等式：p1 = KP , p2 = K (RP + t)
取x1 = K 1p1, x2 = K 1p2，这里的 x1, x2 是两个像素点的归一化平面上的坐标
代入上式得：x2 = Rx1 + t
两边同时与t做外积，然后在左乘xT得：xT2 t∧x2 = xT2 t∧Rx1

这两个式子都称为对极约束。它的几何意义是 O1, P, O2 三者共面。对极约束中同时包含了平移和旋转。
把中间部分记作两个矩阵：基础矩阵（FundamentalMatrix）F 和本质矩阵（Essential Matrix）E，可以进一步简化对极约束：

对极约束简洁地给出了两个匹配点的空间位置关系
相机位姿估计问题就分为了两步：

1. 根据配对点的像素位置，求出 E 或者 F；
2. 根据 E 或者 F，求出 R, t。
   E为本质矩阵，F为基础矩阵，
   后面还会有单应矩阵H,它描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（墙面、地面等），则可以通过单应性来进行运动估计。

对极约束求解估计相机运动

//-- 估计两张图像间运动
    Mat R,t;
    pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );

    //-- 验证E=t^R*scale
    //求t的反对称矩阵
    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;
    }
    return 0;
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3 
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2 
    // Ptr<FeatureDetector> detector = FeatureDetec