视觉里程计——基于特征点的位姿估计

slam系统分为前端和后端，其中前端也叫视觉里程计，视觉里程计根据相邻图像的信息进行粗略的位姿估计，为后端提供较好的初始值，发展至今，视觉里程计的算法主要分为两大类：特征点法与直接法；其中特征点法具有稳定、对光照、动态物体不敏感的优势，被认为是其主流的方法。接下来将从特征点法入手进行阐述。

核心问题：如何根据图像估计相机运动

一、引言

从图像中选取有代表性的点，即两帧之间图像的共同点，这些点在激光slam中称为路标，在视觉slam中称为图像特征，一般情况下，我们希望特征点在相机运动之后保持稳定；数字图像在计算机中以灰度值矩阵的方式存储，角点和边缘点在不同图像间的辨识度更高，可作为特征点，一种直观的方式是在不同图像间辨认角点（边缘与区块分辨较为困难）。由于单纯的角点不能满足需求，随着研究，出现了更加稳定的人工设计的局部图像特征（SIFT、SURF、ORB（保持的旋转、尺度不变特性，同时速度方面提升明显））。

二、特征点法

特征点：由关键点和描述子组成
关键点：即角点，指特征点在图像里的位置，还包括朝向、大小等信息
描述子：通常是一个包含0、1的向量，描述了该关键点周围的像素信息
fast关键点：fast是一种角点（优点：速度快 缺点：不具有方向特性与尺度特性），主要检测局部像素灰度变化明显的地方，只需比较像素亮度的大小，十分快捷，如果一个像素与邻域的像素差别较大（过亮或过暗），都可能是角点

一般来说提取特征点指的是提取图像中的关键点，并计算描述子

2.1、ORB特征点法

ORB特征保留了特征子具有的旋转、尺度不变的特性，且速度明显提升，是质量与性能较好的折中，对实时性很高的slam来说是一个很好的选择,其特征包括关键点和描述子两部分，ORB的关键点是一种改进的（添加了尺度和旋转的描述）fast角点。
特征提取分为以下两个步骤

1、 fast关键点提取：

• 在图像中选取像素p,假设它的亮度为Ip
• 设置一个阈值T（比如Ip的20%）
• 以像素p为中心，选取半径为3的圆上的16个像素点
• 加入选取的圆上有连续N个点的亮度大于Ip+T或小于Ip-T,那么像素p可以被认为是特征点（N通常取12，即FAST-12,其他常用的N取9和11，他们被分别称为FAST-9和FAST-11）
• 循环以上步骤，对每一个像素执行相同的操作
  注：原始的fast角点经常出现“扎堆”现象，所以一遍检测后，需用非极大值抑制，避免角点集中问题
  针对FAST不具有方向性和尺度的缺点，ORB添加的尺度不变性由构建图像金字塔解决

2、 计算BRIEF描述子：

• 对比关键点附近两个随机像素（比如p和q）的大小关系，如果p比q大 取1，反之取0
• ORB在fast特征点提取阶段计算了关键点的方向，利用了方向信息，使其描述子具有较好的旋转不变性
• 二进制比较距离大小需要用到汉明距离

三、代码实践

#include 

#include 

#include 

#include 

#include 
using namespace std;
using namespace cv;

//argc:表示传入main函数的参数个数   argv :表示传入的参数  

//第一个参数argv[0]一定为程序的名称，故传入的参数个数为argc-1

int main(int argc, char **argv) {

  if (argc != 3) {  //判断是否传入了两张图片

    cout << "usage: feature_extraction img1 img2" << endl;

    return 1;

  }

  //-- 读取图像  imread( 图片路径，加载图像的颜色类型 )

  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);//转换图像到彩色一体

  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

  assert(img_1.data != nullptr && img_2.data != nullptr);//如果图片为空，则终止执行



  //-- 初始化

  std::vector keypoints_1, keypoints_2;//保存两张图像中的特征点

  Mat descriptors_1, descriptors_2;//保存两张图像中的描述子

  Ptr detector = ORB::create();//初始化关键点     可以传入关键点的数量，进行修改

  Ptr descriptor = ORB::create();//初始化描述子

  Ptr matcher = DescriptorMatcher::create("BruteForce-Hamming");//初始化匹配变量，传入度量范数，汉明距离



  //-- 第一步:检测 Oriented FAST 角点位置（提取关键点）

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

  detector->detect(img_1, keypoints_1);

  detector->detect(img_2, keypoints_2);



  //-- 第二步:根据角点位置计算 BRIEF 描述子（由0、1组成的向量）

  descriptor->compute(img_1, keypoints_1, descriptors_1);

  descriptor->compute(img_2, keypoints_2, descriptors_2);

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

  chrono::duration time_used = chrono::duration_cast>(t2 - t1);

  cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;//输出提取ORB特征点的时间



  Mat outimg1;

  drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);

  imshow("ORB features", outimg1);

  imwrite("./ORB_features.png", outimg1);



  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离

  vector matches;

  t1 = chrono::steady_clock::now();

  matcher->match(descriptors_1, descriptors_2, matches);//计算第一个图里面的每一个点与第二图里的每一个点之间的汉明距离

  t2 = chrono::steady_clock::now();

  time_used = chrono::duration_cast>(t2 - t1);

  cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;//输出特征匹配的时间



  //-- 第四步:匹配点对筛选

  // 计算最小距离和最大距离

  //tips:auto可以在声明变量的时候根据变量初始值的类型自动为此变量选择匹配的类型

  auto min_max = minmax_element(matches.begin(), matches.end(),

                                [](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });

  double min_dist = min_max.first->distance;

  double max_dist = min_max.second->distance;



  printf("-- Max dist : %f \n", max_dist);

  printf("-- Min dist : %f \n", min_dist);



  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.

  std::vector good_matches;

  for (int i = 0; i < descriptors_1.rows; i++) {

    if (matches[i].distance <= max(2 * min_dist, 50.0)) {

      good_matches.push_back(matches[i]);

    }

  }



  //-- 第五步:绘制匹配结果

  Mat img_match;

  Mat img_goodmatch;

  //drawMatches(第1张原始图像，第1张原始图像的关键点，第2张原始图像，第2张原始图像的关键点，第一个图像到第二个图像间的匹配（即两幅图像中对应的一对点），绘制图像结果)

  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);//距离较好的匹配点

  imwrite("./all_matches.png", img_match);

  imwrite("./good_matches.png", img_goodmatch);

  imshow("all matches", img_match);

  imshow("good matches", img_goodmatch);



  waitKey(0);



  return 0;

}

结果展示：
终端输入以下命令：

ORB特征点如下：

所有的特征匹配：

在阈值内的特征匹配：

以上内容参考《视觉slam十四讲》