slam第六讲(视觉里程计1_1)

VO按是否需要提取特征，分为特征点法的前端和不提特征的前端。这一章讲的是基于特征点法的前端，分为以下内容。

1. 特征点法：找到两张2D图像上的匹配点。
2. 对极几何：根据2D-2D特征点对求解R,t。
3. 三角测量：根据2D-2D特征点求深度。
4. PnP：根据3D点云和匹配的2D图像求R,t。
5. ICP：求两个点云之间的R,t。

关系是：

1. 特征点法找到2D图像的匹配点对，用于对极几何和pnp
2. 对极几何求出2D-2D的位姿。
3. 根据对极几何求出的位姿，三角测量求出2D-2D的深度。
4. 根据三角测量求出的深度，可以初始化单目SLAM，得到三维点信息；或用RGBD相机获得三维信息。知道3D信息，对于下一张2D图像，可根据特征点法找到的匹配点对，使用PNP，求出位姿信息和深度信息。
5. 根据pnp求出的深度信息；或直接用RGBD得到的两个点云，可以用ICP，求出两个点云之间的位姿变换。

               

一.特征点法

1.1特征点

(在视觉slam中，可以利用图像特征点作为slam中的路标)

特征是图像信息的另一种数字表达形式。

我们希望特征点在相机运动之后保持稳定。

相比于朴素的角点，人工设计的特征点有如下性质：

• 可重复性Repeatability：相同的区域可以在不同的图像中找到
• 可区别性Distinctiveness：不同的区域有不同的表达
• 高效率Efficiency：同一图像中，特征点的数量应远小于像素的数量
• 本地性Locality：特征仅与一小片图像区域相关

特征点的组成：

• 关键点(Key-Point):指的是特征点在图像中的位置，有些特征点还有朝向，大小等信息
• 描述子(Descriptor)：通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。描述子是按照外观相似的特征应该有相似的描述子的原则设计的，因此，只要两个特征点的描述子在向量空间上的距离越近，就可以认为它们是同样的特征点。

• SIFT特征点
• FAST关键点
• ORB特征点

1.2ORB特征点

• 关键点：ORB的关键点称为“Oriented FAST”,是一种改进的FAST角点。FAST角点提取如下所述：找出图像中的角点，相比于原版的FAST，ORB中计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变性。
• 描述子：BRIEF描述子，对前一步提取出特征点的周围图像区域进行描述

FAST关键点

FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的一个思想是：如果一个像素与邻域像素的差别较大（过亮或过暗），那么它更可能是角点。

1. 选取像素p，假设它的亮度是A
2. 设置一个阈值T=0.2A或其他
3. 以像素p为中心，选取半径r=3的圆上的16个像素点
4. 如果选取的16个像素点中，有连续N个点的亮度>A+T或
5. 对每个像素重复1-4步，找到所有的特征点

                   

注：

FAST-12中可以通过添加预测试来提高效率。在检测时，优先比较p周围第1，5，9，13号像素的亮度，如果全部满足步骤4的条件，则继续判断，否则排除。通过这样的预测试，大大减少了比较次数，提高效率。

FAST角点还会出现“扎堆”现象，需要使用非极大值抑制，在一定区域内仅保留响应极大值的角点，避免焦点集中的问题。

FAST特征点的缺点：

1. FAST特征点通常很多且不确定
2. FAST特征点不具有方向信息
3. 由于r=3恒定，所以存在尺度问题

针对以上三个问题，我们进行改进：

Oriented FAST:

针对问题1：我们可以指定最终要提取的角点数量N，对原始的FAST角点分别计算Harris响应值，然后选取前N个具有最大响应值的角点作为最终的角点集合。

针对问题2和问题3：ORB添加了尺度和旋转的描述。

旋转不变性

FAST特征点至描述了位置，没有方向信息，Oriented FAST通过灰度质心法添加了一个方向。灰度质心法假设角点的中心与质心存在着一个偏移，从中心指向质心的向量可以用于表示方向，如图-质心所示。

                                 

 所谓质心是指以图像块灰度值作为权重的中心，其具体操作如下：

1.在一个小的图像块B中，定义图像块的矩为：

              

I(x,y)表示像素坐标(x,y)处的灰度值，则特征点p邻域的矩定义为公式-矩

2.通过矩可以找到图像块的质心：

                  是x加权的总量与总和相比显示出x在图像的那一列的时候像素值比较大，代表了图像像素在x方向上的偏重，是重心的x坐标

                   代表图像像素在y方向上的偏重，是重心的y坐标

重心C的坐标：  

3.连接图像块的几何中心O与质心C，得到一个方向向量OC,把特征点与质心的夹角定义为FAST特征点的方向：

                                                                            

方向的添加，使得Oriented FAST具有了旋转不变性。

尺度不变性

尺度问题是指，在远处看起来点A像是角点，但是走近之后发现点A不是角点；或者说在当前分辨率下，看到点A是角点，当放大分辨率时发现点A不是角点。

Oriented FAST通过构建图像金字塔来实现尺度不变性。

                        

如图-金字塔所示，把原始图像按指定比例缩放，在缩放后的每一层上提取一次FAST角点，由此获得尺度不变的特性。

BRIEF描述子

BRIEF描述子是由0和1组成的向量，在计算时，取特征点P周围的N对像素点，图-BRIEF中N=4，比较两个像素点之间的大小关系（如图中的黄色像素对，令上方点的灰度值为A，下方点的灰度值为B，如果A>B则记为1，否则记为0）。当N=4时，BRIEF描述子就是一个四维向量。相邻图片采用同样的选点模式，才能保证两张图片的描述子具有可比性。

                            

在取点时，通常以关键点为原点，水平方向为x轴，建立坐标系。如果图片发生了旋转，不改变建系的方式，那么即使采用相同的模式，两张图片所选取的点也不一定相同，得到的描述子就不具有可比性。改进的BRIEF以关键点为原点，关键点和重心的连线方向为x轴建立坐标系，这样可以把旋转考虑进去，保证旋转不变性。 

opencv中的ORB特征点提取结果：

C++代码实现ORB特征点提取：

    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    //-- 初始化
    std::vector keypoints_1, keypoints_2;
    Mat descriptors_1, descriptors_2;
#ifndef __OPENCV_2__
    // used in OpenCV3
    Ptr detector = ORB::create();
    Ptr descriptor = ORB::create();
#else
    // use this if you are in OpenCV2
    Ptr detector = FeatureDetector::create ( "ORB" );
    Ptr descriptor = DescriptorExtractor::create ( "ORB" );
#endif
    Ptr 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);

１.３特征匹配

特征匹配解决了SLAM中的数据关联问题(data association)，即确定当前帧的图像特征与前一帧的图像对应关系，通过描述子的差异判断哪些特征为同一个点。

1.3.1 存在的问题
由于场景图像中存在大量重复的纹理，使得描述特征非常相似，导致错误匹配广泛存在。

1.3.2 匹配方法
在图像中提取到特征点,在图像中提取到特征点 。

寻找这两个集合之间的对应关系最简单的方法就是使用暴力匹配方法(Brute-Force Matcher)：

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

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

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

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

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector 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 );

ORB特征点以及匹配点对如下所示： 

 

二.2D-2D:对极几何

2.1对极约束

如下图所示，我们希望求取两帧图像I1和I2之间的运动，设第一帧到第二帧的运动为R,t。两个相机的中心分别为O1和O2。

I1中特征点p1,它在I2中对应着特征点p2,如果匹配正确，则这两个点确实是同一个空间点在两个成像平面上的投影。

极平面(Epipolar plane)：P-O1-O2

极点(Epipoles)：e1,e2(O1,O2连线与像平面I1，I2的交点)

基线(Baseline)：O1-O2

极线(Epipolar line)：l1,l2(极平面与像平面I1，I2之间的交线)

从第一帧的角度看，射线O1-p1是某个像素可能出现的空间位置，因为该射线上所有点都会投影到同一个像素点。

在第二帧的图像上看时，连线e2-p2也就是图像的极线，就是P可能出现的投影的位置，也就是射线O1-p1在第二个相机中的投影。

(注：若没有特征匹配，则无法确定p2到底在极线的什么位置，则我们首先通过极线l1在I2平面上的投影得到极线l2,则在该极线上通过搜索得到p1的匹配点p2)

代数关系运算：

在第一帧的坐标系下，设P的空间位置为,相机内参矩阵K，而R,t为两个坐标系的相机运动。

对极约束：

若记,则x1,x2是两个像素点的归一化平面上的坐标。

则对极约束也可以写为：

对极约束的几何意义是O1,O2,P三点共面。

对极约束中同时包含了平移和旋转。

本质矩阵(Essential Matrix)E：

基础矩阵(Fundamental Matrix)F：

则对极约束为：

 

对极约束简洁的给出了两个匹配点之间的空间位置关系，于是相机位姿估计问题变为以下两步：

1. 根据匹配点的像素位置(p1,p2)求出E或F
2. 根据E或F求出R,t

2.2本质矩阵

本质矩阵，是一个3*3的矩阵，内有9个未知数。

并不是任意一个3*3的矩阵就是E，本质矩阵E有以下几个性质：

• 本质矩阵由对极约束定义，由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足，称E在不同尺度下是等价的。
• 根据，本质矩阵E的奇异值必定是的形式，这称为本质矩阵的内在性质。
• 由于平移和旋转各有3个自由度，总共6个自由度，所以共有6个自由度。但是由于尺度等价性，E实际上只有5个自由度

E具有5个自由度，表明我们可以理论上最少用5对点来求解E，但是E的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计E，这就是经典的八点法(Eight-point-algorithm)。

八点法只利用了E的线性性质，因此可以在线性代数框架下求解。

八点法详解：

设一对匹配点,,根据对极约束有;

                                                         

将矩阵E展开为向量的形式：

                                                          

则对极约束可以写为与e有关的线性形式：

                                                          

同理，对于其他点对也有相同的表示，我们把所有的点都放到一个方程中，变成线性方程组(表示第i个特征点)：

                                             

若这8对匹配点组成的矩阵满秩，则E的各个元素就可由上述方程解得。

接下来的问题则是如何根据已经估计得到的本质矩阵E，恢复出相机的运动R，t。这个过程是由奇异值分解(SVD)得到的。

设E的SVD分解为：

                                                        

其中U,V为正交阵，为奇异值矩阵。

根据E的内在性质，我们知道

在SVD分解中，对于任意的一个E，存在两个可能的t,R与它对应：

                                           ，

                                          ，

注意：

• 由于-E和E等价，所以对任意一个t取负号，也会得到同样的结果
• 从工程实现角度，往往有几十对甚至几百对匹配点，对于这种多于八对点的情况，采用最小二乘或随机采样一致性（RANSAC）的方法。
•  

因此，从E分解得到R，t，一共存在4个可能的解。

 在这四种解中，只有一种解中P在两个相机中都具有正的深度，因此，只要把任意一点代入4种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

还有一个问题：

根据线性方程求出的E，可能不满足E的内在性质，它的奇异值不一定为,,0的形式，这时我们在做SVD分解时，我们会刻意把矩阵调为这种形式，具体做法：

对八点法求得的E进行SVD分解后，会得到奇异值矩阵,不妨设,取：

                                                   

这相当于是把求出来的矩阵投影到了E的流形上，更为简单的做法是将奇异值矩阵取成(1,1,0)因为E具有尺度等价性，所以这样做也是合理的。

2.3单应矩阵

单应矩阵H(Homography)描述了两个平面之间的映射关系。

适用场景：

若场景中的特征点都落在同一平面上(比如墙或地面)，则可以通过单应性来进行运动估计。

单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系，考虑在图像I1和图像I2之间有一对已经匹配好的特征点p1,p2，这些特征点落在图像平面P上，设这个平面满足方程：

                                                                                   

稍加整理，得：

                                                                                         

由摄像机成像模型得到：

于是我们得到了一个直接描述图像坐标p1和p2之间的变换，把中间这部分记为H，于是：

                                                                                               

H的定义与旋转，平移以及平面的参数有关。

与基础矩阵F类似，单应矩阵H也是一个3*3的矩阵，求解时的思路也和F类似，同样也可以根据匹配点计算H，然后将它分解以计算旋转和平移。

将上式展开，得到：

                                                                      

这里等号是在非零因子下成立的。

实际处理时，通常乘以一个非零因子使得h9=1(在它取非零值时)，然后根据第三行，去掉这个非零因子，于是有：

                                                                               

                                                                               

整理得到：

                                                                 

                                                                 

这样一组匹配点会构造出两项约束，于是自由度为8的单应矩阵可以通过4对匹配特征点算出(注意：这些特征点不能有三点共线的情况出现)，即求解出以下的线性方程组(当h9=0时，右侧为0)：

 

这种做法把H矩阵看成了向量，通过解该向量的线性方程组来恢复H，又称为直线线性变换法。

求出H后，需要进行分解，得到R和t,分解的方法包括数值法和解析法。

H的分解同样会得到4组R和t，并且同时可以计算出对应的场景点所在的平面的法向量。

4组R和t的筛选方法：

• 若已知成像地图点的深度全为正值(即在相机前方)，则可以排除两组解
• 通过先验信息，例如假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量n的理论值为。

注意：

当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化。 

现实中的数据总包含噪声，若这时还利用八点法求解基础矩阵，则基础矩阵多余出来的自由度将会主要有噪声决定。

为了避免退化现象造成的影响，通常我们会同时估计基础矩阵F和单应矩阵H，选择重投影误差小的那个作为最终的运动估计矩阵。

2.4对极约束求解相机运动

在经过特征匹配的基础上，利用已经匹配好的特征点来计算E，F和H。进而分解E得到R，t。

c++代码如下：

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

}

然后在main函数中调用上式位姿R,t的估计函数即可：

int main(int argc,char **argv)
{
    if(argc!=3)
    {
        cout<<"usage:feature_extraction img1 img2"< keypoints_1, keypoints_2;
    vector 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_ ( 3, 3 ) <<
                0,                      -t.at ( 2, 0 ),     t.at ( 1, 0 ),
                t.at ( 2, 0 ),      0,                      -t.at ( 0, 0 ),
                -t.at ( 1.0 ),     t.at ( 0, 0 ),      0 );

    cout << "t^R=" << endl << t_x*R << endl;

    //-- 验证对极约束
    Mat K = ( Mat_ ( 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_ ( 3, 1 ) << pt1.x, pt1.y, 1 );
        Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
        Mat y2 = ( Mat_ ( 3, 1 ) << pt2.x, pt2.y, 1 );
        Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }
    return 0;
}

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

总结：

• 尺度不确定性
  对t长度的归一化，直接导致单目视觉的尺度不确定性(Scale Ambiguity),在单目视觉中对两张图像的t归一化相当于固定了制度，我们不知道长度是多少，但是我们这是t的单位是1，计算相机运动和特征点的3D的位置。这被称为单目视觉SLAM的初始化。在初始化之后就可以利用3D-3D来计算相机运动了，初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。因此，单目slam有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。(也可以令初始化时所有特征点平均深度为1，也可以固定一个尺度，这样可以控制场景的规模大小，使计算在数值上更稳定些。)
• 初始化的纯旋转问题
  单目视觉初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，从E分解到R,t 的过程中，导致t为零，那么得到的E为零，这将导致无法求解R。不过此时可以依靠H求取旋转，但是仅仅有旋转时，无法使用三角测量估计特征点的空间位置。(简单做法：在单目slam情况下，让相机左右平移以顺利地进行初始化。)
• 对于8对点的情况
  当给定匹配点多余8个的时候，比如算出79对匹配点，使用一个最小二乘计算。记方程为Ae=0，对于八点法，A的大小为8*9，如果给定的匹配点多于8，该方程构成一个超定方程，即不一定存在e使得上式成立，因此，可以通过最小化一个二次型来求：，这样就求出了最小二乘意义下的E矩阵。不过在存在错误匹配的情况下使用随机采样一致性(Random Sample Concensus,RANSAC)来求，可以处理带有错误匹配的数据。

• 单应矩阵和本质矩阵使用情景区别：1.单应矩阵用于场景的特征点都落在同一个平面上的时候使用，比如墙，地面，可以通过单应性来估计运动，因为当特征点共面的时候，基础矩阵的自由度下降也就是出现退化现象。2.本质矩阵用于估计特征点不共面的情况

三.三角测量

知道了相机的运动，下一步需要知道物体的空间坐标，单目相机的缺点是无法获得深度，只能通过目前的已知量对深度进行估计，然后再计算坐标。

三角测量是指，通过在两处观察同一个点的夹角，从而确定该点的距离。

 

如图7-9所示，理想情况下，O1p1和O2p2应该相交于点P，但实际中由于噪声的存在，会使得二者不相交，可以通过深度求解。

以左边的图片为参考，设x1,x2分别为两个特征点的归一化坐标，有 ，此时R，t，x1，x2已知，求解s1,s2，左乘，有求解这个方程，可得到s2，同理可得s1。

注：由于有噪声存在，所以上式不一定完全等于0，所以可以求最小二乘解。

三角测量必须要有平移，纯旋转无法进行三角测量。 

 如图7-10所示，当存在误差时，平移越大，误差对深度计算的影响越小。但当平移变大时，会导致图像的外观成像发生变化，变化越大特征提取与匹配就越困难。于是产生矛盾：平移太大会导致匹配失效，平移太小三角化精度不够。