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

数据类型和结构

程序运行过程

一、顺序执行过程

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_)比较，以此决定状态status为TRACKING_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)，此时需要将这些路标点从激活路标点中去除，使其不再参与优化。