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

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

  • 1. 《SLAM 14讲》程序编译问题
    • 1.1. g2o 问题
    • 1.2. double free or corruption问题
  • 2. 开始的开始
    • 2.1. 数据准备
    • 2.2. 视觉里程计原理
  • 3. 实现过程
    • 3.1. 实现步骤解析
    • 3.2. 编程结构分析
  • 4. 结果

这讲主要是视觉里程计部分的实现,大部分代码就直接用《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中的局部地图方法来处理每一个相机的位姿,即把每帧与地图点之间进行匹配,如下图所示:
《SLAM 14讲》:第九讲 - 视觉里程计完整实现过程 1.0_第1张图片这里的地图是不断更新的,即每插入新的一帧时优化一下局部地图:删掉一些不在视野内的点,加入一些新的点。最后我们得到的是所有的关键帧的信息。

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

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

3. 实现过程

3.1. 实现步骤解析

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

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

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

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

4: 当路标点不被当前帧所观测到时,删掉路标点

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

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

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

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

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

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

3.2. 编程结构分析

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

一些 i d id 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. 主要储存每一帧的 i d id 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. 主要储存每一个路标点的 i d id 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_ 的静态数据成员用于生成 i d 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;
	}
}

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

你可能感兴趣的:(SLAM,slam)