视觉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