《SLAM 14讲》：第九讲 - 视觉里程计完整实现过程 1.0

这讲主要是视觉里程计部分的实现，大部分代码就直接用《SALM 14讲》上的了，目标是输出所有关键帧的位姿。

---

1. 《SLAM 14讲》程序编译问题

在开始自己的实现之前先说一下《SLAM 14讲》第九讲里面程序编译的一些问题

1.1. g2o 问题

书中g2o和最新的版本有点区别，最新版本中有些类的显示转化输入的参数是一个unique_ptr指针，故直接运行时会报错，修改方式参见《SLAM 14讲》第十讲：后端优化的求解BA问题的主程序中的g2o_bunddle_adjustment.cpp部分。

1.2. double free or corruption问题

修改好g2o部分后编译正常，但运行时出现了double free or corruption问题，如下图：
为解决这个问题，把CMakelists.txt中的

set( CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3" )

改成

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

这个问题的解决参考了《高翔slam project0.3出现double free or corruption (out)问题解决》，这个问题和代码无关，所以具体的原因还没有细究。

2. 开始的开始

2.1. 数据准备

和书中一样，直接用了 TUM 的RGB-D SLAM Dataset and Benchmark，在使用了数据集提供的对齐代码后（associate.py）就得到了对应的相机拍摄的彩色图和深度图。

2.2. 视觉里程计原理

视觉里程计部分总的目标就是估计每一个相机的位姿，或者说是每一个关键帧下相机的位姿。我们可以只进行相邻两帧之间的匹配，但这样过于依赖参考帧。故我们采用0.4中的局部地图方法来处理每一个相机的位姿，即把每帧与地图点之间进行匹配，如下图所示：
这里的地图是不断更新的，即每插入新的一帧时优化一下局部地图：删掉一些不在视野内的点，加入一些新的点。最后我们得到的是所有的关键帧的信息。

所以为了实现上述过程，我们便有了一下几个问题：

1. 如何构建局部地图 ？
2. 如何匹配局部地图中的点与当前帧的特征点，并估算当前帧的位姿 ？
3. 如何判断当前帧是关键帧 ？
4. 如何优化局部地图 ？

---

3. 实现过程

3.1. 实现步骤解析

对于之前的问题一一回答：

1： 我们认为第一帧为关键帧，提取所有的特征点并放入局部地图。

2： 判断局部地图中的点是否在当前帧可以被观测到（这里我们开始并不知道当前帧的位姿，所以把当前帧的位姿设为上一参考帧的位姿），提取所有可以观测到的点，并与当前帧的特征点匹配。之后我们便可以用PnP或者ICP匹配运动。

3： 计算当前帧和参考帧之间的运动，若相机旋转或移动超过一定的大小便设为关键帧。这个最小旋转角或者最小移动距离是自己设定的值。

4： 当路标点不被当前帧所观测到时，删掉路标点；

   当路标点 匹配的次数/被观测到的次数小于某个系数时，删除路标点；

   当路标点初始观测角度与现观测角度大于一定角度时，删除路标点；

   当当前帧匹配的特征点过少时，把当前帧未匹配的特帧点加入局部地图；

   当地图过大时，删除一些路标点。

可以注意到，在上述回答中加粗的对象可以分成四类：帧、路标点、地图、相机。故我们在用C++实现的时候可以创建四个类（仿照《SLAM 14讲》），另外也可以添加一个Config类来读取一些参数。最后，我们还需要一个VisualOdometry类来连接这四个类来实现视觉里程计部分。

我们下面分析一下四类的构成。

3.2. 编程结构分析

这部分主要分析一下书上的代码，基本和书上的都一样

一些 $id$ 的生成借助了 static 关键字，详情请参见《c++中static的用法详解》。

相机类：camera.h

1. 主要储存RGB-D相机的内参和深度系数（depth scale）；
2. 定义一些变换参考系的函数，比如：

   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 );
   

帧类：frame.h

1. 主要储存每一帧的 $id$ 、时间戳、位姿、对应的相机模型、彩色图和深度图、是否为关键帧；
2. 成员函数如下：

   // 创建一个空的Frame指针
   // 定义了一个 static int factory_id 用于生成 id
   static Frame::Ptr createFrame(); 
   // 寻找某一特征点的深度
   double findDepth( const cv::KeyPoint& kp );
   // 得到该帧下相机中心坐标
   Vector3d getCamCenter() const;
   // 设置该帧的位姿
   void setPose( const SE3& T_c_w );
   // 判断该帧是否能观测到某个路标点
   bool isInFrame( const Vector3d& pt_world );
   

路标类：mappoint.h

1. 主要储存每一个路标点的 $id$ 、空间位置、第一次观测到时的方向（路标点坐标-相机中心，用于判断优化局部地图时是否保留）、对应的描述子数据、可观测到这个路标点的帧（list）、被观测到的次数、匹配的次数；
2. 成员函数：

   // 创建MapPoint指针
   static MapPoint::Ptr createMapPoint();
   static MapPoint::Ptr createMapPoint( 
       const Vector3d& pos_world, 
       const Vector3d& norm_,
       const Mat& descriptor,
       Frame* frame );
   

3. 另外还有一个 static int factory_id_ 的静态数据成员用于生成 $id$。

地图类：map.h

1. 主要储存所有的路标点和所有的关键帧（unordered_map）；
2. 成员函数：

   // 插入帧
   void insertKeyFrame( Frame::Ptr frame );
   // 插入路标点
   void insertMapPoint( MapPoint::Ptr map_point );
   

Visual Odometry：visual_odometry.h

#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H

#include "common/common_include.h"
#include "common/map.h"

#include 

class VisualOdometry
{
public:
    typedef shared_ptr<VisualOdometry> Ptr;
    enum VOState {
        INITIALIZING=-1,
        OK=0,
        LOST
    };
    
    VOState     state_;     // 当前视觉里程计的状态
    Map::Ptr    map_;       // 地图：局部路标点和所有的关键帧
    
    Frame::Ptr  ref_;       // 参考帧
    Frame::Ptr  curr_;      // 当前帧
    
    cv::Ptr<cv::ORB> orb_;  // ORB特征点识别器
    vector<cv::KeyPoint>    keypoints_curr_;    // 当前帧特征点
    Mat                     descriptors_curr_;  // 当前帧描述子
    
    cv::FlannBasedMatcher   matcher_flann_;     // flann 匹配器
    vector<MapPoint::Ptr>   match_3dpts_;       // 匹配的路标点
    vector<int>             match_2dkp_index_;  // 匹配的当前帧特征点
   
    SE3 T_c_w_estimated_;    // 估算的当前帧位姿
    int num_inliers_;        // PnP考虑进去的特征点（inlier points）
    int num_lost_;           // 丢失的帧数
    
    // ORB特征点识别器的参数
    int num_of_features_; 
    double scale_factor_;
    int level_pyramid_;
    
    float match_ratio_;     // 寻找好的匹配的系数：最小距离*系数
    int max_num_lost_;      // 最大可接受的丢失帧数
    int min_inliers_;       // 最小可接受的 inlier points
    double key_frame_min_rot;   // 两个关键帧之间的最小旋转量
    double key_frame_min_trans; // 两个关键帧之间的最小平移量
    double  map_point_erase_ratio_; // 优化地图时的系数：匹配次数/观测次数 < 系数，删去路标点
    
public: 
    VisualOdometry();
    ~VisualOdometry();
    // 添加新的一帧
    // 若状态为INITIALIZING：
    // 设置参考帧为该帧、该帧为关键帧加入map_
    // 若状态为OK：
    // 匹配特征点并估计位姿、判断估计是否可接受并优化地图、判断是否为关键帧...
    // 若状态为LOST：退出并输出异常
    bool addFrame( Frame::Ptr frame );   
protected:  
	// 提取匹配特征点并优化
    void extractKeyPoints();
    void computeDescriptors(); 
    void featureMatching();
    void poseEstimationPnP(); 
    // 优化地图
    void optimizeMap();
    
    // 添加关键帧、路标点
    void addKeyFrame();
    void addMapPoints();
    // 判断估计是否可以接受
    bool checkEstimatedPose(); 
    // 判断是否为关键帧
    bool checkKeyFrame();
    
    // 获得某路标点在当前帧视角
    double getViewAngle( Frame::Ptr frame, MapPoint::Ptr point );
};

---

4. 结果

之后的步骤就非常简单了，就是不断的往VisualOdometry类实例 VisualOdometry::Ptr vo 中加入帧（addFrame），最后的 vo 中储存的为所有关键帧的信息。

增加了一个写入文件的函数，文件格式为：

帧 id 帧时间戳 旋转向量 平移向量
…

void VisualOdometry::WriteToFile(const string& filename)
{
	ofstream fout;
	fout.open(filename);
	for (auto iter=map_->keyframes_.begin(); iter!=map_->keyframes_.end();iter++)
	{
	    fout << std::setprecision(16) << iter->second->id_ << " " << iter->second->time_stamp_ << " ";
	    Eigen::Matrix3d R = iter->second->T_c_w_.rotation_matrix();
        Vector3d t = iter->second->T_c_w_.translation();
        Eigen::AngleAxisd r(R);
        double angle = r.angle();
        Vector3d r_vector = r.axis() * angle;
        fout << r_vector(0,0) << " " << r_vector(1,0) << " " << r_vector(2,0) << " " << t(0,0) << " " << t(1,0) << " " << t(2,0) << endl;
	}
}

因为这只是一个简单的视觉里程计，我们把特征点用过之后就丢掉了，故最后输出的只是每一帧的位姿信息。