第二篇 特征点匹配以及openvslam中的相关实现详解

配置文件

在进入正题之前先做一些铺垫，在openvslam中，配置文件是必须要正确的以.yaml格式提供，通常需要指明使用的相机模型，ORB特征检测参数，跟踪参数等。

#==============#
# Camera Model #
#==============#

Camera.name: "EuRoC monocular"
Camera.setup: "monocular"
Camera.model: "perspective"

# 相机内参
Camera.fx: 458.654
Camera.fy: 457.296
Camera.cx: 367.215
Camera.cy: 248.375

# 畸变参数
Camera.k1: -0.28340811
Camera.k2: 0.07395907
Camera.p1: 0.00019359
Camera.p2: 1.76187114e-05
Camera.k3: 0.0

# 帧率
Camera.fps: 20.0
# 图像宽高
Camera.cols: 752
Camera.rows: 480

# 颜色模式
Camera.color_order: "Gray"

#================#
# ORB Parameters #
#================#

Feature.max_num_keypoints: 1000
Feature.scale_factor: 1.2
Feature.num_levels: 8
Feature.ini_fast_threshold: 20
Feature.min_fast_threshold: 7

...

相机参数

enum class setup_type_t {
    Monocular = 0,
    Stereo = 1,
    RGBD = 2
};

enum class model_type_t {
    Perspective = 0,
    Fisheye = 1,
    Equirectangular = 2
};

enum class color_order_t {
    Gray = 0,
    RGB = 1,
    BGR = 2
};

可以看到openvslam支持单目（Monocular）、双目（Stereo）以及RGBD相机，成像模型支持Perspective、Fisheye（鱼眼）、Equirectangular（全景，等距圆柱图）。

其中Perspective、Fisheye的内参、外参都和opencv一致，使用中可以用opencv做相机内参标定。

颜色模式根据输入选择就好，由于openvslam特征提取采用的ORB，最终输入的图片都会转为灰度图。

关于ORB参数第一篇中已有详细解释，不再赘述。

配置过程

我们从最基础的单目slam开始分析，即example/run_image_slam.cc。将配置文件路径传入后，会在下面的程序中作配置初始化。

// example/run_image_slam.cc:191
// load configuration
std::shared_ptr<openvslam::config> cfg;
try {
    cfg = std::make_shared<openvslam::config>(config_file_path->value());
}
catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

加载完相机参数后直接实例化响应的相机类型；

system初始化

// example/run_image_slam.cc:39
// build a SLAM system
openvslam::system SLAM(cfg, vocab_file_path);

system初始化内容比较多，我们先来看匹配功能需要哪些必要的条件。首先加载ORB辞典，用于作回环检测，先不用关心：

// src/openvslam/system.cc:46
bow_vocab_ = new data::bow_vocabulary();
bow_vocab_->loadFromBinaryFile(vocab_file_path);

在初始化跟踪模块时，会实例化orb特征点提取器。可以看到初始化中用的特征提取最大点数是正常的两倍。

// src/openvslam/tracking_module.cc:29
extractor_left_ = new feature::orb_extractor(cfg_->orb_params_);
if (camera_->setup_type_ == camera::setup_type_t::Monocular) {
    ini_extractor_left_ = new feature::orb_extractor(cfg_->orb_params_);
    ini_extractor_left_->set_max_num_keypoints(ini_extractor_left_->get_max_num_keypoints() * 2);
}
if (camera_->setup_type_ == camera::setup_type_t::Stereo) {
    extractor_right_ = new feature::orb_extractor(cfg_->orb_params_);
}

跟踪器

在跟踪之前读取图像数据，然后送入跟踪器，可以看到输入的图像会被直接转为灰度图。然后进行帧初始化。

// example/run_image_slam.cc:62
SLAM.track_for_monocular(img, frame.timestamp_, mask);

帧初始化

帧初始化有三种初始化函数分别对应单目、双目和深度相机。本篇只看单目的。

// src/openvslam/tracking_module.cc:85
// create current frame object
if (tracking_state_ == tracker_state_t::NotInitialized || tracking_state_ == tracker_state_t::Initializing) {
    curr_frm_ = data::frame(img_gray_, timestamp, ini_extractor_left_, bow_vocab_, camera_, cfg_->true_depth_thr_, mask);
}
else {
    curr_frm_ = data::frame(img_gray_, timestamp, extractor_left_, bow_vocab_, camera_, cfg_->true_depth_thr_, mask);
}

src/openvslam/data/frame.cc:20
帧初始化
    从system初始化中实例化的orb特征点提取器获取提取器的一些信息
    ORB特征提取（上篇已经详细讲过）
    根据相机模型去畸变（直接使用opencv函数）
    将去畸变的点转为相机坐标下归一化的空间点（convert_keypoints_to_bearings）
    初始化landmarks（特征点的空间位置信息）容器
    特征点珊格化（assign_keypoints_to_grid）

convert_keypoints_to_bearings

下式中，\(u,v\)是图像中的点，\(X_c, Y_c,Z_c\)是对应的相机坐标系下的坐标值，\(\begin{pmatrix}X_c/Z_c\\ Y_c/Z_c \\ 1 \end{pmatrix}\)是相机坐标系下归一化平面上（z=1）的坐标值。

\[\begin{pmatrix}u\\ v\\ 1\end{pmatrix}=
\begin{pmatrix}f_x & 0 & c_x\\0 & f_y & c_y\\ 0 & 0 & 1\end{pmatrix}
\begin{pmatrix}X_c\\ Y_c \\ Z_c \end{pmatrix}=
\frac{1}{Z_c}\begin{pmatrix}f_x & 0 & c_x\\0 & f_y & c_y\\ 0 & 0 & 1\end{pmatrix}\begin{pmatrix}X_c/Z_c\\ Y_c/Z_c \\ 1 \end{pmatrix}
\]

//src/openvslam/camera/perspective.cc:124
const auto x_normalized = (undist_keypts.at(idx).pt.x - cx_) / fx_;
const auto y_normalized = (undist_keypts.at(idx).pt.y - cy_) / fy_;
const auto l2_norm = std::sqrt(x_normalized * x_normalized + y_normalized * y_normalized + 1.0);
bearings.at(idx) = Vec3_t{x_normalized / l2_norm, y_normalized / l2_norm, 1.0 / l2_norm};

\[\begin{pmatrix}X_n\\ Y_n \\ 1 \end{pmatrix}=
\begin{pmatrix}f_x & 0 & c_x\\0 & f_y & c_y\\ 0 & 0 & 1\end{pmatrix}^{-1}\begin{pmatrix}u\\ v\\ 1\end{pmatrix}
\]

程序中计算的就是归一化平面上的座标值，然后将该座标值再次进行归一化。

assign_keypoints_to_grid

将所有的特征点分配到设定的珊格中，匹配时，由于运动特征点的位置会发生变化，但是这个变化是有限的，我们只需搜索之前特征点附近的特征点，通过珊格化，可以快速的获取到指定珊格中的特征点，加速匹配过程。

// 3072个珊格
num_grid_cols_ = 64
num_grid_rows_ = 48

// src/openvslam/camera/perspective.cc:26
inv_cell_width_ = static_cast<double>(num_grid_cols_) / (img_bounds_.max_x_ - img_bounds_.min_x_);
inv_cell_height_ = static_cast<double>(num_grid_rows_) / (img_bounds_.max_y_ - img_bounds_.min_y_);

//img_bounds_ 是图像反畸变后的区间，由畸变系数和相机模型决定。
//区间的边界值可能为负，比如鱼眼相机反畸变后的图像区域肯定比原始图像大。

匹配

前面做了这么多铺垫，终于来到了正题。这里我们把匹配的内容从openvslam的流程中剥离出来分析并且与opencv中自带的算法作比较。

match_in_consistent_area

匹配问题描述：

已知参考帧（图像）的特征点（每个特征点包含哪些内容？忘记的话，看上篇文章）和当前帧的特征点信息，求当前帧与参考帧相同的特征点的过程，叫做匹配。

// match/openvslam/match/area.cc:8
/*
frm_1: 参考帧
frm_2：当前镇
prev_matched_pts： 参考帧中的特征点
matched_indices_2_in_frm_1： 经过匹配后，参考帧中的特征点在当前帧的序号
margin： 设定特征匹配时在原特征点位置搜索的范围大小，单位是像素。
*/
unsigned int area::match_in_consistent_area(data::frame& frm_1, data::frame& frm_2, std::vector<cv::Point2f>& prev_matched_pts,
                                            std::vector<int>& matched_indices_2_in_frm_1, int margin)

空间一致匹配（只匹配金字塔第0层，即原始图像上的特征点）
    每一个参考帧中的特征点在珊格化后都会有对应的珊格序号，获取当前帧相同序号以及周围一定范围的珊格中的特征点，作为匹配候选特征点
    计算候选特征点与参考帧特征点的汉明距离，得到匹配的特征点具体思路看下文
    通过角度检查，筛除不合格的匹配点

汉明距离

Hamming Distance：表示两个等长字符串在对应位置上不同字符的数目，数值越小说明越相似。还不清楚的话，自行查阅相关内容。

openvslam在计算候选特征点的时候会将最小和第二小的汉明距离记录下来，满足下面两个条件才认为匹配成功：

1. 最小距离必须<HAMMING_DIST_THR_LOW，这里HAMMING_DIST_THR_LOW=50；回忆下前一篇文章，描述子的长度为32字节=256位，这里就要求小于50个位不同才算匹配；
2. 这里有篇文章《Distinctive Image Features from Scale-Invariant Keypoints》，作者是David G. Lowe，lowe_ratio_应该是源自这篇文章，取值0.9。这里的条件就是说，要求最小值一定要比次小值有比较大的差值，也就是要求匹配到的特征点一定要是这些候选者中有较强的区分性，否则就不要，正所谓宁缺毋滥，尽可能得到准确的匹配结果。

// match/openvslam/match/area.cc:66
if (second_best_hamm_dist * lowe_ratio_ < static_cast<float>(best_hamm_dist)) {
    continue;
}

关于汉明距离的计算有十分高效的方法，具体看程序。

角度检查

openvslam中首先统计所有匹配的特征点角度差的直方图（直方图的步长为360/30=12度），直方图按值由大到小排序，然后将不再前3直方图中的特征点认为无效点，排除掉。基本的思想应该是区域内的角度变化应该是一致的。这部分代码使用大量现代C++语法，对于还不是很熟悉新特性的同学来说，可以好好看看。

//match/openvslam/match/angle_checker.h:20
explicit angle_checker(const unsigned int histogram_length = 30, // 直方图的步长360/histogram_length
                       const unsigned int num_bins_thr = 3);     // 最值有效门限

结果对比

下图为openvslam提取的特征点，分布是不是十分的均匀？原因上篇已经分析的很清楚了。

下图为使用opencv提取的ORB特征点，明显不分布不够均匀。

openvslam的匹配方法比起暴力匹配有很多优势。运动时，假设运动不是十分激烈，前后帧中的特征点的位置的变化是有限的，在上帧位置附近搜索的方法自然比暴力搜索更高效科学，而且通过珊格化分类特征点，更加速了匹配过程。

测试代码

https://github.com/hardjet/slam

问题

1. true_depth_thr_的作用？
2. 再次进行归一化的目的？
3. 珊格有3072个,通常并没有这么多特征点,是否可以减小?
4. 只匹配原始层上的特征点？