SLAM十四讲：搭建VO框架(9)

内容总览：VO框架的实现，对书上的代码进行理解，写了一部分注释，很久以前看的了，如果有需要再回头再捋一遍，解决解决当时疑惑的点

程序框架/数据结构

库/目录结构

bin: 存放可执行的二进制
include/myslam: 存放slam头文件.h
src: 存放源代码文件，主要是.cpp
test: 存放测试文件，也是.cpp
lib: 存放编译好的库文件
config: 存放配置文件
cmake_modules: 第三方库的cmake文件，在使用如ceres等库时使用

这里都是自己写的头文件，不容易和别的库混淆

基本数据结构

帧：好理解
路标：特征点，可以放到地图中进行匹配来估计位姿
配置文件：一些常用的不变的量，如内参，比例等
坐标变换：各种坐标之间转换直接定义一个大类

Camera类

头文件:

//单孔RGBDmodel
  class Camera
  {
  public:
    typedef std::shared_ptr<Camera> Ptr;    //定义了一个智能指针类型，可用于传递参数
    float fx_, fy_, cx_, cy_, depth_scale_;        //内参
    
    Camera();
    Camera( float fx, float fy, float cx, float cy, float depth_scale=0):
    fx_( fx ), fy_( fy ), cx_( cx ), cy_( cy ), depth_scale_( depth_scale )
    {}
    //坐标变换 ,2 表示two = to，SE3来表达相机位姿
    Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w );
    Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w );
    Vector2d camera2pixel( const Vector3d& p_c );
    Vector3d pixel2camera( const Vector2d& p_p, double depth = 1 );
    Vector3d pixel2world( const Vector2d& p_p, const SE3& T_c_w, double depth = 1 );
    Vector2d world2pixel( const Vector3d& p_w,const SE3& T_c_w );
  };

功能理解：
1.储存相机的内参和外参，坐标转换，外参用参数的形式输入。
2.先定义了一个智能指针，然后是内参，构造函数初始化深度为0
3.坐标变换都用的李代数
4.类都被命名空间namespace myslam包裹，可以防止不小心定义出别的库的同名函数

实现方法.cpp：

namespace myslam    //这东西不套上，代码补全出不来
{
  Camera::Camera()
  {
  }
  Vector3d Camera::world2camera( const Vector3d & p_w, const SE3& T_c_w )
  {
    return T_c_w * p_w;
  }

  Vector3d Camera::camera2world( const Vector3d& p_c, const SE3& T_c_w )
  {
    return T_c_w.inverse() * p_c;
  }
  
  Vector2d Camera::camera2pixel( const Vector3d& p_c )
  {
    return Vector2d(
      fx_ * p_c( 0, 0 ) / p_c( 2, 0 ) + cx_ ,
            fy * p_c( 1, 0 ) / p_c( 2, 0 ) + cy_
       );
  }
  
  Vector2d Camera::pixel2camera( const Vector2d&p_p, double depth)
  {
    return Vector3d(
      ( p_p( 0, 0 ) - cx_ ) * depth/fx_,
            ( p_p( 1, 0 ) - cy_ ) * depth/fy_,
            depth
       );
  }
  
   Vector2d Camera::world2pixel( const Vector3d&p_w, const SE3&T_c_w)
   {
     return camera2pixel( world2camera( p_w, T_c_w ) );
     //利用了两个变换一起
   }
   
    Vector3d Camera::pixel2world( const Vector2d&p_p, const SE3&T_c_w, double depth)
    {
      return camera2world( pixel2camera( p_p, depth ), T_c_w );
    }
}

就是些坐标变化的实现

Frame类

头文件：

class Frame
{
public:
  typedef std::shared_ptr<Frame> Ptr;
  unsigned long id_;    //这一帧的编号
  double time_stamp_;    //被记录的时间，用于后期的匹配,时间戳
  SE3 T_c_w_;    //变换矩阵
  Camera::Ptr camera;        //针孔摄像机模型
  Mat color_, depth_;    //图像的颜色和深度矩阵
  
public:    //数据成员
  Frame() ;
  Frame( long id, double time_stamp=0, SE3 T_c_w = SE3(), Camera::Ptr camera=nullptr,
     Mat color=Mat(), Mat depth=Mat() );
  ~Frame() ;
  
  //制造Frame函数
  static Frame::Ptr createFrame();
  
  //寻找给定关键点的深度
  double findDepth( const cv::KeyPoint& kp );
  
  //获取摄像机中心
  Vector3d getCamCenter() const;
  
  //检查点在不在这帧上
  bool isInFrame( const Vector3d& pt_world );
};
  
//路标点类，用当前帧提取到的特征点和地图中的特征点匹配，来估计相机运动，需要存储描述子
//记录并存储点被观测和匹配的次数，作为评价好坏的指标
class MapPoint
{
public:
  typedef shared_ptr<MapPoint> Ptr;
  unsigned long_id;    //ID
  Vector3d pos_;   //世界位置
  Vector3d norm_;    //视野的范数的方向
  Mat descriptor_;        //匹配的描述子
  int observed_times_;    //匹配次数？？？
int correct_times_;     //估计次数？？？
MapPoint();
MapPoint( long id, Vector3d position, Vector3d norm );
//建造函数
static MapPoint::Ptr createMapPoint();
};

帧的自我属性：
编号，时间戳，摄像机模型，变换矩阵，图像的颜色和深度
功能：
1.创建以一个Frame，返回带编号的Frame指针
2.获得摄像机中心（T的转置的位移向量）
3.寻找给定关键点的深度（深度图已有）
4.检测某点是否在该帧上（根据摄像机坐标和像素坐标判断）

MapPoint类

class MapPoint
{
public:
  typedef shared_ptr<MapPoint> Ptr;
  unsigned long_id;    //ID
  Vector3d pos_;   //世界位置
  Vector3d norm_;    //视野的范数的方向
  Mat descriptor_;        //匹配的描述子
  int observed_times_;    //观测次数
int correct_times_;     //匹配次数
MapPoint();
MapPoint( long id, Vector3d position, Vector3d norm );
//建造函数，都是空的，就是带的编号
static MapPoint::Ptr createMapPoint();
};

路标点类，会估计点的世界坐标，用当前帧提取到的特征点和地图中的特征点匹配，来估计相机运动，需要存储描述子。
记录并存储点被观测和匹配的次数，作为评价好坏的指标

Map类

class Map  
{
public:
  typedef shared_ptr<Map> Ptr;
  //需要随即访问，还需要随时插入和删除
  unordered_map<unsigned long, MapPoint::Ptr > map_points_;    //全部路标
  unordered_map<unsigned long, Frame::Ptr > keyframes_;    //全部关键帧
  
  Map() {}
  
  void insertKeyFrame( Frame::Ptr frame );
  void insertMapPoint( MapPoint::Ptr map_point );
};

功能：管理所有路标点，添加、删除等操作

Config类

头文件:

class Config        
{
private:
  static std::shared_ptr<Config> config_;
  cv::FileStorage file_;    //可以读取YAML文件，存放各种参数，file_[key] 调用
  
  Config () {}    //写成单件模式，构造函数为私有
public:
  ~Config();    //析构函数，销毁时关闭函数
  
  //设置一个新配置文件,创建对象并读取参数文件，析构函数也在其中
  static void setParameterFile( const std::string& filename );
  
  //载入参数值,用的是模板函数，为的是可以获取任意类型的参数值
  template< typename T >
  static T get( const std::string& key )
  {
    return T( Config::config_->file_[key] )l
  }
};

单件模式singleton，在程序任意地方都可以随时提供参数值
功能：设置、读取参数文件，因为有多种类型，所以用模板函数

实现函数：

void Config::setParameterFile( const std::string& filename )
{
  if ( config_ == nullptr )
    config_ = shared_ptr<Config>( new Config );    //没有指针就定义一个
  config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ );    //给智能指针的对象指向赋值
  if ( config_->file_.isOpened() == false )
  {
    //如果文件false，说明文件不存在//是不是计数器起效?
    std::cerr<<" parameter file "<<filename<<" does not exise."<<std::end;
    config_->file_.release();
    return ;
  }
}

Config::~Config()    //析构函数，关闭文件
{
  if ( file.isOpened() )
    file_.release();
}

shared_ptr<Config> Config::config_ = nullptr;

我们使用智能指针，优点是可以自动析构
读取的文件储存在FileStorage类中
使用范例：
1.在参数文件加入：“Camera.fx:500”。
2. myslam::Config::setParameterFile(“parameter.yaml”);
3. double fx=myslam::Config::get(“Camera.fx”);

基本的VO，特征提取和匹配

两两帧

设定参考帧和当前帧，以参考帧为坐标系，把当前帧和参考帧进行特征匹配，并估计运动关系

思路

/* 思路：每次两个帧比较，一个参考帧（上一帧），一个当前帧，二者进行特征匹配（以参考帧为坐标系），
* 并估计运动关系，得 T_c_w = T_c_r * T_r_w
* 当前相对世界坐标系的变化 = 当前相对参考的坐标变换 * 参考相对世界坐标系的变换
* 求VO流程：
* 1.对新来的当前帧，提取关键点和描述子
* 2.如果系统没初始化，就把当前帧当作参考帧，返回1
* 3.估计参考帧和当前帧的运动
* 4.如果成功，就把当前帧设为参考帧，返回1
* 5.如果不成功，计算连续不成功次数，超过某个次数就置VO为丢失状态，
* 算法退出。没超过该次数，就返回1
*/```
## 算法

```cpp
class VisualOdometry
  {
  public:
  typedef shared_ptr<VisualOdometry> Ptr;
  enum VOState {        //状态，是否初始化，是否丢失
    INITIALIZING=-1,
    OK = 0,
    LOST
  };
  
  VOState state_;    //目前的VO状态，initialize or ok or lost
  Map::Ptr map_;    //装有全部的帧和特征点？？
  Frame::Ptr ref_:    //参考帧
  Frame::Ptr curr_;        //当前帧
  
  cv::Ptr<cv::ORB> orb_;        //orb检测和计算方式
  vector<cv::Point3f> pts_3d_ref_;    //参考帧中的3d点
  vector<cv::KeyPoint> keypoints_curr_;    //现在帧的特征点
  Mat descriptors_curr;        
  Mat descriptors_ref;
  vector<cv::DMatch> feature_matched_;
  
  SE3 T_c_r_estimated_;    //参考到现在的变换矩阵估计
  int num_inliers_;     //内点
  int num_lost_;
  
  //parameters
  int  num_of_features_;
  double scale_factor_;
  int level_pyramid_;    //number of pyramid levels
  float match_ratio_;    //ratio for selecting good matches
  int max_num_lost_;
  int min_inliers_;
  
  double key_frame_min_rot;    // minimal rotation of tow key-frames
  double key_frame_min_trans;
  
  public:
    VisualOdometry();
    ~VisualOdometry();
    
    bool addFrame( Frame::Ptr frame );
    
  protected:
    //inner operation
    void extractKeyPoints();
    void computeDescriptors();
    void featureMatching();
    void poseEstimationPnP();
    void setRef3DPoints();
    
    void addKeyFrame();
    bool checkEstimatedPose();
    bool checkKeyFrame();
  };

参数：
全部的帧和特征点，估计变换矩阵，变换最小程度限制
功能：
1.增加帧
2.提取特征点
3.计算描述子
4.特征匹配
5.pnp估计位姿
6.设置参考3d点
7.检验估计点
3.检验关键帧

bool myslam::VisualOdometry::addFrame(Frame::Ptr frame)
{
  switch( state_ )
  {
    case INITALIZING:
    {
      state_ = OK;
      curr_ = ref_ = frame;    //第一个
      map_->insertKeyFrame( frame );
      //from first frame,ref and curr
      extractKeyPoints();
      computeDescriptors();
      setRef3DPoints();
      break;
    }
    case OK:
    {    //直接用类的参数，真方便
      curr_ = frame;
      extractKeyPoints();
      computeDescriptors();
      featureMatching();
      poseEstimationPnP();
      //checkEstimatedPose中有内点数量和移动最大距离限制
      if ( checkEstimatedPose() == true )     //a good    //poseEstimationPnP
      {
    curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;
    ref_ = curr_;
    setRef3DPoints();
    num_lost = 0;        //set 0
    if ( checkKeyFrame() == true )
    {
      addKeyFrame();
    }
      }
      else
      {
    num_lost_++ ;        //以每组（ref and curr）为单位
    if ( num_lost_ > max_num_lost_ )
    {
      state_ = LOST;
    }
    return false;
      }
      break;
    }
    case LOST:
    {
      cout<<"vo has lost."<<endl;
      break;
    }
  }
  return true;

}

新增Frame，内可判断是否lost,内置各种函数，如获取特征点，计算描述子等，算完直接加入到新Frame属性中
可估计位姿，自带检验算法，检验内点数量和最大移动距离限制判断是一个好的估计位姿
如果是个关键帧，就加入到关键帧中
根据丢失帧数量判断是否丢帧

运行

if ( argc != 2 )
  {
    cout<<"usage: run_vo parameter_file"<<endl;
    return 1;
  }
  myslam::Config::setParameteFile( argv[1] );
  myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );
  
  string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
  cout<<"dataset: "<<dataset_dir<<endl;
  ifstream fin (dataset_dir + "/associate.txt" );
  if ( !fin )
  {
     cout<<"please generate the associate file called associate.txt!"<<endl;
     return 1;
  }
  
  vector<string> rgb_files, depth_files;
  vector<double> rgb_times, depth_times;
  while( !fin.eof() )    //读取各种数据
  {
    string rgb_time, rgb_file, depth_time, depth_file;
    fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
    rgb_times.push_back( atof( rgb_time.c_str() ) );
    depth_times.push_back( atof( depth_time.c_str() ) );
    rgb_files.push_back( dataset_dir + "/" + rgb_file );
    depth_files.push_back( dataset_dir + "/" + depth_file );
    
    if ( fin.good() == false )
      break;
  }
  

1.准备工作，读取各种数据

  myslam::Camera::Ptr camera( new myslam::Camera );
  
  //visualization,初始化并设置各种关于世界坐标系和摄像机坐标系的信息
  cv::viz::Viz3d vis("Visual Odometry");

  cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);    //坐标系设置,此处可能是比例

  cv::Point3d cam_pos( 0, -1.0, -1.0 ), camera_focal_point(0,0,0), cam_y_dir(0,1,0);    //一些重要的点的位置
  cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir );    //what？？？
  vis.setViewerPose( cam_pose );        //视野设置？？？，应该就是这相机前面视野
  
  //渲染和显示？？？
  world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);        //渲染属性
  camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH,1.0);
  vis.showWidget("World", world_coor);
  vis.showWidget("Camera", camera_coor);
  
  //读取帧信息,并添加到vo里计算
  cout<<"real total"<<rgb_files.size() <<" entried" <<endl;
  for( int i=0; i<rgb_files.size(); i++)
  {
    Mat color = cv::imread( rgb_files[i] );
    Mat depth = cv:imread( depth_files[i], -1);
    if ( color.data == nullptr || depth.data==nullptr )
      break;
    myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
    pFrame->camera_ = camera;
    pFrame->color_ = color;
    pFrame->depth_ = depth;
    pFrame->time_stamp_ = rgb_times[i];
    
    boost::timer timer;
    vo->addFrame( pFrame );
    cout<<"VO cost time: "<<timer.elapsed()<<endl;
    if ( vo->state_ == myslam::VisualOdometry::LOST )
      break;
    SE3 Tcw = pFrame->T_c_w.inverse();
    
    //show th map and the camera cam_pose
    //M构造分两部分
    cv::Affine3d M(
      cv::Affine3d::Mat3(
                    Tcw.rotation_matrix()(0,0),Tcw.rotation_matrix()(0,1),Tcw.rotation_matrix()(0,2),
            Tcw.rotation_matrix()(1,0),Tcw.rotation_matrix()(1,1),Tcw.rotation_matrix()(1,2),
            Tcw.rotation_matrix()(2,0),Tcw.rotation_matrix()(2,1),Tcw.rotation_matrix()(2,2)
    ),
    cv::Affine3d::Vec3(
       Tcw.translation()(0,0),Tcw.translation()(1,0),Tcw.translation()(2,0)
              )
    );
    cv::imshow("image", color);
    cv::waitKey(1);
    vis.setWidgetPose("Camera", M);
    vis.spinOnce(1, false);
  }
  

2.初始化视觉里程计，相机的内参，映射，视野设置和相机映射相同
3.渲染属性
4.读取帧的信息，放入VO中计算
（1）创建帧，给帧输入图片的各种信息
（2）将创建的帧的属性放入到addFrame函数中计算
5.得出矩阵M

存在的问题

/*效果并不好
* 1.只用RANSAC容易受噪声影响        solve：用RANSAC作初始值，再用非线性优化求最优值
* 2.特征点多是边缘，所以特征点的深度不准确        solve：将特征点也优化
* 3.帧与帧的比较太依赖帧，不好的帧甚至会导致漂移        solve：让当前帧与地图匹配
* 4.特征点的提取和匹配耗时太多    solve：直接法，光流法```


# 优化PnP的结果
## 方法
RANSCA提供初始值，pnp求相机位姿，g2o优化（最小化冲投影误差）相机位姿
## g2o
**头文件**：

```cpp
  class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    virtual void computeError();
    virtual void linearizeOplus();
    
    virtual bool read( std::istream& in ) {}
    virtual bool write(std::ostream& os) const {};
    
    Vector3d point_;    //方便计算重投影误差和呀可比
    Camera* camera_;
  };```

  **实现函数**：
  

```cpp
void EdgeProjectXYZ2UVPoseOnly::computeError()
{
  const g2o::VertexSE3Expmap * pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
  _error = _measure - camera->camera2pixel(
    pose->estimate().map(point_)        //重投影误差
                  );
}
  
  
void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()        
//jacobin，优化一个位姿。是一个一元边
{
  g2o::VertexSE3Expmap * pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );

  g2o::SE3Quat T ( pose->estimate() );

  Vector3d xyz_trans = T.map( point_ );    //映射点
  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double z = xyz_trans[2];
  double z_2 = z*z;
   _jacobianOplusXi(0,0)=x*y/z_2*camera_->fx_;
  _jacobianOplusXi(0,1)=-(1+(x*x/z_2))*camera_->fx_;
  _jacobianOplusXi(0,2)=y/z*camera_->fx_;
  _jacobianOplusXi(0,3)=-1./z*camera_->fx_;
  _jacobianOplusXi(0,4)=0;
_jacobianOplusXi(0,5)=x/z_2*camera_->fx_;

_jacobianOplusXi(1,0)=(1+y*y/z_2)*camera_->fy_;
_jacobianOplusXi(1,1)=-x*y/z_2*camera_->fy_;
_jacobianOplusXi(1,2)=-x/z*camera_->fy_;
_jacobianOplusXi(1,3)=0;
_jacobianOplusXi(1,4)=-1./z*camera_->fy_;
_jacobianOplusXi(1,5)=y/z_2*camera_->fy_;

}

修改PnP

void visual_odometry::poseEstimationPnP()
  {
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2> Block;

    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

    Block* solver_ptr = new Block( linearSolver );

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    );
    //稀疏优化,设置优化器，优化方法是LM
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );
    //设置顶点，即要优化的向量，即相机位姿
    g2o::VertexSE3Expmap * pose = new g2o::VertexSE3Expmap();
    pose->setId( 0 );
    pose->setEstimate( g2o::SE3Quat(
      T_c_r_estimated_.rotation_matrix(), T_c_r_estimated_.translation()
    ));
    optimizer.addVertex( pose );
    
    //edges,设RANSAC已经提取完Inline点，现对这些点进行g2o优化
    for ( int i=0; i<inliers.rows; i++)
    {
      int index = inliers.at<int>(i,0);
      //3D->2D映射，设置边的一系列
      EdgeProjectXYZ2UVPoseOnly * edge = new EdgeProjectXYZ2UVPoseOnly();
      edge->setId(i);        //设置边的编号
      edge->setVertex(0, pose);    //设置边连接的顶点，就是相机位姿
      edge->camera_ = curr_->camera_.get();
      edge->point_ = Vector3d( pts3d[Index].x, pts3d[index].y, pts3d[index].z);//什么点？
      edge->setMeasurement( Vector2d(pts2d[Index].x, pts2d[index].y) );    //测量值
      //信息矩阵就是协方差的逆，表示每条边误差分担多少，单位矩阵就是误差分摊
      edge->setInformation(Eigen::Matrix2d::Identity() );
      optimizer.addEdge( edge );
    }
    optimizer.initializeOptimization();
    //10是迭代次数
    optimizer.optimize(10);
    T_c_r_estimated_ = SE3(
      pose->estimate().rotation(),
               pose->estimate().translation()
               );
      //上面都是对pose进行优化，先给点（来自RANSAC），然后设为顶点，用边优化，
    //优化完了再赋值给初始值向量
      

以PnP结果为初始值，调用g2o优化
1.先设置优化器
2.设置顶点，各种信息及编号
optimizer.addVertex( pose );
3.设置边，将边与顶点连接
optimizer.addEdge( edge );

局部地图

思路

将VO匹配到的特征点放到地图中，并将当前帧和地图点进行匹配，计算位姿

这样每一帧都能有所贡献

MapPoint

class MapPoint
  {
  public:
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long_id_;
    static unsigned long factory_id_;    //建造id
    bool good_;
    Vector3d pos_;        //position in world
    Vector3d norm_;        //normal of viewing direction
    Mat descriptor_;        //descriptor for matching
    
    list<Frame*> observed_frames_;    //key-frames that can observe this MapPoint
    
    int matched_times_;        //inliner matched_times_
    int visible_times_;        //being visible in current frame
    
    MapPoint();        //对地图点的构建，每次都有元素进入
    MapPoint(
      unsigned long id,
      const Vector3d& position,
      const Vector3d& norm,
      Frame* frame = nullptr,
      const Mat& descriptor=Mat()
    );
    
    inline cv::Point3f getPositionCV() const{
      return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) );
    }
    
    static MapPoint::Ptr createMapPoint();        //新建，比上面少个id
    static MapPoint::Ptr createMapPoint(
      const Vector3d& pos_world,
      const vector3d& norm_,
      const Mat& descriptor,
      Frame* frame
             );

此处的地图：将各帧的特征点缓存到一处，构成的特征点的集合，称为地图
类的属性：
关键帧，匹配描述子，匹配、观测次数，位置

VisuOdometry类的修改

添加关键帧：

void VisualOdometry::addKeyFrame()
  {
    if ( map_->keyframe_.empty() )    //if not have keyframe
    {
      //first frame , add all 3d point into map_
      for ( size_t i=0; i< keypoints_curr_.size(); i++)
      {
    double d = curr_->findDepth( keypoints_curr_[i] );
    if ( d<0 )
      continue;
    Vector3d p_world = ref_->camera_->pixel2world(
      Vector2d( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ), curr_->T_c_w_, d
                      );
    Vector3d n = p_world - ref_->getCamCenter();    //world * cameracenter ???
    n.normalize();//正则化，是对焦距的处理？
    
    MapPoint::Ptr map_point = MapPoint::createMapPoint(
      p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
                        );
    map_->insertMapPoint( map_point );
      }
    }
    map_->insertKeyFrame( curr_ );    //current frame match with mappoint,compute pose
    ref_ = curr_;
  }

在提取第一帧特征带你之后，将第一帧所有的特征点放入地图中
1.根据像素坐标算出世界坐标，深度在camera类里有
2.算出normalize，调用creatMapPoint函数创造特征点
3.将创造出来的MapPoint加入到map中
4.添加关键帧

map优化（匹配点）增删:

  void VisualOdometry::optimizeMap()
  {
    //remove the hardly seen and no visible points
    for( auto iter = map_->map_point_.begin(); iter !=map_->map_point_.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_)    //匹配的比例太少，删除一些提高比例
      {
    iter = map_->map_points_.erase(iter);
    continue;
      }
      double angle = getViewAngle( curr_, iter->second );
      if ( angle > M_PI/6. )        //转角过大也不要
      {
    iter = map_->map_point_.erase(iter);
    continue;
      }
      if ( iter->second->good_ == false )    //三角测量？
      {
    
      }
      iter++;
    }
    
    if ( match_2dkp-index_.size() < 100 )    //匹配点数量不够的话，加一些地图点
      addMapPoints();
    if( map_->map_points_.size() > 1000 )    //太多了就删一个，同时要增加删除的比例
    {
      iter = map_->map_point_.erase(iter);
      map_point_erase_ratio_ += 0.05;
    }
    else
      map_point_erase_ratio_ = 0.1;
    cout << "map points : " << map_->map_points_.size() << endl;
  }

在后续的帧中，使用OptimizeMap对地图进行优化
删除：
1.不在帧里的
2.匹配比例小，删一部分
3.转动角度过大
4.匹配点太多，大于阈值。同时增加删除的比例
增加：
1.匹配点数量太少，少于阈值

特征匹配:

  void VisualOdometry::featureMatching()
  {
    boost::timer timer;
    vector<cv::DMatch> matches;        //存放所有匹配到的点
    //select the candidates in map
    Mat desp_map;    //构建地图的描述子矩阵
    vector<MapPoint::Ptr> candidate;
    //allpoint
    for ( auto& allpoints: map_->map_points_ )
    {
      MapPoint::Ptr& p = allpoints.second;
      //check if p in curr frame image
           //在视野内就入选
      if( curr_->isInFrame(p->pos_) )
      {
    //add to candidate
    p->visible_times_++;
    candidate.push_back(p);
    desp_map.push_back( p->descriptor_ );
      }
    }
    
    matches_flann_.match( desp_map, descriptors_curr, matches);
    //select the best matches,distance is min
    float min_dis = std::min_element(
      matches.begin(), matches.end(),
      []( const cv::DMatch& m1, const cv::DMatch& m2)
                     {
                       return m1.distance < m2.distance;
                     } )->distance;
    )
    
    match_3dpts_.clear();
    match_2dkp_index_.clear();
    //筛选，距离最小的上面已经选出来了，这里再把比阈值还小的去掉
    for ( cv::DMatch& m: matches )
    {
      if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
      {
    match_3dpts_.push_back( candidate[m.queryIdx] );    //query 测试下标
    match_2dkp_index_.push_back( m.trainIdx );    //样本图像特征带你描述符下标
      }
    }
    
    cout<<" good matched: " << match_3dpts_.size() << endl;
    cout << " mathc cost time: "<<timer.elapsed() << endl;

匹配之前，选出一些视野内的候选点，将它们与当前帧的特征描述子进行匹配
1.选出内点，添加构建地图的描述子矩阵
2.根据两个描述子计算，将匹配信息放入matches,先搞了波最小距离（平均值？）
3.根据距离，选比最小还小的匹配点，但是如果最小距离大于某阈值，那就取小于该阈值的匹配点

缺点:
1.容易丢失，一旦丢失，就要等相机转回来，否则就要重置VO
2.轨迹漂移，主要是误差累计引起的

c++知识

智能指针 shared_ptr

创建：要声明类型 shared_ptr p1;
默认初始化：智能指针中保存着空指针。
使用方法和指针类似


make_shared必须初始化，返回指向此类型shared_ptr //shared_ptr p3 = make_shared(42)
计数器：如表12.2
自动销毁：当没有shared_ptr再指向这个对象时，这个对象就会被销毁
自动释放内存：erase删除不需要的元素后，会自动释放相关联的内存


直接初始化智能指针：
shared_ptr p1 = new int(1024);//错误！
shared_ptr p2( new int(1024) );//正确!

   //特征匹配，拿出一些inline和当前帧的特征描述子匹配
  void VisualOdometry::featureMatching()
  {
    boost::timer timer;
    vector<cv::DMatch> matches;        //存放所有匹配到的点？
    //select the candidates in map
    Mat desp_map;    //存放匹配到的描述子
    vector<MapPoint::Ptr> candidate;
    //allpoint
    for ( auto& allpoints: map_->map_points_ )
    {
      MapPoint::Ptr& p = allpoints.second;
      //check if p in curr frame image
      if( curr_->isInFrame(p->pos_) )
      {
    //add to candidate
    p->visible_times_++;
    candidate.push_back(p);
    desp_map.push_back( p->descriptor_ );
      }
    }
    
    matches_flann_.match( desp_map, descriptors_curr, matches);
    //select the best matches,distance is min
    float min_dis = std::min_element(
      matches.begin(), matches.end(),
                     []( const cv::DMatch& m1, const cv::DMatch& m2)
                     {
                       return m1.distance < m2.distance;
                     } )->distance;
    )
    
    match_3dpts_.clear();
    match_2dkp_index_.clear();
    //筛选，距离最小的上面已经选出来了，这里再把比阈值还小的去掉
    for ( cv::DMatch& m: matches )
    {
      if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
      {
    match_3dpts_.push_back( candidate[m.queryIdx] );    //query 测试下标
    match_2dkp_index_.push_back( m.trainIdx );    //样本图像特征带你描述符下标
      }
    }
    
    cout<<" good matched: " << match_3dpts_.size() << endl;
    cout << " mathc cost time: "<<timer.elapsed() << endl;
  }