配置文件
在进入正题之前先做一些铺垫,在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在计算候选特征点的时候会将最小和第二小的汉明距离记录下来,满足下面两个条件才认为匹配成功:
- 最小距离必须<HAMMING_DIST_THR_LOW,这里HAMMING_DIST_THR_LOW=50;回忆下前一篇文章,描述子的长度为32字节=256位,这里就要求小于50个位不同才算匹配;
- 这里有篇文章《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
问题
- true_depth_thr_的作用?
- 再次进行归一化的目的?
- 珊格有3072个,通常并没有这么多特征点,是否可以减小?
- 只匹配原始层上的特征点?