《视觉SLAM十四讲精品总结》10.4: 添加局部地图

0.3版本的VO仍受两帧间匹配的局限性影响，一旦视频序列的某帧丢失，导致后面匹配失败。

将VO匹配到的特征点放到地图中，并将当前帧与地图点进行匹配，计算和地图之间位姿。每一帧为地图贡献一些信息，比如添加新的特征点或更新旧特征点的位置估计。地图中的特征点一般都是使用世界坐标，所以当前帧到来时，匹配它和地图之间运动关系可以直接得到Tcw。

这样做的好处就是，维护一个不断更新的地图，只要地图正确，即使中间某帧出了问题，不至于崩溃。 
另外地图分为局部和全局。这里我们实时定位的需要的是局部地图，全局地图一般用于回环检测和整体建图。

一、地图点Mappoint

头文件.h

地图点本质上就是空间中3D点，成员函数为地图点的基本信息。

有一个Frame*类型的 list，用于记录观察到此地图点的帧。list<Frame*>    observed_frames_; 

还有个取得地图点3维坐标的功能函数getPositionCV()直接写成内联了。还有个地图点生成函数createMapPoint()，传入成员变量所需要的信息就好了。

#ifndef MAPPOINT_H
#define MAPPOINT_H

#include "myslam/common_include.h"
namespace myslam
{   
class Frame;
class MapPoint
{
public:
	//1. 变量：ID、出厂ID、好(坏)点、3D坐标、正常视角、描述子
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long      id_;    
    static unsigned long factory_id_;   
    bool        good_;    
    Vector3d    pos_;     
    Vector3d    norm_;     
    Mat         descriptor_; 
    // 记录观察到此地图点的帧
    list<Frame*>    observed_frames_; 
    // 内点和当前帧可观测
    int         matched_times_;     // being an inliner in pose estimation
    int         visible_times_;     // being visible in current frame 
    //2.构造函数
    MapPoint();
    MapPoint( unsigned long id, const Vector3d& position,const Vector3d& norm,Frame* frame=nullptr,const Mat& descriptor=Mat() );
    //3.内联成员函数，取得地图点3D坐标
    inline cv::Point3f getPositionCV() const{return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) );}
    //4.地图点生成函数，参数（3D坐标，正常视角、描述子、关键帧）
    static MapPoint::Ptr createMapPoint();
    static MapPoint::Ptr createMapPoint( const Vector3d& pos_world,const Vector3d& norm_,const Mat& descriptor,Frame* frame );
};
}
#endif // MAPPOINT_H

.cpp

#include "myslam/common_include.h"
#include "myslam/mappoint.h"

namespace myslam
{
//1. 构造函数，参数(ID、3D坐标、正常视角、好(坏)点、内点、当前帧可观测)
MapPoint::MapPoint(): id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), good_(true), visible_times_(0), matched_times_(0){}
MapPoint::MapPoint ( long unsigned int id, const Vector3d& position, const Vector3d& norm, Frame* frame, const Mat& descriptor ): id_(id), pos_(position), norm_(norm), good_(true), visible_times_(1), matched_times_(1), descriptor_(descriptor)
{
    observed_frames_.pus