视觉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)
{
}