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


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

    创建地图

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

    // 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. 恒速模型没搞明白
      (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_);
    
    

    对应程序,velocity_就是(T_W^{C_{k-1}}*(T_{C_{k-2}}^W)^{-1})

  • 相关阅读:
    springMVC3学习(二)--ModelAndView对象
    springMVC3学习(一)--框架搭建
    JS作用域
    JS阻止事件冒泡
    Oracle常用函数
    Oracle中复制表结构和表数据
    转:JavaBean 、 Serverlet 总结
    form插件ajaxForm和ajaxSubmit方法传递对象参数说明
    http status 汇总
    浅谈HTTP中Get与Post的区别
  • 原文地址:https://www.cnblogs.com/hardjet/p/11512657.html
Copyright © 2020-2023  润新知