第二篇 特徵點匹配以及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在計算候選特徵點的時候會將最小和第二小的漢明距離記錄下來，滿足下面兩個條件才認為匹配成功：
- 最小距離必須<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個,通常並沒有這麼多特徵點,是否可以減小?
- 只匹配原始層上的特徵點？