第四篇 跟踪过程以及openvslam中的相关实现详解

• 2019 年 10 月 3 日
• 筆記

在成功初始化之后，会创建地图以及局部地图。

创建地图

在初始化正常过后，紧接着会创建地图

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

创建单目地图      在init_matches_中，将所有匹配点对儿中没有三角化的位置标记为无效      以初始帧为原点，设置当前帧的位姿      创建初始帧和当前帧的关键帧      计算帧描述子对应的bow向量      将关键帧添加入map_db_中      更新帧统计信息      逐个匹配点          创建landmark：lm就是特征点对应的世界座标下的点          将lm和关键帧关联起来          向lm中添加可观测信息：关键帧和对应的特征点id          计算lm的描述子          计算lm的几何信息          将lm加入map_db_      global_bundle_adjuster      计算初始关键帧的中位深度值      更改当前关键帧t的尺度和lm尺度，（将深度中间值设置为1的尺度，1.0 / median_depth）      更新当前帧的位姿      设置地图的起始关键帧

计算lm的描述子

一个lm可能被很多帧看到，每个帧中由于拍摄的时间、空间、光照条件的原因导致相同的特征点的描述子会稍微不同，通过计算找到一个与其他描述子距离相近的描述子作为最终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_: 特征点(世界座标系下)到参考帧相机位置（世界座标系下）的最大有效距离，在计算的时候将特征点所处的图像金字塔层数也考虑进来  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_:

计算初始关键帧的中位深度值

将关键帧中的lm（世界坐标系）转为关键帧坐标系下，对深度排序后获取中间深度值。

更新局部地图及后续

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

更新局部地图      更新局部关键帧(局部关键帧的限制是60个)          统计出当前帧和相邻的关键帧共享的lm数量          通过比较共享lm的数量，将共享lm数量最多的关键帧设置为最近的共视关键帧          将有共视的关键帧都添加到local_keyfrms_          将有共视的关键帧的相邻关键帧中的top10也加入local_keyfrms_          将最近的共视关键帧设置为当前跟踪模块的参考关键帧      更新局部lm:将local_keyfrms_中的lm都添加为local_landmarks_  将所有的关键帧传入mapper_模块  设置跟踪模块状态为Tracking

保存参考关键帧到当前帧的变换矩阵=last_cam_pose_from_ref_keyfrm_

tracking流程

更新lm：全局BA会优化lm的位置，因此将上一帧lm中调整为优化过的值  更新上一帧相机位姿：mapping模块有可能优化该位姿  设置当前帧的参考关键帧：=跟踪模块的参考关键帧  跟踪当前帧(track_current_frame)  如果成功：      更新局部地图      使用局部地图对当前帧进行优化  如果成功：      更新运动学模型update_motion_model  更新帧统计信息  如果跟踪后很快就丢失，则会重新开始跟踪  跟踪成功后，检测是否需要插入关键帧      1.当前的帧id不大于上个关键帧id+max_num_frms_（这里max_num_frms_=fps）      2.当前的帧id不小于上个关键帧id+min_num_frms_（这里max_num_frms_=0）      3.有一定的匹配点但是不能太多，太多说明视觉变化比较小，不需要新关键点；  添加关键帧      单目直接添加      立体和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  当前帧与关键帧做匹配：使用bow tree  如果匹配点数大于num_matches_thr_=10，将上一帧的位姿作为当前帧的位姿的初始值，使用pose_optimizer_进行优化。  剔除当前帧中不好的lm（优化中会将一些lm设值为outliers）  

robust_match_based_track

计算当前帧和参考关键帧的匹配情况（match_frame_and_keyframe）      使用暴力匹配的方式获取匹配点（brute_force_match）      计算本质矩阵E，获取匹配点（essential_solver）  如果匹配点数大于num_matches_thr_=10，将上一帧的位姿作为当前帧的位姿的初始值，使用pose_optimizer_进行优化。  剔除当前帧中不好的lm（优化中会将一些lm设值为outliers）

在计算本质矩阵E的时候使用的是归一化平面上的点对儿，而不是图像上的像素点对儿。

motion_based_track

基于运动模型跟踪，这里的运动模型就是恒速运动模型。

首先使用恒速运动模型更新当前位姿  将最后一帧中观察到的3D点重新投影到当前帧并在帧中记录相应的lm信息（match_current_and_last_frames）      计算当前帧到上一帧的平移向量trans_lc      非单目可以判断运动方向      将最后一帧的特征点对应的3D点重新投影到当前帧，在重投影位置寻找特征点，进行匹配  pose optimization  剔除当前帧中不好的lm（优化中会将一些lm设值为outliers）

重定位

跟踪失败后会调用该函数进行重定位。
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);

重定位有关的变量

//! 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_;

获取候选关键帧（acquire_relocalization_candidates）      统计地图中所有当前帧有共享的单词的关键帧以及共享单词的数量（set_candidates_sharing_words）      将最大共享单词数量*0.8作为过滤门限      计算大于门限的关键帧与当前帧的相似性得分（DBoW内置计算方法）      设置相似分门限过滤      计算每个候选关键帧（score_keyfrm_pairs）邻域的得分并取总和，返回最高分best_total_score      将大于0.75*best_total_score的关键帧设置为候选关键帧  遍历候选帧      关键帧与当前帧进行特征点匹配（bow_matcher_.match_frame_and_keyframe）,>50个才算有效      构建pnp_solvers  使用PnP(+RANSAC)求解位姿  使用pose_optimizer优化  重投影匹配检验proj_matcher_.match_frame_and_keyframe  再次使用pose_optimizer优化  有效特征点>50，即认为重定位成功

使用局部地图对当前帧进行优化

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

通过将局部lm重投影到当前帧的方式获取更多的2D-3D点对儿（search_local_landmarks）      当前帧中的lm不需要重投影，标记对应的lm      在局部lm中逐个使用can_observe函数检测可以被观测到的lm      投影匹配当前帧和局部lm  pose_optimizer  计算跟踪到的lm数量，判断是否少于门限

can_observe  获取lm的世界座标值pos_w  判断该lm是否可以重投影到当前帧的图像平面  通过判断有效距离检查是否在orb_scale中  检测角度是否有效0.5度  预测当前lm所对应的图像金字塔层数

更新运动学模型

这里的运动学模型是恒速模型：
更新速度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. 恒速模型没搞明白；