第三篇 視覺里程計（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(三角化)

問題

1. 評估H和F矩陣的好壞時是如何轉為卡方檢驗的？
   已解決，見 SLAM中的卡方分布。
2. triangulator::triangulate還沒有徹底搞明白？