第三篇 視覺里程計（VO）的初始化過程以及openvslam中的相關實現詳解
視覺里程計（Visual Odometry, VO），通過使用相機提供的連續幀影象資訊（以及區域性地圖，先不考慮）來估計相鄰幀的相機運動，將這些相對執行轉換為以第一幀為參考的位姿資訊，就得到了相機載體（假設統一的剛體）的里程資訊。
初始化例項
在例項化跟蹤器的時候會例項化一個初始化例項，有一些比較重要的引數需要注意下，看程式碼註釋以及初始值，引數值也可以在yaml檔案中自定義。
// src/openvslam/module/initializer.h:83 //! max number of iterations of RANSAC (only for monocular initializer) const unsigned int num_ransac_iters_; //! min number of triangulated pts const unsigned int min_num_triangulated_; //! min parallax (only for monocular initializer) const float parallax_deg_thr_; //! reprojection error threshold (only for monocular initializer) const float reproj_err_thr_; //! max number of iterations of BA (only for monocular initializer) const unsigned int num_ba_iters_; //! initial scaling factor (only for monocular initializer) const float scaling_factor_; // src/openvslam/module/initializer.cc:16 initializer::initializer(const camera::setup_type_t setup_type, data::map_database* map_db, data::bow_database* bow_db, const YAML::Node& yaml_node) : setup_type_(setup_type), map_db_(map_db), bow_db_(bow_db), num_ransac_iters_(yaml_node["Initializer.num_ransac_iterations"].as<unsigned int>(100)), min_num_triangulated_(yaml_node["Initializer.num_min_triangulated_pts"].as<unsigned int>(50)), parallax_deg_thr_(yaml_node["Initializer.parallax_deg_threshold"].as<float>(1.0)), reproj_err_thr_(yaml_node["Initializer.reprojection_error_threshold"].as<float>(4.0)), num_ba_iters_(yaml_node["Initializer.num_ba_iterations"].as<unsigned int>(20)), scaling_factor_(yaml_node["Initializer.scaling_factor"].as<float>(1.0)) { spdlog::debug("CONSTRUCT: module::initializer"); }
使用不同的相機型別，初始化的方法也不相同，openvslam使用了perspective和bearing_vector兩種初始化方法。
// src/openvslam/module/initializer.cc:91 void initializer::create_initializer(data::frame& curr_frm) { // 將當前幀設定為初始化過程中的參考幀 init_frm_ = data::frame(curr_frm); // 將當前幀的特徵點當作previously matched coordinates prev_matched_coords_.resize(init_frm_.undist_keypts_.size()); for (unsigned int i = 0; i < init_frm_.undist_keypts_.size(); ++i) { prev_matched_coords_.at(i) = init_frm_.undist_keypts_.at(i).pt; } // initialize matchings (init_idx -> curr_idx) std::fill(init_matches_.begin(), init_matches_.end(), -1); // build a initializer initializer_.reset(nullptr); switch (init_frm_.camera_->model_type_) { case camera::model_type_t::Perspective: case camera::model_type_t::Fisheye: { initializer_ = std::unique_ptr<initialize::perspective>(new initialize::perspective(init_frm_, num_ransac_iters_, min_num_triangulated_, parallax_deg_thr_, reproj_err_thr_)); break; } case camera::model_type_t::Equirectangular: { initializer_ = std::unique_ptr<initialize::bearing_vector>(new initialize::bearing_vector(init_frm_, num_ransac_iters_, min_num_triangulated_, parallax_deg_thr_, reproj_err_thr_)); break; } } // 設定狀態為初始化狀態 state_ = initializer_state_t::Initializing; }
執行完上面的函式後回直接直接退出，然後讀取下一幀影象，進入初始化階段。下面的匹配過程，在上一篇已經詳細講過了，有一點需要注意在匹配完成後，會將匹配到的點的prev_matched_coords_改為當前幀的特徵點座標。如果匹配到的點數小於min_num_triangulated_（這裡是50），則直接重置初始化，會把下一幀當作初始化的參考幀。
bool initializer::try_initialize_for_monocular(data::frame& curr_frm) { assert(state_ == initializer_state_t::Initializing); match::area matcher(0.9, true); const auto num_matches = matcher.match_in_consistent_area(init_frm_, curr_frm, prev_matched_coords_, init_matches_, 100); if (num_matches < min_num_triangulated_) { // rebuild the initializer with the next frame reset(); return false; } // try to initialize with the current frame assert(initializer_); return initializer_->initialize(curr_frm, init_matches_); }
初始化過程
通過計算單應矩陣和基礎矩陣，評估出兩幀之間的變換矩陣。首先搞清楚幾個基本概念。
基礎(Fundamental)矩陣
假設\(I1\)與\(I2\)為相鄰幀，\(p1\)與\(p2\)為相鄰幀匹配到的特徵點，P為特徵點的空間位置。対極約束描述了影象中特徵點位置與幀間運動資訊之間的關係。用過幾何計算我們可以得到如下公式（對極約束）。具體推導可以參考兩檢視對極約束-基礎矩陣：
\[p_2^TK^{-T}t^{\wedge }RK^{-1}p_1=0\]
定義本質矩陣\(E=t^{\wedge }R\)，基礎矩陣\(F=K^{-T}EK\)。通過匹配點對兒的畫素位置可以計算出\(E或F\)，進而計算出\(R,t\)。
此外，假設相鄰相機只做了旋轉運動，即t為0，可以看到対極約束對於任意R都成立，因此想要通過對極約束評估相機運動內在要求不能只是純旋轉運動（可以使用單應矩陣來求解）。
單應Homography
單應(Homography)是射影幾何中的概念，又稱為射影變換。它把一個射影平面上的點(三維齊次向量)對映到另一個射影平面上，並且把直線對映為直線，具有保線性質。總的來說，單應是關於三維齊次向量的一種線性變換，可以用一個3×3的非奇異矩陣\(H\)表示:
\[
\begin{pmatrix}u_1\\ v_1\\ 1\end{pmatrix}=
\begin{pmatrix}h_{11} & h_{12} & h_{13}\\h_{21} & h_{22} & h_{23}\\ h_{31} & h_{32} & h_{33}\end{pmatrix}
\begin{pmatrix}u_2\\ v_2\\ 1\end{pmatrix}
\]
其中，\((u1,v1,1)^T\)表示影象1中的像點，\((u2,v2,1)^T\)是影象2中的像點，也就是可以通過單應矩陣H將影象2變換到影象1，描述的是兩個平面之間的對映關係。
上圖表示場景中的平面\(π\)在兩相機的成像，設平面\(π\)在第一個相機座標系下的單位法向量為\(n^T\)，其到第一個相機中心（座標原點）的距離為\(d\)，則平面\(π\)可表示為：
\[n^TX_1=d\]
其中，\(X_1\)是三維點P在第一相機座標系下的座標，其在第二個相機座標系下的座標為\(X_2\)，則
\[X_2 = RX_1 + t \\
X_2 = RX_1 + t\frac{1}{d}n^TX_1=(R+t\frac{1}{d}n^T)X_1=HX_1 \\
H = R+t\frac{1}{d}n^T\]
假設\(p_1,p_2\)為\(X_1,X_2\)對應的影象上的點，則
\[X_1=K^{-1}p_1, X_2=K^{-2}p_1\\
H = K(R+t\frac{1}{d}n^T)K^{-1}\]
這樣求解出\(H\)便可以求解出\(R,t\)，並卻\(t=0\)也不影響求解R。
實踐
在實際中，通常會同時計算H和F。
\\ src/openvslam/initialize/perspective.cc:27
bool perspective::initialize(const data::frame& cur_frm, const std::vector<int>& ref_matches_with_cur)
{
...
// compute H and F matrices
auto homography_solver = solve::homography_solver(ref_undist_keypts_, cur_undist_keypts_, ref_cur_matches_, 1.0);
auto fundamental_solver = solve::fundamental_solver(ref_undist_keypts_, cur_undist_keypts_, ref_cur_matches_, 1.0);
std::thread thread_for_H(&solve::homography_solver::find_via_ransac, &homography_solver, num_ransac_iters_, true);
std::thread thread_for_F(&solve::fundamental_solver::find_via_ransac, &fundamental_solver, num_ransac_iters_, true);
thread_for_H.join();
thread_for_F.join();
...
}
初始化
使用兩個執行緒分別計算H和F矩陣
homography_solver::find_via_ransac
fundamental_solver::find_via_ransac
通過判斷0.4<rel_score_H = score_H / (score_H + score_F來決定用H還是F來評估R，t，
在評估的過程中還會通過三角化確定特徵點的空間位置資訊
通過一些列複雜的篩選得出到滿足條件的R，t即算完成初始化看完下面的文章再回過頭來看這段話。總的來說初始化過程涉及到多視幾何的基礎內容很多，openvlsam的實現很大程度上都是借鑑ORB2的實現方法（三角化除外）。可以看到由於要求前後幀（可以中間隔若干幀）特徵點有一定的視差，想要成功的初始化就一定要發生位移。由於同時評估H和F矩陣，純旋轉也有初始化成功的可能性，但是需要有比較多的特徵點在相同的平面上。不過最好還是就有旋轉又有平移吧。這裡在評估過程中都是採用歸一化平面上的特徵點（沒有深度資訊），所以得到的t是不知道其物理尺度的，然後三角化又是基於R，t求解的，因此特徵點的空間位置值，也是無法知道其物理尺度的，可以認為尺度和t一致。
homography_solver::find_via_ransac
首先正則化特徵點，目的是為了後面使用RANSAC（就是隨即從大樣本集抽取小樣本集用於求解問題的方法）計算SVD時得到更一致的解決，消除特徵點在影象中位置分佈對結果的影響。更詳細的解釋可參考《Multiple View Geometry in Computer Vision 》P104 “4.4 Transformation invariance and normalization”。
openvslam取樣的方法略有不同，分析如下。
\\src/openvslam/solve/common.cc:6
void normalize(const std::vector<cv::KeyPoint>& keypts, std::vector<cv::Point2f>& normalized_pts, Mat33_t& transform) {計算單應矩陣
正則化特徵點
迴圈num_ransac_iters_=100次
生成8個點的RANSAC序列
計算正則化特徵點的單應矩陣compute_H_21
計算特徵點的單應矩陣
評估當前求解出單應矩陣的好壞，更新最好的結果
將篩選過的好的點重新評估H矩陣，得到最優結果正則化特徵點
正則化的公式如下，還是比較直觀的。
\[x_i^{\prime} = {x_i - \bar{x} \over \sigma_x},y_i^{\prime} = {y_i - \bar{y} \over \sigma_y} \\
\bar{x} = {\Sigma{x_i} \over N},\bar{y} = {\Sigma{y_i} \over N} \\
\sigma_x = {\Sigma{\left|x_i - \bar{x}\right|} \over N}, \sigma_y = {\Sigma{\left|y_i - \bar{y}\right|} \over N} \\
\begin{pmatrix}x_i^{\prime} \\ y_i^{\prime} \\ 1\end{pmatrix}=
\begin{pmatrix}1/\sigma_x & 0 & -\frac{\bar{x}}{\sigma_x} \\ 0 & 1/\sigma_y & -\frac{\bar{y}}{\sigma_y} \\ 0 & 0 & 1\end{pmatrix}
\begin{pmatrix}x \\ y \\ 1\end{pmatrix}=T\begin{pmatrix}x \\ y \\ 1\end{pmatrix}\]
\(x_i^{\prime},y_i^{\prime}\)為正則化後的座標值，T就是正則化矩陣。
compute_H_21
參考https://www.uio.no/studier/emner/matnat/its/UNIK4690/v16/forelesninger/lecture_4_3-estimating-homographies-from-feature-correspondences.pdf
計算特徵點的單應矩陣
程式中normalized_H_21表示則正則化的參考幀到當前幀的單應矩陣。
H_21_in_sac: 幀1->幀2的homography， 注意21表示的是1->2。
\[p^{\prime}_{cur} = T_{cur}p_{cur} \\
p^{\prime}_{ref} = T_{ref}p_{ref} \\
\]
\(T_{cur},T_{ref}\)是當前幀特徵點和參考幀特徵點的正則化矩陣。
\[p^{\prime}_{cur}=H^{\prime}_{cr}p^{\prime}_{ref}\]
\(H^{\prime}_{rc}\)是正則化當前幀特徵點與正則化參考幀特徵點的單應矩陣。展開得到:
\[
T_{cur}p_{cur}=H^{\prime}_{cr}T_{ref}p_{ref} \\
p_{cur}=T^{-1}_{cur}H^{\prime}_{cr}T_{ref}p_{ref}
\]
因此當前幀特徵點與參考幀特徵點的單應矩陣\(H_{cr}\)為：
\[H_{cr}=T^{-1}_{cur}H^{\prime}_{cr}T_{ref}\]
評估當前求解出單應矩陣的好壞
說實話，研究了半天也沒搞清楚是如何和卡方檢驗聯絡起來的。在我看來就是定義了一個最小重投影誤差門限（5.991），重投影誤差小於門限就標記為inlier, 將最好的結果儲存下來。這裡面作者只累加小於門限的重投影誤差，即score += 門限值 - 重投影誤差，這個差值是>0的，這樣做的好處大家可以思考下。
fundamental_solver::find_via_ransac
同樣是先正則化特徵點，然後採用RANSAC的方法計算F矩陣compute_F_21，過程和計算H矩陣完全一致。
從F或H恢復R，t
//src/openvslam/initialize/perspective.cc:91
bool perspective::reconstruct_with_H(const Mat33_t& H_ref_to_cur, const std::vector<bool>& is_inlier_match)
//src/openvslam/initialize/perspective.cc:114
bool perspective::reconstruct_with_F(const Mat33_t& F_ref_to_cur, const std::vector<bool>& is_inlier_match)reconstruct_with_H: Motion and structure from motion in a piecewise planar environment (Faugeras et al. in IJPRAI 1988)
reconstruct_with_F: https://en.wikipedia.org/wiki/Essential_matrix#Determining_R_and_t_from_E
在得到若干組候選的R，t後，需要計算找到最佳的R，t。
// src/openvslam/initialize/base.cc:31
bool base::find_most_plausible_pose尋找最佳位姿
逐個候選位姿進行check_pose
選取有效點最多的那組位姿，做以下判斷：
1.有效點數必須大於min_num_triangulated_（50）;
2.一個好的結果應該是隻有一組有比較多的有效點，，如果發現有很多組都有個數相近的有效點，則這些位姿都不對；
3.視差不能太小，因為太小的視差評估出的深度不可靠，這裡的門限parallax_deg_thr_=1度；
在評估R，t的同時會對特徵點進行三角化，注意儲存到triangulated_pts_的點是特徵點在參考幀下的三維座標。
check_pose
// src/openvslam/initialize/base.cc:86
unsigned int base::check_pose迴圈所有的匹配點
三角化計算得到特徵點在參考幀下的三維座標
計算視差角的餘弦值（向量內積的方法）
排除視差小於0.5度和深度為負數的三維座標
驗證特徵點是否在參考幀和當前幀中可見，重投影誤差L2 < 16
以上條件都滿足的點才認為有效
對視差排序後返回第50小或者（個數小於50則返回最小）的視差Triangulate(三角化)
問題
- 評估H和F矩陣的好壞時是如何轉為卡方檢驗的？
- triangulator::triangulate還沒有徹底搞明白？