• 第四篇 跟踪过程以及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})

  • 相关阅读:
    c语言结构体数组引用
    c语言结构体数组定义的三种方式
    如何为SAP WebIDE开发扩展(Extension),并部署到SAP云平台上
    SAP SRM ABAP Webdynpro和CFCA usb key集成的一个原型开发
    使用SAP API portal进行SAP SuccessFactors的API测试
    SAP UI5应用里的页面路由处理
    在SAP WebIDE Database Explorer里操作hdi实例
    如何使用SAP事务码SAT进行UI应用的性能分析
    使用SAP WebIDE进行SAP Cloud Platform Business Application开发
    SAP CRM WebClient UI ON_NEW_FOCUS的用途
  • 原文地址:https://www.cnblogs.com/hardjet/p/11512657.html
Copyright © 2020-2023  润新知