视觉SLAM十四讲学习笔记——第十二讲 建图

         SLAM实际上是一种底层技术，往往用来给上层应用提供信息。在前文已实现的部分，我们最多得到的仅是一个稀疏的地图，在需要实现导航、避障、重建等应用时，稀疏地图很难提供足够的信息，需要考虑如何重建稠密地图，即建模所有看到过的部分。

        根据使用相机的不同可以分为：“单目稠密重建”和“RGB-D稠密建图”（双目应该和RGBD类似，都可以主动测得或计算出图像深度）。其中RGB-D相机的稠密重建较为简单，最直观、简单的方法就是将RGB-D数据转换为点云地图，再结合相机位姿进行拼接，并对点云加一些滤波处理。这里使用了PCL点云库和两种滤波方式：

（1）外点去除滤波器：由于Kinect量程有限，去掉深度值无效的点。

（2）体素网格的降采样滤波器：多视角点云拼接时，由于视野重叠，重叠区域会有大量位置十分相近的点，体素滤波器保证一定大小的体素内仅有一个点，相当于对三维空间降采样。

        点云地图属于比较初级的地图形式，我们可以在点云的基础上重建网格，同时还可以实现了常用于导航中的“八叉树地图”，这里使用octomap库可以简单地生成八叉树地图。最后介绍了一些常见的地图类型。

        相比于RGB-D相机的稠密重建，单目相机的稠密重建较为复杂，因为不能主动测得图像深度，我们需要匹配相邻的两幅图像，并根据三角重建原理自行计算图像点的深度（视觉里程计讲到过）。因此给出了单目稠密重建的详细理论以及代码示例。

1.理论分析

        单目相机估计稠密深度的完成过程如下：

（1）初始假设：所有像素深度都满足某一高斯分布（不是同一个）。

（2）通过极线搜索和块匹配确定重投影点的位置：极线搜索是根据已知的两次拍摄相机间的位置关系将一个点的匹配搜索范围从整幅图像缩小为一根直线；此时匹配仅能借助亮度不变性进行区分，但是由于单个点的亮度区分度不大，改为“图像块的灰度不变性”，设定评价标准（SAD、SSD、NCC等），提高匹配的准确度。

（3）根据几何关系计算三角化后的深度（均值）及不确定性（协方差矩阵）。

（4）融合上一次的估计值（高斯分布的乘积），若收敛则停止，认为估计到较为准确的深度分布信息，否则返回第二部。

        了解了理论分析部分，再来看给出的非常复杂的示例代码。

2.代码分析

        分析代码前要明确这段代码在做什么，示例程序使用开源数据集，共200张无人机采集的单目俯视图像，现在要利用这些图像估计第一帧图像（0号图像）每个像素的深度，即单目稠密重建。我们已知的信息有201张图像及其对应相机的外部参数（旋转和平移）。

        先看主程序：首先是读取所有所需数据，然后初始化第0幅图像（0号）的深度图（高斯分布的矩阵）和不确定图像（高斯分布方差），初始化时认为每个像素的分布参数相同，注意不能为0，否则没办法更新了。初始化后循环1-200号图像，不断更新第0幅图像的像素深度分布信息。

int main(int argc, char **argv) {

    // 从数据集读取数据
    Vector3d test;
    test<<2,2,2;
    double angle=test.dot(test);
    cout<<angle<<endl;
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles("/home/svv12138/Downloads/slambook2-master/ch12/dense_mono/remode_test_data/test_data", color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // 第一张图
    Mat ref = imread(color_image_files[0], 0);                // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];//初始图像位姿
    double init_depth = 3.0;    // 深度初始值
    //double init_depth_inv = 0.3;  // 深度逆初始值
    double init_cov2 = 3.0;     // 方差初始值