视觉SLAM视觉里程计基础学习：使用图片序列，进行ORB特征点提取并暴力匹配

相邻图片暴力匹配的原理

ORB 特征亦由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”，是
一种改进的 FAST 角点，什么是 FAST 角点我们将在下文介绍。它的描述子称为 BRIEF
（Binary Robust Independent Elementary Features）。因此，提取 ORB 特征分为两个步骤：
一. FAST 角点提取：找出图像中的” 角点”。相较于原版的 FAST, ORB 中计算了特征
点的主方向，为后续的 BRIEF 描述子增加了旋转不变特性。
FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思
想是：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是角点。相比于
其他角点检测算法，FAST 只需比较像素亮度的大小，十分快捷。它的检测过程如下（见
图 7-3 ）：

1. 在图像中选取像素 p，假设它的亮度为 Ip。
2. 设置一个阈值 T(比如 Ip 的 20%)。
3. 以像素 p 为中心, 选取半径为 3 的圆上的 16 个像素点。
4. 假如选取的圆上，有连续的 N 个点的亮度大于 Ip + T 或小于 Ip √ T，那么像素 p
   可以被认为是特征点 (N 通常取 12，即为 FAST-12。其它常用的 N 取值为 9 和 11，
   他们分别被称为 FAST-9，FAST-11)。
5. 循环以上四步，对每一个像素执行相同的操作。
   二. BRIEF 描述子：对前一步提取出特征点的周围图像区域进行描述。
   BRIEF 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成，这里的 0 和 1 编
   码了关键点附近两个像素（比如说 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就
   取 0。如果我们取了 128 个这样的 p, q，最后就得到 128 维由 0，1 组成的向量。p、q的选取是按照某种概率
   分布，随机地挑选 p 和 q 的位置。
   BRIEF 使用了随机选点的比较，速度非常快，而且由于使用了二进制表
   达，存储起来也十分方便，适用于实时的图像匹配。原始的 BRIEF 描述子不具有旋转不
   变性的，因此在图像发生旋转时容易丢失。而 ORB 在 FAST 特征点提取阶段计算了关键
   点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征，使 ORB 的
   描述子具有较好的旋转不变性。
   FAST 和 BRIEF 的组合也非常的高效，使得 ORB 特征在实时 SLAM 中非常受欢
   迎。
   特征匹配
   通过对图像与图像，或者图像与地图之间的描述子进行准确的匹配，我们可以为后续的姿态估计，优化等操作减轻大量负担
   最简单的特征匹
   配方法就是暴力匹配（Brute-Force Matcher）。即对每一个特征点 xmt ，与所有的 xnt+1
   测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示了两个特征之
   间的相似程度，不过在实际运用中还可以取不同的距离度量范数。对于浮点类型的描述子，
   使用欧氏距离进行度量即可。而对于二进制的描述子（比如 BRIEF 这样的），我们往往使
   用汉明距离（Hamming distance）做为度量——两个二进制串之间的汉明距离，指的是它
   们不同位数的个数。

创建文件夹和准备图片序列

在Ubuntu下创建一个一个文件夹，在其文件夹下创建一个computerORB.cpp文件，并将准备的图片序列放到该文件下。
我使用的是无人驾驶车数据集的一小段kitti_image

读取图片序列

循环读取图片，一次读取三张图片并进行显示

char fileName1[50]; //用来保存文件名1
	char fileName2[50]; //用来保存文件名2	
	char fileName3[50]; //用来保存文件名3
	namedWindow("frame1", WINDOW_AUTOSIZE); //3个显示窗口
	namedWindow("frame2", WINDOW_AUTOSIZE);
	namedWindow("frame3", WINDOW_AUTOSIZE);
	
       std::string filename ="./kitti_Image/Kitti_image_2/*.png";
       std::vector<cv::String> image_files;
       cv::glob(filename, image_files);

	for(int i=0;i<=4661;i++) //限定循环次数
	{

	cv::Mat Image1=cv::imread(image_files[i],0); 
	cv::Mat Image2=cv::imread(image_files[i+1],0); 
	cv::Mat Image3=cv::imread(image_files[i+2],0); 
	cv::imshow("frame1", Image1); //在之前创建好的window中显示本帧图片
	cv::imshow("frame2", Image2);
	cv::imshow("frame3", Image3);
        cv::waitKey(25);

提取特征点

一次提取了图片序列的三张图片，提取图片的特征点，然后进行显示
使用了computeORBDesc（）和computeAngle（）函数
这些都在之后有定义

 // detect FAST keypoints using threshold=40

    vector<cv::KeyPoint> keypoints;

    cv::FAST(Image1, keypoints, 40);

    cout << "keypoint1: " << keypoints.size() << endl;


    // compute angle for each keypoint

    computeAngle(Image1, keypoints);

    // compute ORB descriptors

    vector<DescType> descriptors;

    computeORBDesc(Image1, keypoints, descriptors);

    // plot the keypoints

    cv::Mat image_show;

    cv::drawKeypoints(Image1, keypoints, image_show, cv::Scalar::all(-1),

                      cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

    cv::imshow("feature1", image_show);

    cv::imwrite("feat1.png", image_show);

    cv::waitKey(25);
// compute the descriptor

void computeORBDesc(const cv::Mat &image, vector<cv::KeyPoint> &keypoints, vector<DescType> &desc) {

    for (auto &kp: keypoints) {
        DescType d(256, false);
        double un_up,un_vp,un_uq,un_vq; // 一对点；
        double up,vp,uq,vq; // 旋转后的点；
        for (int i = 0; i < 256; i++) {

            // START YOUR CODE HERE (~7 lines)
            d[i] = 0;  // if kp goes outside, set d.clear()
            un_up = ORB_pattern[i*4];

            un_vp = ORB_pattern[i*4+1];

            un_uq = ORB_pattern[i*4+2];

            un_vq = ORB_pattern[i*4+3];           // 比较两组点的灰度值大小；
            // 旋转到主方向上；

            double angle = kp.angle * (pi/180);
            up =kp.pt.x+  cos(angle)*un_up-sin(angle)*un_vp;

            vp =kp.pt.y+ sin(angle)*un_up + cos(angle)*un_vp;

            uq =kp.pt.x+  cos(angle)*un_uq-sin(angle)*un_vq;

            vq =kp.pt.y+ sin(angle)*un_uq + cos(angle)*un_vq;
            //边界约束；

            if(up>image.cols||up<0 || vp <0||vp>image.rows||uq>image.cols||uq<0 || vq <0||vq>image.rows)
            {
               d.clear();//超出边界，特征点描述子清零；
               break;

            }
            else if(image.at<uchar>(vp,up)<image.at<uchar>(vq,uq))
            {
                d[i]=1;
            }
        // END YOUR CODE HERE
        }
        desc.push_back(d);
    }
    int bad = 0;
    for (auto &d: desc) {
        if (d.empty()) bad++;
    }
    cout << "bad/total: " << bad << "/" << desc.size() << endl;
    return;
}

 

特征点匹配

因为我们只学了如何进行两张图片的特征匹配，三张图片，我选择的是进行两两特征匹配，然后依次显示三张特征匹配对比后的图片
使用了bfMatch()和drawMatches函数，在完整代码中都有定义

 // find matches

    vector<cv::DMatch> matches1;

    bfMatch(descriptors, descriptors2, matches1);
    if (matches1.size()<4)
    cout<<"匹配點太少"<<endl;
    else
    cout << "matches1: " << matches1.size() << endl;
  // plot the matches

    cv::drawMatches(Image1, keypoints, Image2, keypoints2, matches1, image_show);

    cv::imshow("matche1",