第四篇 跟蹤過程以及openvslam中的相關(guān)實現(xiàn)詳解

在成功初始化之后，會創(chuàng)建地圖以及局部地圖。

image

創(chuàng)建地圖

在初始化正常過后，緊接著會創(chuàng)建地圖

// src/openvslam/module/initializer.cc:67
// create new map, then check the state is succeeded or not
create_map_for_monocular(curr_frm);

創(chuàng)建單目地圖
    在init_matches_中，將所有匹配點對兒中沒有三角化的位置標記為無效
    以初始幀為原點，設(shè)置當前幀的位姿
    創(chuàng)建初始幀和當前幀的關(guān)鍵幀
    計算幀描述子對應(yīng)的bow向量
    將關(guān)鍵幀添加入map_db_中
    更新幀統(tǒng)計信息
    逐個匹配點
        創(chuàng)建landmark：lm就是特征點對應(yīng)的世界座標下的點
        將lm和關(guān)鍵幀關(guān)聯(lián)起來
        向lm中添加可觀測信息：關(guān)鍵幀和對應(yīng)的特征點id
        計算lm的描述子
        計算lm的幾何信息
        將lm加入map_db_
    global_bundle_adjuster
    計算初始關(guān)鍵幀的中位深度值
    更改當前關(guān)鍵幀t的尺度和lm尺度，（將深度中間值設(shè)置為1的尺度，1.0 / median_depth）
    更新當前幀的位姿
    設(shè)置地圖的起始關(guān)鍵幀

計算lm的描述子

一個lm可能被很多幀看到，每個幀中由于拍攝的時間、空間、光照條件的原因?qū)е孪嗤奶卣鼽c的描述子會稍微不同，通過計算找到一個與其他描述子距離相近的描述子作為最終lm的描述子。

計算lm的幾何信息

max_valid_dist_ = dist * scale_factor;
min_valid_dist_ = max_valid_dist_ / ref_keyfrm->scale_factors_.at(num_scale_levels - 1);

max_valid_dist_: 特征點(世界座標系下)到參考幀相機位置（世界座標系下）的最大有效距離，在計算的時候?qū)⑻卣鼽c所處的圖像金字塔層數(shù)也考慮進來
min_valid_dist_: 特征點(世界座標系下)到參考幀相機位置（世界座標系下）的最小有效距離



Vec3_t mean_normal = Vec3_t::Zero();
unsigned int num_observations = 0;
for (const auto& observation : observations) {
    auto keyfrm = observation.first;
    const Vec3_t cam_center = keyfrm->get_cam_center();
    const Vec3_t normal = pos_w_ - cam_center;
    mean_normal = mean_normal + normal / normal.norm();
    ++num_observations;
}

mean_normal_:

計算初始關(guān)鍵幀的中位深度值

將關(guān)鍵幀中的lm（世界坐標系）轉(zhuǎn)為關(guān)鍵幀坐標系下，對深度排序后獲取中間深度值。

更新局部地圖及后續(xù)

// src/openvslam/tracking_module.cc:183
update_local_map();


更新局部地圖
    更新局部關(guān)鍵幀(局部關(guān)鍵幀的限制是60個)
        統(tǒng)計出當前幀和相鄰的關(guān)鍵幀共享的lm數(shù)量
        通過比較共享lm的數(shù)量，將共享lm數(shù)量最多的關(guān)鍵幀設(shè)置為最近的共視關(guān)鍵幀
        將有共視的關(guān)鍵幀都添加到local_keyfrms_
        將有共視的關(guān)鍵幀的相鄰關(guān)鍵幀中的top10也加入local_keyfrms_
        將最近的共視關(guān)鍵幀設(shè)置為當前跟蹤模塊的參考關(guān)鍵幀
    更新局部lm:將local_keyfrms_中的lm都添加為local_landmarks_
將所有的關(guān)鍵幀傳入mapper_模塊
設(shè)置跟蹤模塊狀態(tài)為Tracking

保存參考關(guān)鍵幀到當前幀的變換矩陣=last_cam_pose_from_ref_keyfrm_

tracking流程

image
更新lm：全局BA會優(yōu)化lm的位置，因此將上一幀lm中調(diào)整為優(yōu)化過的值
更新上一幀相機位姿：mapping模塊有可能優(yōu)化該位姿
設(shè)置當前幀的參考關(guān)鍵幀：=跟蹤模塊的參考關(guān)鍵幀
跟蹤當前幀(track_current_frame)
如果成功：
    更新局部地圖
    使用局部地圖對當前幀進行優(yōu)化
如果成功：
    更新運動學(xué)模型update_motion_model
更新幀統(tǒng)計信息
如果跟蹤后很快就丟失，則會重新開始跟蹤
跟蹤成功后，檢測是否需要插入關(guān)鍵幀
    1.當前的幀id不大于上個關(guān)鍵幀id+max_num_frms_（這里max_num_frms_=fps）
    2.當前的幀id不小于上個關(guān)鍵幀id+min_num_frms_（這里max_num_frms_=0）
    3.有一定的匹配點但是不能太多，太多說明視覺變化比較小，不需要新關(guān)鍵點；
添加關(guān)鍵幀
    單目直接添加
    立體和RGBD，按深度降序排列，添加前100深度且大于true_depth_thr_的lm
清理當前幀的lm

跟蹤當前幀

\\ src/openvslam/tracking_module.cc:278
bool tracking_module::track_current_frame()

有三種跟蹤方法：
motion_based_track：
bow_match_based_track：
robust_match_based_track：
一種重點位方法：
relocalize

// Tracking mode
if (velocity_is_valid_ && last_reloc_frm_id_ + 2 < curr_frm_.id_) {
    // if the motion model is valid
    succeeded = frame_tracker_.motion_based_track(curr_frm_, last_frm_, velocity_);
}
if (!succeeded) {
    succeeded = frame_tracker_.bow_match_based_track(curr_frm_, last_frm_, ref_keyfrm_);
}
if (!succeeded) {
    succeeded = frame_tracker_.robust_match_based_track(curr_frm_, last_frm_, ref_keyfrm_);
}

bow_match_based_track

計算當前幀BoW
當前幀與關(guān)鍵幀做匹配：使用bow tree
如果匹配點數(shù)大于num_matches_thr_=10，將上一幀的位姿作為當前幀的位姿的初始值，使用pose_optimizer_進行優(yōu)化。
剔除當前幀中不好的lm（優(yōu)化中會將一些lm設(shè)值為outliers）

robust_match_based_track

計算當前幀和參考關(guān)鍵幀的匹配情況（match_frame_and_keyframe）
    使用暴力匹配的方式獲取匹配點（brute_force_match）
    計算本質(zhì)矩陣E，獲取匹配點（essential_solver）
如果匹配點數(shù)大于num_matches_thr_=10，將上一幀的位姿作為當前幀的位姿的初始值，使用pose_optimizer_進行優(yōu)化。
剔除當前幀中不好的lm（優(yōu)化中會將一些lm設(shè)值為outliers）

在計算本質(zhì)矩陣E的時候使用的是歸一化平面上的點對兒，而不是圖像上的像素點對兒。

motion_based_track

基于運動模型跟蹤，這里的運動模型就是恒速運動模型。

首先使用恒速運動模型更新當前位姿
將最后一幀中觀察到的3D點重新投影到當前幀并在幀中記錄相應(yīng)的lm信息（match_current_and_last_frames）
    計算當前幀到上一幀的平移向量trans_lc
    非單目可以判斷運動方向
    將最后一幀的特征點對應(yīng)的3D點重新投影到當前幀，在重投影位置尋找特征點，進行匹配
pose optimization
剔除當前幀中不好的lm（優(yōu)化中會將一些lm設(shè)值為outliers）

重定位

跟蹤失敗后會調(diào)用該函數(shù)進行重定位。
tracking_module初始化中會對module::relocalizer進行初始化

relocalizer( data::bow_database* bow_db,
             const double bow_match_lowe_ratio = 0.75, const double proj_match_lowe_ratio = 0.9,
             const unsigned int min_num_bow_matches = 20, const unsigned int min_num_valid_obs = 50);

重定位有關(guān)的變量

//! initial candidates for loop or relocalization
std::unordered_set<keyframe*> init_candidates_;

//! number of shared words between the query and the each of keyframes contained in the database
std::unordered_map<keyframe*, unsigned int> num_common_words_;

//! similarity scores between the query and the each of keyframes contained in the database
std::unordered_map<keyframe*, float> scores_;

//! pairs of score and keyframe which has the larger score than the minimum one
std::vector<std::pair<float, keyframe*>> score_keyfrm_pairs_;

//! pairs of total score and keyframe which has the larger score than the minimum one
std::vector<std::pair<float, keyframe*>> total_score_keyfrm_pairs_;

獲取候選關(guān)鍵幀（acquire_relocalization_candidates）
    統(tǒng)計地圖中所有當前幀有共享的單詞的關(guān)鍵幀以及共享單詞的數(shù)量（set_candidates_sharing_words）
    將最大共享單詞數(shù)量*0.8作為過濾門限
    計算大于門限的關(guān)鍵幀與當前幀的相似性得分（DBoW內(nèi)置計算方法）
    設(shè)置相似分門限過濾
    計算每個候選關(guān)鍵幀（score_keyfrm_pairs）鄰域的得分并取總和，返回最高分best_total_score
    將大于0.75*best_total_score的關(guān)鍵幀設(shè)置為候選關(guān)鍵幀
遍歷候選幀
    關(guān)鍵幀與當前幀進行特征點匹配（bow_matcher_.match_frame_and_keyframe）,>50個才算有效
    構(gòu)建pnp_solvers
使用PnP(+RANSAC)求解位姿
使用pose_optimizer優(yōu)化
重投影匹配檢驗proj_matcher_.match_frame_and_keyframe
再次使用pose_optimizer優(yōu)化
有效特征點>50，即認為重定位成功

使用局部地圖對當前幀進行優(yōu)化

//src/openvslam/tracking_module.cc:209
succeeded = optimize_current_frame_with_local_map();

通過將局部lm重投影到當前幀的方式獲取更多的2D-3D點對兒（search_local_landmarks）
    當前幀中的lm不需要重投影，標記對應(yīng)的lm
    在局部lm中逐個使用can_observe函數(shù)檢測可以被觀測到的lm
    投影匹配當前幀和局部lm
pose_optimizer
計算跟蹤到的lm數(shù)量，判斷是否少于門限

can_observe
獲取lm的世界座標值pos_w
判斷該lm是否可以重投影到當前幀的圖像平面
通過判斷有效距離檢查是否在orb_scale中
檢測角度是否有效0.5度
預(yù)測當前l(fā)m所對應(yīng)的圖像金字塔層數(shù)

更新運動學(xué)模型

這里的運動學(xué)模型是恒速模型：
更新速度velocity = curr_frm.cam_pose_cw * last_frm_cam_pose_wc
后面用來更新位姿：curr_frm.set_cam_pose(velocity * last_frm.cam_pose_cw) 這時候last_frm.cam_pose_cw就是上一幀的curr_frm.cam_pose_cw

問題

  1. 恒速模型沒搞明白；
    T_{C_k}^W表示k時刻相機坐標系到世界坐標系的變換矩陣，恒速模型是說相機k到k-1時刻的變化等于k-1到k-2時刻的變化，即T_{C_k}^{C_{k-1}}=T_{C_{k-1}}^{C_{k-2}}。因此

T_{C_k}^W = T_{C_{k-1}}^W*T_{C_{k}}^{C_{k-1}} = T_{C_{k-1}}^W*T_{C_{k-1}}^{C_{k-2}}

又T_{C_{k-1}}^W=T_{C_{k-2}}^W*T_{C_{k-1}}^{C_{k-2}}可得

T_{C_{k-1}}^{C_{k-2}}=(T_{C_{k-2}}^W)^{-1}*T_{C_{k-1}}^W
因此

T_{C_k}^W = T_{C_{k-1}}^W*(T_{C_{k-2}}^W)^{-1}*T_{C_{k-1}}^W
再取逆，整理得到

T_{W_k}^C = T_W^{C_{k-1}}*(T_{C_{k-2}}^W)^{-1}*T_W^{C_{k-1}}

// src/openvslam/tracking_module.cc:310
velocity_ = curr_frm_.cam_pose_cw_ * last_frm_cam_pose_wc;

// src/openvslam/module/frame_tracker.cc:22
curr_frm.set_cam_pose(velocity * last_frm.cam_pose_cw_);

對應(yīng)程序，velocity_就是T_W^{C_{k-1}}*(T_{C_{k-2}}^W)^{-1}

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成，瀏覽時請結(jié)合常識與多方信息審慎甄別。
平臺聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點，簡書系信息發(fā)布平臺，僅提供信息存儲服務(wù)。

相關(guān)閱讀更多精彩內(nèi)容

友情鏈接更多精彩內(nèi)容