slam十四讲(第二版）之最后一讲-小型的salm系统代码分析

slam十四讲之最后一讲-小型的salm系统代码分析

Vslam是slam十四讲里面，最后一讲，一个简单的slam系统
本来slam十四讲第一版是放在中间一章的，后面第二版就放在最后一章节了，在系统规模上不但包含前端的VO，也包含后端的一些优化，所以这种方式感觉应该更合理一些。不但考虑到了如何实现VO，也展示了后端优化的一些方法。
代码结构
首先代码结构，包括visualOdometry是一个主函数，他将所有的功能都集合在这个CPP文档了，所以在测试的时候，可以很简单的写一个测试函数；该项目的测试文档在APP目录下面，run_kitti_stereo.cpp里面，可以看一下，里面非常简单，初始化visualOdometry，然后Run即可；
除了这个主函数，还包括若干数据结构，主要有，camera类，frame类，feature类，mappoint类，viewer类，当然还包括config类，这个属于非重点啊类了。
项目主体分为两端，前端和后端，该项目里面，前端写的比较多，后端比较少，而且没有回环检测等功能，后面可以考虑添加上去。

具体数据结构说明

• Camera类，主要实现相机的内外参数，以及在各个坐标系的转换，如世界坐标到相机坐标，像素坐标到相机坐标等等

• Frame类，包括左帧，右帧，帧ID，是否关键帧，帧的POS，以及设置关键帧的接口

• Feature类，设置跟踪是靠左图还是右图，是不是异常点，以及关联的地图点（map_point_）关键点的2D坐标，以及该特征点关联的帧

• Mappoint类，空间三维坐标，是否异常点，他对应的观测的特征是谁？等等 Viewer类，把这些画出来

• Config类，配置一些数据，比如data的目录啊，相机的内参啊，之类的；

逻辑结构
从frontend开始

• 从data中读取一帧之后，调用AddFrame接口，将该帧加入系统。
  若该帧为第一帧，根据frontendstatus的初始化值，他需要进行stereoinIt进行初始化；
  若该帧不是第一帧，则根据跟踪的状态好坏，调用track进行跟踪；若状态是lost，则说明跟踪丢失，需要重新初始化reset；
  看一下Addframe做了些什么？他首先把加入的帧作为当前帧用以循环，如果第一帧的话，使用stereinit进行初始化
  Stereinit中，detectfreatures首先检测特征，检测的时候，不需要整张图片，把边上10个像素不用，得到的特征，存入该帧的左特征容器里面;
  FindFeaturesInRight，在右帧中也找特征点，不过，该特征点是通过三维空间点坐标和右边摄像头位姿计算出来的（世界坐标到像素坐标）；但是第一帧的时候，这个世界坐标是没有的，咋办？只需要把左边摄像头的二维像素坐标直接赋值给右边摄像头就好了，做个初值而已；有前后两张图片，有前后两个关键点坐标，使用光流法计算，获取实际跟踪到的点。如果得到点的数量达不到初始要求的点数，那么初始化失败。
  接下来，初始化map，成功的话，将viewer加入 AddCurrentFrame，并触发viewer进行updatemap动作，另外，设置跟踪状态status_ = FrontendStatus::TRACKING_GOOD;
  以上过程比较重要的是初始化map，BuildInitMap，这个函数实现了根据左右摄像头的关键点，位姿，等数据，进行三角测量，获取关键点的Z值的功能。然后将该帧加入关键帧。
  到这里，可以考虑一下，我们都做了哪些工作？
  左右相机的第一帧的特征点以及这些特征点所对应的三维空间点坐标我们都已经知道了；全部初始化已经完全结束了

• 当再来一帧的时候，处理如下：
  跟踪的状态有初始值了，TRACKING_GOOD嘛
  那我下面该进行track了
  bool Frontend::Track()做的事情，首先根据上一次的当前帧和上一帧的转换矩阵relative_motion_，求目前帧的初始化位姿（relative_motion_这个数据其实是根据上一个循环算出来的，这里用，也只是近似）。
  TrackLastFrame，根据上一帧的关键点和三角测量得到的Z值，还有估计得目前帧的初始化位姿，我们可得到三维空间点在目前帧上的二维像素值，那这些像素值，又可以作为光流法跟踪的初值值；然后就是继续光流法跟踪了；最后得到跟踪匹配上的点以及点的坐标；
  EstimateCurrentPose，有初始化位姿，有跟踪匹配上的点，我们可以对目前帧进行更高精度的姿态估计；在这个计算过程中，需要排除一部分异常点，这些异常点的feat的map_point_就置为空了（也许上一帧三角测量出来这个特征点有空间坐标，这个时候也置空了）；
  事实上，我们评价跟踪状态是好还是坏，其实依赖的就是这一步的正常特征点的数目，如果这个数据大于一个阈值（项目用的是50），那么就是一个好的跟踪，若大于20，小于50，则是一个bad的跟踪，小于20的话，我们认为跟踪失败；
  接下来，InsertKeyframe就登场了，这个函数实现的功能非常多，可以看看；
  首先是一个判断，该不该插入这个帧呢？判断依据是，如果上述跟踪点非常大，大于某个阈值了（项目给定的是80），那这个帧和上一帧的相似度太高了，肯定不能作为关键帧，就直接返回了；如果跟踪点规模适当，则设置当前帧为关键帧，并更新map类里面的关键帧数据。既然将其设置了关键帧了，还要更新对应三维空间点所对应的特征点，将这些特征点设置为观察者进行关联；
  接下来，三个函数，DetectFeatures，FindFeaturesInRight，TriangulateNewPoints这三个函数，这几个函数的用途是继续检测左图像的特征点，（之前有TrackLastFrame函数获取到，现在采用giff再检测，累计），再找右图特征点（左图对应空间点，右图pose，计算二维像素坐标做初值），再进行光流法计算跟踪点数据；跟踪点数据有啥用呢？我们获取了左图像特征以及由图像对应特征，是不是可以三角测量了呢？对，TriangulateNewPoints就是对新的点进行三角测量；怎么选择做三角测量的点呢？current_frame_->features_left_[i]->map_point_.expired() 意思是，左图特征点没有对应有三维空间点， current_frame_->features_right_[i] != nullptr意思是左图特征点，对应的有右图特征点，那这些点都是需要进行三角测量的点；然后计算空间点坐标即可；
  这些都做完了，接下来该通知后端进行更新地图了，更新优化采用的是第9章所讲的，同时优化pose和landmark的原理。若关注点不同，其实也可以只优化位姿。需要说明的是，优化全局位姿和路标点的时候，要注意将一些变量加锁，否则可能会有问题。
  最后再通知一下viewer也更新一下map吧