《SLAM十四讲第二版》CH13项目学习笔记

目录

  • 数据类型和结构
  • 程序运行过程
    • 一、顺序执行过程
      • 1.初始化过程(第一帧)。
      • 2.第二次进入循环(第二帧)
    • 二、三角化算法与SVD求解
    • 三、Map::RemoveOldKeyframe()与Map::CleanMap()

数据类型和结构

《SLAM十四讲第二版》CH13项目学习笔记_第1张图片
《SLAM十四讲第二版》CH13项目学习笔记_第2张图片
《SLAM十四讲第二版》CH13项目学习笔记_第3张图片

程序运行过程

一、顺序执行过程

1.初始化过程(第一帧)。

assert(vo->Init() == true);

Init()函数位于visual_odometry.cpp文件中。

  1. 首先传递了配置文件路径(config_file_path_),后来可以发现在配置文件中可以找到数据集文件(dataset)存放的路径。
  2. 然后对各个运行结构初始化,对于前端(主线程)初始化时生成frontend_指针,猜测初始化时相机帧姿态位置与世界坐标系重合。后端初始化时生成backend_指针,并且通过Backendloop函数创建线程并启动,但随后便进入map_update_.wait(lock),被阻塞地图初始化时生成Map_指针。
 vo->Run();

进入run()函数后,总体上进入了如下流程

读数据集
根据数据集建立新的帧
前端局部优化
后端优化
  1. 首先进入step()函数,读取数据集(NextFrame)并在前端新建帧(frontend_->AddFrame(new_frame))。
  2. 进入AddFrame()函数,此时前端状态为FrontendStatus::INITING
  3. 进入StereoInit()函数,首先通过DetectFeatures()检测左图特征点并将特征集添加入该帧(返回左图特征点个数),然后通过FindFeaturesInRight()检测能与左特征点配对的右侧特征点并将特征集添加入该帧(返回匹配成功的特征点个数)。若配对数量<初始化要求最低特征点,则返回false。此时由于状态status未改变仍为FrontendStatus::INITING,下一帧读入时将仍进入StereoInit()函数。
  4. 若配对数量足够,进入BuildInitMap()函数,通过左右配对特征点,三角化还原观测到的路标的三维坐标。(生成各个路标点后,要用AddObservation()添加该路标的观测特征点,同时要在相应的特征点中要添加指向该路标的指针)。
  5. 通过InsertMapPoint()将全部的路标点加入到地图中,并将此帧设为关键帧(因为是第一个可以提取有效数量路标点的帧),并插入该关键帧于Map_。
  6. backend_->UpdateMap()触发后端地图更新,通过map_update_.notify_one()唤醒被阻塞的后端进程,优化位姿和路标点(但是只有第一帧时的优化结果不知道是什么样子)。退出BuildInitMap()函数后,将状态status改为FrontendStatus::TRACKING_GOOD
  7. StereoInit()函数结束,last_frame_ = current_frame_;把当前帧传递给上一帧。
  8. 进入步骤1循环。

2.第二次进入循环(第二帧)

  1. 首先进入step()函数,读取数据集(NextFrame)并在前端新建帧(frontend_->AddFrame(new_frame))。(与初始化过程无异)
  2. switch (status_),(这里无论是bad或是good都会进入Track()函数)。
  3. Track()函数:若上一帧存在,则基于恒速假设(即假设A->B,B->C间的位姿变换相同),将当前帧的位姿T3估计为T2=T21·T1(已知),T3=T21·T2。(但此时relative_motion_为初始默认值,可能为单位矩阵)。
  4. 进入TrackLastFrame()函数,使用上一帧与当前帧的左图进行光流追踪。先预估特征点在当前帧左图出现的位置(光流追踪可能以这个为参考),使用上一帧左图的特征点集[i],若上一帧存在对应的路标点,则用该路标点与当前帧的预计位姿得到该帧左图的特征点位置;若不存在对应路标点(即上一帧的该特征点的路标点三角化失败),则直接使用上一帧特征点位置作为该帧特征点位置。这样得到两组特征点集(kps_last、kps_current),对其光流追踪,得到当前帧追踪后的特征点集(与上一帧匹配成功的),并对匹配的特征点根据上一帧添加路标点指向(注意这里只进行了特征点->路标点的指向)。
  5. 进入EstimateCurrentPose()函数,通过当前帧预估位姿,左图特征点集,提取的路标点建立局部优化,仅对当前帧的位姿进行优化,并将优化后的位姿作为当前帧的位姿。最后将计算误差较大对应的特征点与其指向的路标点解除关系,但并未删除该特征点。(该函数返回值为匹配特征点-异常特征点=误差范围内的有效特征点
  6. 根据有效特征点tracking_inliers的数量与阈值(num_features_tracking_num_features_tracking_bad_)比较,以此决定状态statusTRACKING_GOOD还是TRACKING_BAD还是LOST丢失。随后退出Track()函数。
  7. 进入 InsertKeyframe()函数,在这个函数内部根据有效匹配
    特征点数量判断是否将此帧设为关键帧。
    7.1 若设为关键帧,则为该帧分配关键帧ID,在地图中插入该帧,并为该帧已有特征点与路标添加双指向关联。随后进入DetectFeatures(),通过mask掩模遮盖以有的特征点,以检测新的特征点。(事实上与上一帧追踪到的特征点少,可能是因为含有的新特征->新路标点较多) 然后通过FindFeaturesInRight()函数在右图中找到对应于左图的特征集,对于左图未关联路标的特征点,且在右图能寻到相匹配的特征点,尝试三角化得到对应的路标,为新路标与特征点添上双指向关联,并在地图中添加该路标,随后触发地图更新。
    7.2 若不设为关键帧,则退出InsertKeyframe()函数。
  8. 计算相对位姿变换,得到relative_motion_为下次恒速假设做准备。

二、三角化算法与SVD求解

文件为algorithm.h
算法的数学原理参考该文章 三角测量(Triangulation 三角化)与 SVD 求解
需要注意的是,在程序中triangulation()函数得到的实际上是双目视角下的坐标,需要左乘Twc后得到世界坐标系下的路标点。即Yw=Twc·Yc,

SE3 current_pose_Twc = current_frame_->Pose().inverse();

三、Map::RemoveOldKeyframe()与Map::CleanMap()

根据之前的分析可知,对于提取新路标点做出贡献的仅有关键帧,非关键帧只优化了位姿,其特征点也仅关联了旧的路标点。后端优化时仅优化Map中激活的关键帧和激活的路标点。路标点的观测次数仅由关键帧中的特征点贡献。

  1. Map::RemoveOldKeyframe(): Map新增关键帧后,同时会将该帧加入激活关键帧中,如果激活关键帧总数量多于阈值,则需要剔除旧的关键帧,方法是寻找与当前帧最近与最远的两个关键帧(使用相对位姿变换的范数比较),如果存在很近的帧,优先删掉最近的;否则删掉最远的。激活关键帧的变化会引起激活的路标点的被观测次数的变化,因此通过移除的关键帧的左、右侧特征点寻找其关联的路标点,并减少路标点的观测次数(即激活的路标点与要剔除的关键帧之间的联系)。
  2. Map::CleanMap():减少某些激活的路标点的观测次数后,会出现某些激活的特征点未被任何激活的关键帧观测到的情况(即路标点被观测次数为0),此时需要将这些路标点从激活路标点中去除,使其不再参与优化。

你可能感兴趣的:(计算机视觉)