ch13——建图
主要目标：1.理解单目SLAM中稠密深度估计的原理
2.通过实验了解单目稠密重建的过程
3.了解几种RGB-D重建中的地图形式
本讲我们将更加详细地讨论各种形式的地图，并指出目前视觉SLAM地图中存在着的缺陷。
SLAM作为一种底层设备，往往是用来为上层应用提供信息的。应用层对于“定位”的需求是相似的，而对于“建图”，各应用之间则存在着不同的需求。
地图的应用，大致可分为：1）定位。2）导航：在这个过程中，我们至少需要知道的是地图中哪些地方可以通过，哪些地方不可通过，而要能实现这样的功能，则至少是稠密地图。3）避障：与导航类似，但更加关注局部的、动态的障碍物的处理，同样也需要至少是稠密的地图。4）重建：这种地图也是稠密的，并且我们对这种地图还有外观要求。5）交互：主要指人与地图之间的互动。语义地图。
稀疏地图 VS 稠密地图：稠密地图是相对于稀疏地图而言的，稀疏地图只建模感兴趣的部分，也就是特征点（路标点），而稠密地图是指建模所有看到过的部分。
1.单目稠密重建
在稠密重建中，我们需要知道每一个像素点（或大多数像素点）的距离，有三种解决方法：
1）使用单目相机，通过移动相机之后进行三角化测量像素的距离。（这种方式又称为移动视觉的立体视觉，Moving View Stereo）。
2）使用双目相机，利用左右目的视差计算像素的距离，与多目原理相同。
1）2）两种方式又称为立体视觉（Stereo Vision）：立体视觉的重建质量十分依赖于环境纹理。(该问题无法在现有的算法流程上加以改进和解决的)
3）使用RGB-D相机直接获得像素距离。
这三种方法的优缺点对比：相比于RGB-D直接测量的深度，单目和双目对深度的获取往往是“费力不讨好“的：因为我们要花大量的时间用于计算，最后得到一些并不怎么可靠的深度估计。所以往往我们选择用RGB-D进行稠密重建。但是RGB-D有一些量程、应用范围和光照的限制。单目、双目的好处是：对于目前RGB-D无法很好应用的室外、大场景场合中，仍能通过视觉立体估计深度信息。
我们将来实现一遍单目的稠密估计，体验一下为何说这种方式是”费力不讨好的“。
我们先来考虑：在给定相机轨迹的基础上，如何根据一段时间的视频序列来估计某幅图像图像的深度（也即是先不考虑SLAM，只看建图问题）。步骤如下：
1）假设有一段视频序列，我们已经获得了每一帧对应的轨迹。现在我们以第一幅图像为参考帧，来计算参考帧中每一个像素的深度（距离）；
2）在稠密深度图估计中，我们不能把每个像素都当作特征点计算描述子，所以，这里我们采用极线搜索和块匹配技术来确定第一幅图的某像素出现在其他图里的位置。
3）当我们知道了某个像素在各个图中的位置，就能像特征点那样，利用三角测量确定它的深度。这里要注意的是我们要使用多次三角测量让深度估计收敛，而不仅仅是一次。用到的是深度滤波器技术（深度估计能够随着测量的增加从一个非常不确定的量逐渐收敛至一个稳定值）。
1.1 极线搜索与块匹配
计算小块间差异的方法：SAD（差的绝对值之和，Sum of Absolute Difference）、SSD（平方和，Sum of Squared Distance）、NCC（归一化互相关，Normalized Cross Correlation）
去均值的SAD、去均值的SSD等……
1.2 高斯分布的深度滤波器
估计像素点的深度，可建模为一个状态估计问题。所以有滤波器和非线性优化两种求解思路。而SLAM实时性要求较高，前端已经占据了不少的计算量，所以在建图中，我们通常采用计算量较少的滤波器方式。
1）假设深值服从高斯分布
估计稠密深度的一个完整过程：
（1）假设所有像素的深度满足某个初始的高斯分布
（2）当新数据产生时，通过极线搜索和块匹配确定投影点的位置
（3）根据几何关系计算三角化后的深度及不确定性
（4）将当前观测融合进上一次的估计中，若收敛则停止计算，否则返回第（2）步。
实践一：单目稠密重建
使用REMODE的测试数据集：提供了一架无人机采集的单目俯视图像，共有200张，同时提供了每张图像的真实位姿。在这些数据的基础上，估算第一帧图像上每个像素对应的深度值，即进行单目稠密重建。
由于实验代码篇幅有些长，这里不贴上。
结论：我们根据最后得到的结果可以看出：整个估计大部分是正确的，但是也存在着大量错误的估计。它们表现为深度图中与周围数据不一致的地方，为过大或过小的估计。位于边缘处的地方，由于运动过程中看到的次数少，所以也没有得到正确的估计。我们将讨论应该在哪些地方做出一些改进。
（1）极线方向和梯度方向之间的关系
当像素梯度与极线夹角较大时，极线匹配的不确定性大；反之，不确定性小（想一下，如果极线与像素梯度垂直，所有的匹配程度都一样，也就无法得到有效的匹配，而当极线与梯度平行，就能精确地确定匹配度最高点出现在何处P340图13-7）。
所以我们在上面的程序中把所有的不确定性都设为一个像素的误差，是不够准确的，应该把极线和梯度之间的关系考虑到我们要构建的不确定模型中。
（2）逆深度
把逆深度而不是深度假设为服从高斯分布更为准确，并且逆深度也具有更好的数值稳定性。
（3）考虑图像间的变换 P342
（4）并行化-效率的问题。在进行稠密深度图的估计时，我们对几十万个像素点的深度估计是彼此无关的，所以可以利用并行化。但是在CPU中，这个过程只能是串行进行，可以考虑用GPU。
（5）如果使用并行化使得各像素独立计算，可能存在这个像素深度很小，边上一个又很大的情况。我们可以假设深度途中相邻的深度变化不会太大，从而给深度估计加上了空间正则项。这种做法会让深度图更加平滑。
（6）显示处理外点（Outlier)。处理错误匹配。
总之，知道目前为止，虽然单目和双目能够建立稠密的地图，但是因为它们过于以来环境纹理和光照，我们认为它们不够可靠。
2）假设深度服从均匀-高斯混合分布：见习题
2.RGB-D稠密重建
利用RGB-D进行稠密建图的好处：
1）利用RGB-D相机可以完全通过传感器中硬件测量得到深度估计，而无须像单目和双目中那样通过消耗大量的计算资源来估计。
2）RGB-D的结构光或飞时原理，保证了深度数据对纹理的无关性。即使是纯色的物体，只要它能够反射光，我们就能测量到它的深度。
根据地图形式的不同，存在着若干种不同的主流建图方式：
1）最直观、最简单的方式：根据估算的相机位姿，将RGB-D数据转化为点云（Point Cloud），然后进行拼接，最后得到一个由离散的点组成的点云地图。
2）如果我们希望估计物体的表面，在1）的基础上，使用三角网格(Mesh)、面片(Surfel)进行建图。
3）如果我们想要知道地图的障碍物信息并在地图上导航，亦可通过体素（Voxel）建立占据网格地图（Occupancy Map）。
2.1点云地图
实践二：点云地图
点云：由一组离散的点表示的地图。最基本的点包含x，y，z三维坐标，也可以带有r，g，b的彩色信息。
由于RGB-D相机提供了彩色图和深度图，我们很容易根据相机内参来计算RGB-D点云。如果通过某种手段得到了相机位姿，只要直接把点云进行加和，就可以获得全局的点云。
本实验在ch5点云拼接实验（<<视觉slam十四讲>>_ch5 实践部分之拼接点云joinMap.cpp代码解释及总结）的基础上对点云增加一些滤波处理（主要使用了两种滤波器：外点去除滤波器、降采样滤波器），以获得更好的视觉效果。代码如下：
1 #include <iostream> 2 #include <fstream> 3 using namespace std; 4 5 #include <opencv2/core/core.hpp> 6 #include <opencv2/highgui/highgui.hpp> 7 //#include <Eigen/Core> 8 #include <Eigen/Geometry> 9 #include <boost/format.hpp> 10 #include <pcl/point_types.h> 11 #include <pcl/io/pcd_io.h> 12 #include <pcl/filters/voxel_grid.h> 13 #include <pcl/visualization/pcl_visualizer.h> 14 #include <pcl/filters/statistical_outlier_removal.h> 15 //#include <pcl/filters/statistical_outlier_removal.h> 16 using namespace cv; 17 18 int main(int argc,char** argv) 19 { 20 vector<cv::Mat> colorImgs,depthImgs;//彩色图和深度图 21 vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;//!!vector<Eigen::Isometry3d> poses;报段错误（核心已转储） 22 23 ifstream fin("./data/pose.txt"); 24 if(!fin) 25 { 26 cout<<"cannot find pose.txt!"<<endl; 27 return 1; 28 } 29 30 for(int i=0;i<5;++i) 31 { 32 boost::format fmt("./%s/%s/%d.%s");//图像文件格式 33 //test 34 colorImgs.push_back(cv::imread((fmt%"data"%"color"%(i+1)%"png").str())); 35 depthImgs.push_back(cv::imread((fmt%"data"%"depth"%(i+1)%"pgm").str(),-1));//test 36 37 double data[7]; 38 for(auto& d:data)//test:& 39 { 40 fin>>d; 41 } 42 Eigen::Quaterniond q(data[6],data[3],data[4],data[5]); 43 Eigen::Isometry3d T(q); 44 T.pretranslate(Eigen::Vector3d(data[0],data[1],data[2])); 45 // cout<<T.matrix()<<endl; 46 poses.push_back(T); 47 } 48 49 50 // //1.根据内参计算一对RGB-D图像对应的点云 51 // //计算点云 52 // //内参 53 double cx=325.5; 54 double cy=253.5; 55 double fx=518.0; 56 double fy=519.0; 57 double depthScale=1000.0; 58 59 cout<<"正在将图像转换为点云..."<<endl; 60 61 //定义点云使用的格式：这里用的是XYZRGB 62 typedef pcl::PointXYZRGB PointT; 63 typedef pcl::PointCloud<PointT> PointCloud; 64 65 //2.根据各张图的相机位姿（也就是外参），把点云加起来，组成地图 66 //新建一个点云 67 PointCloud::Ptr pointCloud(new PointCloud); 68 for(int i=0;i<5;++i) 69 { 70 PointCloud::Ptr current(new PointCloud); 71 cout<<"转换图像中: "<<i+1<<endl; 72 // Mat color=colorImgs[i]; 73 // Mat depth=depthImgs[i]; 74 // Eigen::Isometry3d T=poses[i]; 75 cv::Mat color = colorImgs[i]; 76 cv::Mat depth = depthImgs[i]; 77 Eigen::Isometry3d T = poses[i]; 78 79 for(int r=0;r<color.rows;++r) 80 for(int c=0;c<color.cols;++c) 81 { 82 unsigned int d=depth.ptr<unsigned short> (r)[c];//（r,c）对应的深度值 83 // cout<<"???"<<endl; 84 // cout<<d<<" "; 85 if(d==0) continue;//为0表示没有测量到 86 if(d>=7000) continue;//深度值太大时，不稳定，去掉 87 Eigen::Vector3d point; 88 //像素->相机 89 point[2]=double(d)/depthScale;//test:d 90 point[0]=(r-cx)*point[2]/fx; 91 point[1]=(c-cy)*point[2]/fy; 92 //相机->世界 93 Eigen::Vector3d pointWorld=T*point; 94 95 PointT p; 96 p.x=pointWorld[0]; 97 p.y=pointWorld[1]; 98 p.z=pointWorld[2]; 99 p.b=color.data[r*color.step+c*color.channels()]; 100 p.g=color.data[r*color.step+c*color.channels()+1]; 101 p.r=color.data[r*color.step+c*color.channels()+2]; 102 103 current->points.push_back(p); 104 } 105 106 //depth filter and statistical removal 107 //利用统计滤波器方法去除孤立点 108 PointCloud::Ptr tmp ( new PointCloud ); 109 pcl::StatisticalOutlierRemoval<PointT> statistical_filter; 110 statistical_filter.setMeanK(50); 111 statistical_filter.setStddevMulThresh(1.0); 112 statistical_filter.setInputCloud(current); 113 statistical_filter.filter( *tmp ); 114 (*pointCloud) += *tmp; 115 cout<<endl<<endl; 116 } 117 118 pointCloud->is_dense=false; 119 cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl; 120 121 // voxel filter 122 //利用体素滤波器进行降采样 123 pcl::VoxelGrid<PointT> voxel_filter; 124 voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution 125 //分辨率为0.01,表示每立方厘米有1个点 126 PointCloud::Ptr tmp ( new PointCloud ); 127 voxel_filter.setInputCloud( pointCloud ); 128 voxel_filter.filter( *tmp ); 129 tmp->swap( *pointCloud ); 130 131 cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl; 132 pcl::io::savePCDFileBinary("map.pcd",*pointCloud); 133 return 0; 134 }
实验结果及得到的点云地图为：
分析：本实验在ch5点云拼接实验上增加了：1）在生成每帧点云时，去掉深度值太大或无效的点。因为超过Kinect的有效量程之后深度值会有较大的误差。
2）利用统计滤波器方法去除孤立的噪声点。
3）利用体素滤波器（Voxel Filter）进行降采样。由于多个视角存在视野重叠，在重叠区域存在大量位置十分相近的点，白白占用许多内存空间。体素滤波器保证了在某个一定大小的立方体（或称体素）内仅有一个点，相当于对三维空间进行了降采样，从而节省了很多存储空间。
使用点云表示地图的优势：1）点云地图提供了比较基本的可视化工具，让我们能够大致了环境的样子。
2）点云以三维方式存储，使得我们能够快速浏览场景的各个角落，乃至在场景中进行漫游。
3）点云可直接由RGB-D图像高效生成，不需要额外处理。
4）点云的滤波操作非常直观，且处理效率尚能接受。
使用点云表示地图的局限：点云地图是“基础的”、“初级的”：点云地图更接近于传感器读出的原始数据，它具有一些基本的功能，但通常用于调试和基本的显示，不便直接用于应用程序。点云地图是个不错的出发点，我们可以在此基础上构建具有更高功能的地图，大部分由点云转换得到的地图形式都在PCL库中提供。
2.2 八叉树地图
点云地图明显的缺陷：
1）点云地图通常规模很大，所以得到的pcd文件也会很大，需要大量的存储空间。然而这种“大”并不是必须的，它存储了很多不必要的细节。由于这些空间的占用，除非降低分辨率（降低分辨率又会导致地图质量下降），否则在有限的内存中，无法建模较大规模的环境。
2）点云地图无法处理运动物体。因为在生成点云地图时，我们只有“添加点”，而没有“当点消除时把它移除”的做法。但是生命在于运动，现实中，运动的物体又普遍存在，这使得点云地图不怎么实用。（考虑：能不能在点云地图中实现当点消除时把它移除，当地图规模不怎么大时就是用点云地图呢？？？毕竟点云地图的思路比较简单嘛）
所以现在我们来考虑一种灵活的、压缩的、又能随时更新的地图形式——八叉树（Octo-tree）
把一个大的三维空间细分为许多个小方块（或体素）（即从最大空间细分到最小空间的过程），就是一棵八叉树。
我们在建模点云地图时，利用了体素滤波器，限制了一个体素中只有一个点，但是点云仍然要比八叉树占用更多的空间，是因为：在八叉树中，我们在节点中存储它是否被占用的信息，当某个方块的所有子节点都被占局或都不被占据时，就不会展开这个节点（注意此处与点云的区别）。大多数的八叉树节点都无需展开到叶子层面。
关于判断八叉树中节点是否被占据的数学理论见教材P349、P350。
实践三：八叉树地图
我们将演示通过几张图片生成八叉树地图，并将它画出来。代码如下：
1 #include <iostream> 2 #include <fstream> 3 using namespace std; 4 5 #include <opencv2/core/core.hpp> 6 #include <opencv2/highgui/highgui.hpp> 7 8 #include <octomap/octomap.h> 9 10 #include <Eigen/Geometry> 11 #include <boost/format.hpp> 12 using namespace cv; 13 14 int main(int argc,char** argv) 15 { 16 vector<cv::Mat> colorImgs,depthImgs;//彩色图和深度图 17 vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;//!!!! 18 19 ifstream fin("./data/pose.txt"); 20 if(!fin) 21 { 22 cout<<"cannot find pose.txt!"<<endl; 23 return 1; 24 } 25 26 for(int i=0;i<5;++i) 27 { 28 boost::format fmt("./%s/%s/%d.%s");//图像文件格式 29 //test 30 colorImgs.push_back(cv::imread((fmt%"data"%"color"%(i+1)%"png").str())); 31 depthImgs.push_back(cv::imread((fmt%"data"%"depth"%(i+1)%"pgm").str(),-1));//test 32 33 double data[7]; 34 for(auto& d:data)//test:& 35 { 36 fin>>d; 37 } 38 Eigen::Quaterniond q(data[6],data[3],data[4],data[5]); 39 Eigen::Isometry3d T(q); 40 T.pretranslate(Eigen::Vector3d(data[0],data[1],data[2])); 41 // cout<<T.matrix()<<endl; 42 poses.push_back(T); 43 } 44 45 // //内参 46 double cx=325.5; 47 double cy=253.5; 48 double fx=518.0; 49 double fy=519.0; 50 double depthScale=1000.0; 51 52 cout<<"正将图像转换为 Octomap..."<<endl; 53 54 //Octomap tree 55 octomap::OcTree tree(0.05);//参数表示分辨率 56 for(int i=0;i<5;++i) 57 { 58 cout<<"转换图像中: "<<i+1<<endl; 59 cv::Mat color = colorImgs[i]; 60 cv::Mat depth = depthImgs[i]; 61 Eigen::Isometry3d T = poses[i]; 62 63 octomap::Pointcloud cloud;//the point cloud in octomap 64 65 for(int r=0;r<color.rows;++r) 66 for(int c=0;c<color.cols;++c) 67 { 68 unsigned int d=depth.ptr<unsigned short> (r)[c];//（r,c）对应的深度值 69 // cout<<"???"<<endl; 70 // cout<<d<<" "; 71 if(d==0) continue;//为0表示没有测量到 72 if(d>=7000) continue;//深度值太大时，不稳定，去掉 73 Eigen::Vector3d point; 74 //像素->相机 75 point[2]=double(d)/depthScale;//test:d 76 point[0]=(r-cx)*point[2]/fx; 77 point[1]=(c-cy)*point[2]/fy; 78 //相机->世界 79 Eigen::Vector3d pointWorld=T*point; 80 81 //将世界坐标系的点放入点云 82 cloud.push_back(pointWorld[0],pointWorld[1],pointWorld[2]); 83 } 84 85 //将点云存入八叉树地图，给定原点，这样可以计算投射线 86 tree.insertPointCloud(cloud,octomap::point3d(T(0,3),T(1,3),T(2,3)));//Otcomap内部提供了一个点云结构，比PCL的点云稍微简单一点，只携带点的空间位置信息 87 } 88 89 //更新中间节点的占据信息并写入磁盘 90 tree.updateInnerOccupancy(); 91 cout<<"saving octomap..."<<endl; 92 tree.writeBinary("octomap.bt"); 93 return 0; 94 }
我们利用可视化程序octovis打开最后得到的地图文件：
octovis octomap.bt
对比实践二得到的点云地图和实践三得到的八叉树地图的文件大小，分别是7.9MB和94.1KB，由此可见八叉树地图占据的内存小的多，可以有效的建模较大的场景。
3.实时三维重建
之前介绍的地图模型（以定位为主），而是以“建图”为主体，定位居于次要地位的一种做法。所以需要利用GPU进行加速。