视觉SLAM十四讲-第九讲笔记

本讲搭建了一个完整的前端框架。在匹配方案上，使用了相邻两帧匹配、与地图匹配两种方法；在位姿估计方法上，使用了PNP方法、PNP+bundle adjustment的方法。
这份代码实现的只有位姿估计和优化，虽然是PnP，但没有估计3d点深度，而是从深度图中取出的。也没有优化地图中特征点的位置。

一、程序框架

1. 数据结构

1.1. camera

相机内参，实现坐标转换。

成员变量

相机内参数

成员函数

坐标转换函数：2d<->camera<->world三个坐标系的相互转换。

1.2. frame

帧

成员变量

含当前帧的外参（位姿信息），以李代数表示
彩色图
深度图
每一帧的标识信息，ID、时间戳等

成员函数

实现图像中一点与帧的交互，如找到对应点深度，判断是否在当前帧内等。

1.3. maopoint

特征点

成员变量

点位置的描述信息：pos、norm、id
点特征的描述信息：descriptor
点匹配的描述信息：观察到的次数、正确匹配的次数等

1.4. map

地图，即特征点的集合。这份代码里实现的是局部地图，没有把所有特征点都加进去，按照距离当前相机的距离和匹配次数有删减。

成员变量

维护两个hash list，键值为其id：
特征点的list
关键帧的list

成员函数

实现特征点和关键帧的增删

2. 框架

实现数据结构后，还需要其他一些文件完成整个流程。

2.1. visual_odometry

实现vo的整改流程。

成员变量

一个enum，维护当前vo的状态[LOST,INIT,OK]
两个frame，前一帧为参考帧ref_，当前帧curr_
一个map
特征描述相关：当前帧的特征点集、匹配点集、特征点描述子，特征点对应2d点的id_。
如果是两帧匹配，还需要记录上一帧特征点的3d坐标集。

成员函数

• 流程控制
  addFrame。对每一个新来的帧进行匹配和估计。
• 特征提取
  extractKeypoint、conputeDescriptor、featureMatching。这三个都是使用opencv中的函数完成。
  在featureMatching部分，和地图中的特征点匹配，如果是两帧之间的匹配，就使用上一阵的特征点集。最终，都要更新当前帧匹配到的3D特征点集和对应2d点集。这里匹配的点集可能存在无匹配
• 位姿估计
  • poseEstimationPnP。先使用solvePnPRansac，从匹配的特征点中估计位姿，得到估计的旋转矩阵、平移矩阵、内点集。内点是经Ransan后，证明是正确匹配的点。 如果使用bundle adjustment，就以PnP的结果为初始值，再进行迭代优化。这一步最终得到估计的位姿（李代数表示）。
  • checkEstimatePose
    要验证当前估计的位姿够不够好，如内点数、朝向等。
• 地图维护
  • optimizeMap：删去里当前帧相机太远、朝向太偏、总是能看见却匹配不到的点。
  • addKeyFrame：不知道维护这个关键帧序列是干嘛的，当前好像没用到，之后建图和后端似乎会用。
  • addPointMaps：如果当前地图里的点太少，就从当前帧的特征点中，添加进地图。

2.2. config

实现从文件中读取参数。实现了一个模板，能够返回各种类型的参数字段。

2.3. common_include

头文件们

2.4. g2o_types

实现了迭代优化时，g2o依赖的方案。重写了computerError和linearizeOplus，用来实现重投影误差和雅克比矩阵。

2.5. run_vo

调用部分。实现数据读取、把每一帧喂给visual_odometry，得到估计的位姿，可视化。

二、文件结构

这个文件结构也是linux下c++项目的经典结构，值得学习。

• /include
• /src
• /bin：编译出来的可执行文件
• /libs：编译出来的库
• /test：run_co.cpp放在这，用来测试各模块。
• /config：配置文件
• /data
• CMakeLists

三、实现细节

1. namespace

这份代码里slam相关的所有内容都括在了myslam命名空间内。

2. 智能指针

所有类都建立了智能指针成员变量，几乎没有直接创建对象。智能指针应该是方便内存管理。

// 定义
typedef std::shared_ptr<Frame> Ptr;
......
// 使用
myslam::Frame::Ptr curr_(new myslam::Frame);

3. 工厂模式

在Frame和MapPoint类里使用了工厂模式，方便创建多个实例。
设定一个静态成员变量factory_id_，和creat函数，需要创建实例时：

Frame::Ptr Frame::createFrame()
{
    // 静态变量，只在第一次进入这个函数的时候初始化为0，其余不执行初始化，只++。
    static long factory_id = 0;
    return Frame::Ptr(new Frame(factory_id++));
}

而factory_id_作为这一个实例的id，在构造函数中列表初始化。

4 .单例模式

Config类使用了单例模式，所有文件共享它的一个实例。
定义一个静态的指针，指向自己。这个指针初始化为nullptr。

shared_ptr<Config> Config::config_ = nullptr;

以及一个静态函数，读取配置文件

static void setParameterFile(const std::string& filename);
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)
    {
        std::cerr << "parameter file " << filename << "does not exist." << std::endl;
        config_->file_.release();
        return;
    }
}

5. 命名规范

所有成员变量的命名都加了下划线curr_，ref_，与参数和临时变量进行区分。

6. 列表初始化

广泛的使用了列表初始化。

Frame::Frame(long id, double time_stamp, SE3 T_c_w , Camera::Ptr camera , Mat color , Mat depth ) 
: id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth)
{
}