视觉SLAM十四讲之基于特征点的视觉里程计

视觉SLAM十四讲之基于特征点的视觉里程计

1 主要目标

1.1 特征点是如何匹配的;

1.2 如何通过2D-2D的特征点估计相机运动;

1.3 如何从2D-2D的匹配估计一个点的空间位置

1.4 3D-2D的PnP问题，线性解法以及BA解法

1.5 3D-3D的ICP问题，线性解法以及BA解法

2 特征提取与匹配

2.1 特征点法

SLAM系统分为前端和后端，前端称为视觉里程计，主要目的：根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。

视觉里程计的主要算法是特征点法（主流方法）和直接法。

常用的角点提取算法：Harris角点 Fast角点 GFTT角点

著名的特征点提取算法：SIFT SURF ORB;

特征点主要由关键点（Key-point）和描述子(descriptor)组成，关键点指的是特征点在图像里面的位置，描述子描述了关键点周围的像素信息。

2.1.1 ORB特征

ORB关键点（Oriented FAST）:改进的FAST角点;描述子（BRIEF）

提取ORB特征的具体步骤：

1、FAST角点提取
2、BRIEF描述子:对提取到的特征点的周围像素区域进行描述

FAST是一种角点，主要检测局部像素灰度变换明显的地方，速度快。

检测FAST角点的具体步骤：

[外链图片转存失败(img-q9307rRy-1566749854413)(/home/sunshine/.config/Typora/typora-user-images/1566641074353.png)]

还可以通过一系列的预测试操作，提高速度。

金字塔：对图像进行不同层次的降采样，获得不同分辨率的图像。（相当于resize）

ORB通过添加尺度和旋转的描述，解除了FAST角点不具有方向性和尺度的问题。

尺度通过构建图像金字塔的方式来实现尺度不变性，旋转是通过灰度质心（Intensity Centroid）法实现的。

2.2.2 BRIEF描述子

描述子：在关键点附近选取了128个随机像素点，得到了128维的向量，容易存储，适用于实时的图像匹配。

2.2 特征匹配

2.2.1 匹配方法：暴力匹配以及快速最近邻（FLANN）

最简单的匹配方法：暴力匹配（Brute-ForceMatcher）:即对每一个特征点与所有的另一副图像中的特征点测量描述子的距离,然后排序,取最近的一个作为匹配点。

计算距离方法：汉明距离（Hamming distance）:比较二进制不同位数的个数。

具体程序：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <chrono>//计算时间

using namespace std;
using namespace cv;

int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: feature_extraction img1 img2" << endl;
    return 1;
  }
  //-- 读取图像
  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<KeyPoint> keypoints_1, keypoints_2;//关键点
  Mat descriptors_1, descriptors_2;//描述子
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//匹配方式使用暴力匹配,距离使用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 描述子
  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<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;

  Mat outimg1;//定义输出图像
  drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
  imshow("ORB features", outimg1);//展现第一副图像的特征点

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> matches;//匹配方法
  t1 = chrono::steady_clock::now();
  matcher->match(descriptors_1, descriptors_2, matches);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;//统计匹配所需要的时间

  //-- 第四步:匹配点对筛选
  // 计算最小距离和最大距离
  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<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("all matches", img_match);
  imshow("good matches", img_goodmatch);
  waitKey(0);

  return 0;
}

手写ORB特征：

//
// Created by yang on 19-08-24.
//

#include <opencv2/opencv.hpp>
#include <string>
#include <nmmintrin.h>
#include <chrono>

using namespace std;

// 全局变量
string first_file = "./1.png";
string second_file = "./2.png";

// 32 bit unsigned int, will have 8, 8x32=256
typedef vector<uint32_t> DescType; // 结构体定义描述子类型

/**提取ORB特征函数声明
 * compute descriptor of orb keypoints   计算orb关键点的描述子
 * @param img input image 输入图像
 * @param keypoints detected fast keypoints
 * @param descriptors descriptors
 *
 * NOTE: if a keypoint goes outside the image boundary (8 pixels), descriptors will not be computed and will be left as
 * empty
 * 如果关键点超出图像边界,描述子将不会计算或者省略为空
 */
void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors);

/**暴力匹配函数声明
 * brute-force match two sets of descriptors暴力匹配两个描述子
 * @param desc1 the first descriptor
 * @param desc2 the second descriptor
 * @param matches matches of two images
 */
void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches);

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

  // load image
  cv::Mat first_image = cv::imread(first_file, 0);//以灰度图读入
  cv::Mat second_image = cv::imread(second_file, 0);
  //imshow("hah",first_image);
  //cvWaitKey(0);
  assert(first_image.data != nullptr && second_image.data != nullptr);

  // detect FAST keypoints1 using threshold=40
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  vector<cv::KeyPoint> keypoints1;
  cv::FAST(first_image, keypoints1, 40);//FAST算法检测角点
  /*可以看看FAST算法，输入的图像是灰度图,关键点，阈值（阈值为中心像素的亮度值与邻域的像素的亮度的阈值）,可以使用非极大值抑制，效果没有多大改变*/
  vector<DescType> descriptor1;
  ComputeORB(first_image, keypoints1, descriptor1);

  // same for the second
  vector<cv::KeyPoint> keypoints2;
  vector<DescType> descriptor2;
  cv::FAST(second_image, keypoints2, 40);
  ComputeORB(second_image, keypoints2, descriptor2);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;

  // find matches
  vector<cv::DMatch> matches;
  t1 = chrono::steady_clock::now();
  BfMatch(descriptor1, descriptor2, matches);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
  cout << "matches: " << matches.size() << endl;

  // plot the matches
  cv::Mat image_show;
  cv::drawMatches(first_image, keypoints1, second_image, keypoints2, matches, image_show);
  cv::imshow("matches", image_show);
  cv::imwrite("matches.png", image_show);
  cv::waitKey(0);

  cout << "done." << endl;
  return 0;
}

// -------------------------------------------------------------------------------------------------- //
// ORB pattern
int ORB_pattern[256 * 4] = {
  8, -3, 9, 5/*mean (0), correlation (0)*/,
  4, 2, 7, -12/*mean (1.12461e-05), correlation (0.0437584)*/,
  -11, 9, -8, 2/*mean (3.37382e-05), correlation (0.0617409)*/,
  7, -12, 12, -13/*mean (5.62303e-05), correlation (0.0636977)*/,
  2, -13, 2, 12/*mean (0.000134953), correlation (0.085099)*/,
  1, -7, 1, 6/*mean (0.000528565), correlation (0.0857175)*/,
  -2, -10, -2, -4/*mean (0.0188821), correlation (0.0985774)*/,
  -13, -13, -11, -8/*mean (0.0363135), correlation (0.0899616)*/,
  -13, -3, -12, -9/*mean (0.121806), correlation (0.099849)*/,