SLAM十四讲---前端0.4----VO+局部地图

地图VO

之前计算位姿的方法是，两两帧间运用特征匹配，PnP方法计算位姿并用g2o进行单元边的3d-2d的优化。

虽说用g2o优化已经能够避免一部分噪声对RANSAC的PnP的影响。

但是，如果存在帧丢失的情况，可能距离比较远的两帧进行特征匹配和PnP求解的话可能匹配上的点比较少。因此位姿计算可能不够精确。并最终会出现追踪丢失Lost状态。
此外，全用两帧间计算的话误差累计漂移是个很严重的问题，

因此我们加入局部地图的概念（local mapping），每一帧都为VO维护的map_中提供新的地图点（携带着自身的描述子），计算位姿时通过2d关键点与3d携带描述子的地图点进行匹配，然后再进行3d-2d的PnP方法，能够在一定程度上解决VO的LOST问题。

由于地图点的坐标直接采用世界坐标，因此我们可以直接求出当前帧在世界坐标系中的位姿Tcw，而不是Tcf (或者说而不是Tij)

这样就讲得通了，为什么说位姿图中，误差项为Tij^(-1) * Ti * Tj，Ti和Tj是前端的结果，Tij是用特征点法或者直接法或者IMU或者GPS得到的。终于明白了！！！！！！！！

下面着重考虑局部地图 local mapping的VO，被称为——地图VO

这里的局部地图和slam的建图不一样，暂时还没有考虑全局地图。这里只是局部地图，简单的把一些比较近的、视野内的特征点缓存到一个地方，是一个小地图。而slam建图过程中的全局地图却储存了所有的特征点。但是直接在全局上定位，计算量很大（全局地图主要用于回环检测和地图表达）。因此在这里我们维护一个小的局部地图，从而实现通过小地图（局部地图）进行定位当前位姿。

是否使用局部地图，是出于对效率和精度这一矛盾体的仔细考量：

根据看代码，有下面几点发现：

（1）addKeyFrame，增加关键帧的时候，提供了一个功能：
因为我们让第一帧成为了关键帧，所以加入第一帧的时候，把第一帧的特征点加入到地图中，在普通VO也确实是这么做的，但是是根据setRef3DPoints这个单独的函数做的。这里是把功能集成到addKeyFrame中。

（2）Mat竟然也有尾插，这是很让人兴奋的:
desp_map.push_back( p->descriptor_ ); // 插上一行能观察到的地图点的描述子

（3）直接与地图匹配，是地图VO的特点，并且用了cv::FlannBasedMatcher matcher_flann_; 匹配器
matcher_flann_.match ( desp_map, descriptors_curr_, matches ); // flann matcher将嫩观测到的地图点与当前帧关键点进行匹配，注意，这里和普通VO是不一样的

（4）向地图中增加关键帧的机制是：当前帧的关键点一旦和地图点匹配上了，那么就无视他。没有匹配上的关键点需要映射到世界坐标系中，增加局部地图点的规模。具体看函数：void Map_VisualOdometry::addMapPoints()

（5）void Map_VisualOdometry::optimizeMap()这个函数实现了添加新点（嵌套着addMapPoints()），并且实现了控制局部地图的规模，控制手段如下：
（i）根据匹配率筛选：
匹配率 = 匹配上次数/观测到的次数
有些三维点被观测到了很多次（isInFrame），但是匹配上的次数很少，不知道原因为何，那既然匹配的少，就直接删掉吧。
（ii）根据视角删除点
angle = getViewAngle( curr_, iter->second );获得视角，这个点如果在当前帧视野中太偏，即处在边缘位置，那么就删掉。
（iii）当前帧与路标点的匹配少于100个，才加入地图，否则地图规模容易太大— 但我觉得问题不大，因为有下面这一步：
（iv）直接控制地图点规模，当地图点增加到1000个以上的时候，迭代增加门槛map_point_erase_ratio_，这样有更多匹配率比较低的点被删除，一直删到总地图点小于1000个。------这一块代码是我自己补充的。

后话

由于我一直在0.1上增删代码，没有直接用后面的0.2 0.3 0.4版本的代码，所以直接继承是一个好方法：

map_visual_odometry.h

继承部分如下。子类叫做：map_visual_odometry

#ifndef MAP_VISUALODOMETRY_H
#define MAP_VISUALODOMETRY_H

#include "visual_odometry.h"

// 在这里继承一下父类(VisualOdometry)，实现地图VO

namespace myslam
{
  
  class Map_VisualOdometry:public VisualOdometry  // 继承一下，避免大量修改原始VO代码
  {
    
public:
    typedef shared_ptr<Map_VisualOdometry> Ptr;

附上优化地图的代码：（有我自己写的一点） Map_VisualOdometry::optimizeMap()

void Map_VisualOdometry::optimizeMap()  // 加入新点，清理一波观测次数太少的、太边缘的点
{
    // remove the hardly seen and no visible points 
    for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
    {
        if ( !curr_->isInFrame(iter->second->pos_) )   // 位姿估计完毕以后，如果当前帧看不到的地图点，那么清除掉
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        float match_ratio = float(iter->second->matched_times_)/iter->second->visible_times_;  // 这个点的，匹配上次数/可观测到的次数
        if ( match_ratio < map_point_erase_ratio_ )  // map_point_erase_ratio_ = 0.1，为yaml读入
        {
            iter = map_->map_points_.erase(iter);   // 在map_中去除这个点，因为这个点匹配率太低了，可以控制局部地图的规模
            continue;
        }
        
        double angle = getViewAngle( curr_, iter->second );
        if ( angle > M_PI/6. )
        {
            iter = map_->map_points_.erase(iter);  // 太边缘的点也给擦掉
            continue;
        }
        if ( iter->second->good_ == false )
        {
            // TODO try triangulate this map point 
	    // 我觉得暂时不用——直接在上面3p-2p的g2o处同时优化位姿和对应的特征点即可
	    // 也就是用二元边，这个以后再加代码吧
        }
        iter++;
    }
    
    if ( match_2dkp_index_.size()<100 )  // 匹配上少于100个再加入地图，否则地图规模容易太大
        addMapPoints();  // 在这里加入新的地图点
    if ( map_->map_points_.size() > 1000 )  // 控制地图点规模，迭代增加门槛map_point_erase_ratio_
    {
      // TODO map is too large, remove some one
      map_point_erase_ratio_ += 0.05;
      while (map_->map_points_.size() > 1000)
      {
	Map_VisualOdometry::optimizeMap();  // 提高门槛，调用自己
      }
    }
    else 
        map_point_erase_ratio_ = 0.1;
    cout<<"map points: "<<map_->map_points_.size()<<endl;
}

至此前端完成。（普通VO + 地图VO的实现）

暂时还没有用到关键帧，仅仅是当前后两帧距离较远的时候插入关键帧。

关键帧主要是后端用的。

可以联系我获取完整的带注释的和有我自己补充的、适应各种新版本库（比如模板类sophus和新版本g2o）的代码。