SLAM14讲 ch13 双目里程计代码结构

最新推荐文章于 2023-10-04 17:58:41 发布

原创最新推荐文章于 2023-10-04 17:58:41 发布 · 430 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#slam

slam 专栏收录该内容

12 篇文章

订阅专栏

本文详细介绍了SLAM系统中双目里程计的代码结构和工作流程。从初始化开始，通过VisualOdometry类加载数据集，接着前端利用光流法跟踪特征点并估计位姿，当满足一定条件时插入关键帧。后端则对激活的关键帧和地图点进行优化，控制优化规模并剔除旧关键帧。整个过程涉及特征匹配、三角化、地图更新和优化等多个核心步骤。

SLAM14讲 ch13 双目里程计代码结构

头文件	功能
Algorithm.h	SVD的三角化
backend.h	后端优化，map更新时进行优化
config.h	set配置文件，获得配置数据
dataset.h	数据集读取dataset_dir
feature.h	获得2D特征点，并关联地图点
frame.h	分配ID，建立关键帧
frontend.h	前端，估计当前帧位姿，并想后端加入满足条件的关键帧进行优化
map.h	地图，前端插入帧和地图点，后端优化剔除
mappoint.h	获得三角化之后的地图点
viewer.h	可视化

大概流程：

【初始化】调用VisualOdometry类，Init()初始化，读取dataset_dir中的数据放到dataset_里。创建每一部分的智能指针以及Set各部分之间的联系。调用Run()启动VO，通过step()函数（dataset_里的NextFrame()），创建并返回下一帧的图像，调用frontend中的AddFrame() 加入新的一帧。
【前端】获得num_features_init_和num_features_,调用AddFrame()添加一个帧(向后迭代)，判断前端状态(Initing,tracking_good,tracking_bad,lost)
（若为Initing状态，调用StereoInit(),检测左目特征，并获得对应的右目中的特征，若对应特征数目小于num_features_init_,初始化失败，对下一帧仍然进行初始化，直到满足条件）。若初始帧特征点足够建立初始地图，前端状态转变为tracking_good,并向viewer中添加并更新 current_frame_。）
（若为tracking_good || tracking_bad状态，运行track()，假设相机恒速运动，帧之间的相对运动不变，使用光流法TrackLastFrame(),获得匹配特征点并返回特征点数，估计当前帧的位姿EstimateCurrentPose()。若inliers大于num_features_tracking_,状态good;若inliers大于num_features_tracking_bad,状态bad;若追踪到的特征点太少，lost, reset()。将当前帧加入viewer中）
InsertKeyframe()中,当inliers大于等于num_features_need_for_keyframe_（80），说明当前帧中还有足够的inliers特征，与之前的关键帧还很相似。当小于阈值时，将当前帧设为关键帧，并加入map中（InsertKeyFrame()），输出当前帧ID和关键帧ID。检测当前帧左目图像中新的特征点，对于未关联地图点的新特征点，在右目图像中追踪，三角化新的地图点。后端运行（更新了关键帧）。viewer更新)
【后端】backendloop()仅优化激活的Keyframe和landmark。optimize()使用g2o对给定关键帧和路标点进行优化。Map中num_ActiveKeyFrame=7,控制优化的规模。若激活关键帧大于7，则丢弃最早的一个关键帧。