主要内容
1. 概述
在视觉SLAM看来，“建图”是服务于“定位”的。从应用层面来看，地图的作用如下：
1.1） 定位
1.2） 导航
至少需要知道地图中哪些地方可以通过，哪些地方不可通过——至少是一种稠密的地图
1.3） 避障 —— 稠密地图
1.4） 重建 —— 稠密地图
1.5） 交互 —— 语义地图
2. 单目稠密重建
2.1） 立体视觉
2.1.1）单副图像像素：只能提供物体与相机成像平面的角度及物体采集到的亮度，而无法提供物体的距离。
2.1.2）在稠密重建中，获取每一个像素点（或大部分像素点）的距离 的解决方案：
a 使用单目相机，通过移动相机之后进行三角化测量像素的距离
b 使用双目相机，利用左右目的视差计算像素的距离(多目原理相同)
c 使用RGB-D相机直接获得像素距离
前两种方式称为立体视觉（Stereo Vision）,其中移动单目的又称为移动视角的单目视觉（Moving View Stereo）
2.1.3） 优缺点
a 单目，双目：需要花费大量的计算，得到的 深度估计不可靠（fragile）
b RGB-D：量程，应用范围和光照的限制，稠密重建中更常选择
c 室外，大场景场合中，通过立体视觉估计深度信息。
2.1.4） 单目稠密重建关键点
a 像素匹配——极线搜索和块匹配技术
b 三角测量确定深度——深度滤波器技术
2.2） 极线搜索与块匹配
2.2.1） 极线概念
对于参考帧图像中的任意像素，其深度值可能在某个最小值到无穷远之间，像素对应的空间特征点分布在一条射线上；
在当前帧图像看来，这条射线的投影形成图像平面上的一条线——极线
当知道两帧图像之间的运动时，极线边可求解得到。
2.2.2） 块匹配
a：特征点方法中，通过特征匹配找到确定当前帧中P2的位置，而稠密重建中，需要在极线上搜索确定相似的点。由于单个像素没有区分性，因此选取像素块进行比较。
b: 具体方法是：首先，在参考帧p1点处去w*w的小块；然后在极线上区很多同样大小的小块；最后通过图像块差异的度量方法确定像素点的相似性（算法的假设从像素灰度的不变性到图像块灰度不变性）
c： 图像块相似性度量方法：
SAD(Sum of Absolute Difference): 
SSD(Sum of Squared Distance):
NCC(Normalized Cross Correlation):
（ 注：SAD,SSD接近0表示相似，而NCC接近1表示相似；
实际计算中，可以先对小块进行去均值操作，在进行相似度计算，可避免图像块整体亮度差异的影响，算法更稳定）
NCC计算的代码如下：
double NCC ( const Mat& ref, const Mat& curr, const Vector2d& pt_ref, const Vector2d& pt_curr ) { // 零均值-归一化互相关 // 先算均值 double mean_ref = 0, mean_curr = 0; vector<double> values_ref, values_curr; // 参考帧和当前帧的均值 for ( int x=-ncc_window_size; x<=ncc_window_size; x++ ) for ( int y=-ncc_window_size; y<=ncc_window_size; y++ ) { double value_ref = double(ref.ptr<uchar>( int(y+pt_ref(1,0)) )[ int(x+pt_ref(0,0)) ])/255.0; mean_ref += value_ref; double value_curr = getBilinearInterpolatedValue( curr, pt_curr+Vector2d(x,y) ); mean_curr += value_curr; values_ref.push_back(value_ref); values_curr.push_back(value_curr); } mean_ref /= ncc_area; mean_curr /= ncc_area; // 计算 Zero mean NCC double numerator = 0, demoniator1 = 0, demoniator2 = 0; for ( int i=0; i<values_ref.size(); i++ ) { double n = (values_ref[i]-mean_ref) * (values_curr[i]-mean_curr); numerator += n; demoniator1 += (values_ref[i]-mean_ref)*(values_ref[i]-mean_ref); demoniator2 += (values_curr[i]-mean_curr)*(values_curr[i]-mean_curr); } return numerator / sqrt( demoniator1*demoniator2+1e-10 ); // 防止分母出现零 }
2.2.3）极线搜索-块匹配 实际实现
a: 计算参考帧p1的相机坐标（归一化相机平面）
b：根据初始的深度信息，结合运动信息，得到在当前帧下的像素坐标p2
c: 根据初始深度信息的方差信息，得到深度信息的可能范围（正负3倍sigma)，投影到当前帧得到极线的信息（极线的方向，投影点在极线上的可能的长度（实际对极线的长度进行限制，避免过长））
d: 在初始值附近，按照极线的方向，在可能长度的范围内，逐点进行图像块相似性计算，记录在搜索曲线内相似性最大的点和对应的值（注意搜索时步进量的设置，代码中设置sqrt(2)）
e: 相似性门限值检测
注意： 当前帧中，点的坐标步进以后可能不是整数，需要进行内插
// 双线性灰度插值 inline double getBilinearInterpolatedValue( const Mat& img, const Vector2d& pt ) { uchar* d = & img.data[ int(pt(1,0))*img.step+int(pt(0,0)) ]; double xx = pt(0,0) - floor(pt(0,0)); double yy = pt(1,0) - floor(pt(1,0)); return (( 1-xx ) * ( 1-yy ) * double(d[0]) + xx* ( 1-yy ) * double(d[1]) + ( 1-xx ) *yy* double(d[img.step]) + xx*yy*double(d[img.step+1]))/255.0; }
代码如下：
// 极线搜索 // 方法见书 13.2 13.3 两节 bool epipolarSearch( const Mat& ref, const Mat& curr, const SE3& T_C_R, const Vector2d& pt_ref, const double& depth_mu, const double& depth_cov, Vector2d& pt_curr ) { Vector3d f_ref = px2cam( pt_ref ); f_ref.normalize(); Vector3d P_ref = f_ref*depth_mu; // 参考帧的 P 向量 Vector2d px_mean_curr = cam2px( T_C_R*P_ref ); // 按深度均值投影的像素 double d_min = depth_mu-3*depth_cov, d_max = depth_mu+3*depth_cov; if ( d_min<0.1 ) d_min = 0.1; Vector2d px_min_curr = cam2px( T_C_R*(f_ref*d_min) ); // 按最小深度投影的像素 Vector2d px_max_curr = cam2px( T_C_R*(f_ref*d_max) ); // 按最大深度投影的像素 Vector2d epipolar_line = px_max_curr - px_min_curr; // 极线（线段形式） Vector2d epipolar_direction = epipolar_line; // 极线方向 epipolar_direction.normalize(); double half_length = 0.5*epipolar_line.norm(); // 极线线段的半长度 if ( half_length>100 ) half_length = 100; // 我们不希望搜索太多东西 // 取消此句注释以显示极线（线段） showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr ); // 在极线上搜索，以深度均值点为中心，左右各取半长度 double best_ncc = -1.0; Vector2d best_px_curr; for ( double l=-half_length; l<=half_length; l+=0.7 ) // l+=sqrt(2) { Vector2d px_curr = px_mean_curr + l*epipolar_direction; // 待匹配点 if ( !inside(px_curr) ) continue; // 计算待匹配点与参考帧的 NCC double ncc = NCC( ref, curr, pt_ref, px_curr ); if ( ncc>best_ncc ) { best_ncc = ncc; best_px_curr = px_curr; } } if ( best_ncc < 0.85f ) // 只相信 NCC 很高的匹配 return false; pt_curr = best_px_curr; return true; }
2.3） 高斯分布的深度滤波器
根据极线搜索确定了当前帧中的像素坐标点（可能含有误差），然后通过三角化计算新的深度信息，并量化新的深度信息的不确定度（量测噪声，受极线搜索得到带有误差的像素坐标点影响），然后和前一时刻的深度信息进行融合。
2.3.1）三角化测量
三角化最早由高斯提出，并应用于测量学中。
原理：在不同的位置观测同一个三维点P，已知在不同位置处观测到的三维点的二维投影点x1,x2，利用三角关系，恢复出三维点的深度信息z.
在SLAM中深度求解的方式常用的有三种，如下：
a: 克莱默法则求解方法
代码如下：
归一化坐标解释：
利用Cramer's法则求解得到s1和s2后，得到s1的过程需要注意（分别利用了s1和s2的信息恢复出相机坐标p，在求模得到深度信息）
// 用三角化计算深度 SE3 T_R_C = T_C_R.inverse(); Vector3d f_ref = px2cam( pt_ref ); f_ref.normalize(); Vector3d f_curr = px2cam( pt_curr ); f_curr.normalize(); // 方程 // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC // => [ f_ref^T f_ref, -f_ref^T f_cur ] [d_ref] = [f_ref^T t] // [ f_cur^T f_ref, -f_cur^T f_cur ] [d_cur] = [f_cur^T t] // 二阶方程用克莱默法则求解并解之 Vector3d t = T_R_C.translation(); Vector3d f2 = T_R_C.rotation_matrix() * f_curr; Vector2d b = Vector2d ( t.dot ( f_ref ), t.dot ( f2 ) ); double A[4]; A[0] = f_ref.dot ( f_ref ); A[2] = f_ref.dot ( f2 ); A[1] = -A[2]; A[3] = - f2.dot ( f2 ); double d = A[0]*A[3]-A[1]*A[2]; Vector2d lambdavec = Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ), -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d; Vector3d xm = lambdavec ( 0,0 ) * f_ref; Vector3d xn = t + lambdavec ( 1,0 ) * f2; Vector3d d_esti = ( xm+xn ) / 2.0; // 三角化算得的深度向量 double depth_estimation = d_esti.norm(); // 深度值
附： Cramer's法则如下：
b: 利用叉乘消元进行求解
c: 利用Mid Point Method 进行求解(Hartley 大名鼎鼎的《Mutiple View Geometry》中有讲解)
2.3.2）深度滤波器
对深度的分布假设存在着若干种不同的做法。首先，在比较简单的假设条件下，可以假设深度值服从高斯分布，得到一种类卡尔曼式的方法；而另一些文献中，亦采用了均匀-高斯混合分布的假设，推导了另一种形式更为复杂的深度滤波器。
高斯分布假设下的滤波器实现：
a: 设某个像素点的深度d服从：
b: 当新的数据到来，通过计算得到其深度信息，同时包括不确定性结果，假设这次观测亦是一个高斯分布：
c： 使用观测的信息更新原先d的分布（两个高斯分布的乘积依然是一个高斯分布），则假设融合后d的分布为
,那么根据高斯分布的乘积，有：
d: 代码如下：
// 高斯融合 double mu = depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ]; double sigma2 = depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ]; double mu_fuse = (d_cov2*mu+sigma2*depth_estimation) / ( sigma2+d_cov2); double sigma_fuse2 = ( sigma2 * d_cov2 ) / ( sigma2 + d_cov2 ); depth.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = mu_fuse; depth_cov.ptr<double>( int(pt_ref(1,0)) )[ int(pt_ref(0,0)) ] = sigma_fuse2;
2.3.3) 量测信息方差确定
关于量测信息的计算亦有一些不同的处理方式。有些文献考虑了几何不确定性和光度不确定性二者之和，而有些文献仅考虑了几何不确定性，下面以仅考虑几何不确定性来进行计算：
a: 首先通过平移信息（t的大小影响三角测量的精度，三角测量矛盾），根据相机坐标系下的坐标（其中涉及特征点深度信息），计算角度值
：
b: 对p2扰动一个像素，将使得
产生一个变化量
,由于相机的焦距为f,于是：
所以:
c: 根据正弦定理，可计算
的大小如下：
d: 由此可以确定由单个像素的不确定引起的深度不确定性。如果认为极线搜索的块匹配仅有一个像素的误差，那么就可以设：
e: 代码如下：
// 计算不确定性（以一个像素为误差） Vector3d p = f_ref*depth_estimation; Vector3d a = p - t; double t_norm = t.norm(); double a_norm = a.norm(); double alpha = acos( f_ref.dot(t)/t_norm ); double beta = acos( -a.dot(t)/(a_norm*t_norm)); double beta_prime = beta + atan(1/fx); double gamma = M_PI - alpha - beta_prime; double p_prime = t_norm * sin(beta_prime) / sin(gamma); double d_cov = p_prime - depth_estimation; double d_cov2 = d_cov*d_cov;
注意：如果极线搜索的不确定性大于一个像素，亦可按照此推导放大这个不确定性。
实际工程中，当不确定性小于一定阈值之后，就可认为深度数据已经收敛。（实际当中，有些像素点可能观测比较好，其深度和平移t比较合适，量测噪声小，因此收敛比较块，收敛以后这部分像素点可以作为已知位置点，而对于其余的像素点，则需要继续深度滤波，直到观测比较好时，才能收敛）
2.4） 单目稠密深度估计流程
a: 假设所有像素的深度满足某个初始的高斯分布
b：当新的数据产生时，遍历所有待估计深度的像素点，通过极线搜索和块匹配确定投影点位置；
c: 根据几何关系计算三角化后的深度及不确定性
d: 将当前观测融合进上一次的估计中。检测方差是否收敛，停止解算该像素点深度，否则继续。
// 第一张图 Mat ref = imread( color_image_files[0], 0 ); // gray-scale image SE3 pose_ref_TWC = poses_TWC[0]; double init_depth = 3.0; // 深度初始值 double init_cov2 = 3.0; // 方差初始值 Mat depth( height, width, CV_64F, init_depth ); // 深度图 Mat depth_cov( height, width, CV_64F, init_cov2 ); // 深度图方差 for ( int index=1; index<color_image_files.size(); index++ ) { cout<<"*** loop "<<index<<" ***"<<endl; Mat curr = imread( color_image_files[index], 0 ); if (curr.data == nullptr) continue; SE3 pose_curr_TWC = poses_TWC[index]; SE3 pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC; // 坐标转换关系： T_C_W * T_W_R = T_C_R update( ref, curr, pose_T_C_R, depth, depth_cov ); plotDepth( depth ); imshow("image", curr); waitKey(1); } cout<<"estimation returns, saving depth map ..."<<endl; imwrite( "depth.png", depth ); cout<<"done."<<endl;
// 根据新的输入图像 对整个深度图进行更新 bool update(const Mat& ref, const Mat& curr, const SE3& T_C_R, Mat& depth, Mat& depth_cov ) { //#pragma omp parallel for for ( int x=boarder; x<width-boarder; x++ ) //#pragma omp parallel for for ( int y=boarder; y<height-boarder; y++ ) { // 遍历每个像素 if ( depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov ) // 深度已收敛或发散 continue; // 在极线上搜索 (x,y) 的匹配 Vector2d pt_curr; bool ret = epipolarSearch ( ref, curr, T_C_R, Vector2d(x,y), depth.ptr<double>(y)[x], sqrt(depth_cov.ptr<double>(y)[x]), pt_curr ); if ( ret == false ) // 匹配失败 continue; // 取消该注释以显示匹配 showEpipolarMatch( ref, curr, Vector2d(x,y), pt_curr ); // 匹配成功，更新深度图 updateDepthFilter( Vector2d(x,y), pt_curr, T_C_R, depth, depth_cov ); } }
结果如下：（上为初始估计的深度信息，下为迭代 35 轮观测以后估计的深度结果）
结果分析：
当迭代次数超过一定值以后，深度图趋于稳定，不再对新的数据产生改变。
稳定后的结果，整个估计大部分是正确的，但也存在这大量错误的估计
（表现为深度图中与周围的数据不一致的地方，为过大或过小的估计）
（位于边缘处的地方，由于运动过程中看到的次数比较少，亦没有得到正确的估计）
2.5） 实验分析与讨论
由于真实数据的复杂性，需要周密的考虑和大量的工程技巧。
2.5.1） 像素梯度的问题
a: 有明显梯度的小块将具有良好的区分度，不易引起误匹配；对于梯度不明显的像素，由于在块匹配时没有区分性，所以难以有效的估计其深度。对于单目还是双目，立体视觉的重建质量十分依赖于环境纹理——该问题是无法在现有的算法流程上加以改进并解决的（如果依然只关心单个像素周围的邻域小块）。
b: 当像素梯度与极线夹角较大时，极线匹配的不确定性大；而当夹角较小时，匹配的不确定性变小——考虑像素梯度和极线的关系，采用更精确的不确定性模型
2.5.2） 逆深度
a: 像素坐标u,v及深度信息d，三维坐标x,y,z不同的参数化形式
b: 逆深度
原有的高斯模型假设：室外应用，初始值难以涵盖比较远的点，用高斯模型分布描述会有一些数值计算上的困难。
假设逆深度，即深度的倒数为高斯分布是比较有效的。在实际应用中，具有更好的数值稳定性，成为一种通用的做法，存在于现有的SLAM方案中的标准做法中。
2.5.3） 图像间的变换
在进行块匹配时，假设图像小块在相机运动是保持不变，但是这个假设在相机平移时能够保持成立，在相机发生纯旋转时无法成立——根据仿射变换矩阵，把当前帧（或参考帧）的像素进行变换后再进行块匹配，以期获得对旋转更好的效果。
2.5.4） 并行化：效率的问题
利用GPU的稠密深度估计是可以在主流GPU上实时化的（每个像素深度计算可以并行执行）
2.5.5） 其他的改进
a: 假设深度图中相邻的深度变化不会太大，从而给深度估计加上了空间正则项——使得到的深度图更加平滑
b: 由于遮挡、光照、运动模糊等各种因素影响，需要考虑错误匹配的处理——显示地将内点与外点进行区别并进行概率建模，能够较好的处理外点数据。
2.6） 总结
存在理论上的困难（环境纹理的依赖，像素梯度与极线方向平行等情况），很难通过调整代码实现来解决。
截止目前，尽管双目和移动单目能够建立稠密的地图，但通常认为它们过去依赖环境纹理和光照，不够可靠。
3. RGB-D稠密建图
RGB-D稠密建图是相对容易的——利用传感器测量深度信息，无须消耗大量的计算资源，避免对纹理环境的依赖
3.1 点云地图
用一组离散的点表示的地图，最基本的点包含三维坐标，也可以带有r,g,b的彩色信息。
利用RGB-D提供的彩色图和深度图，根据内计算RGB-D点云
如果得到相机的位姿，只要直接把点云进行加和，就可以获得全局的点云。
程序实现：
在实际处理中，会对点云加一些滤波处理，以获得更好的视觉效果
a: 在生成每一帧点云时，去掉深度值太大或无效的点（考虑到Kinect的有效量程，超过量程误差比较大）
b: 对每一帧生成的点云，利用统计滤波器方法去除孤立点（统计每个点与它距离最近的N个点的距离值分布，去除距离均值过大的点，去掉孤立的噪声点）
c: 对之后生成的整体的点云，利用体素滤波器（Voxel Filter）进行降采样（由于多个视角存在视野重叠，在重叠区域存在大量位置十分接近的点，体素滤波器确保在某个一定大小的立方体（或称体素）内仅有一个点，相当于对三维空间进行了降采样，从而节省了内存空间）
#include <iostream> #include <fstream> using namespace std; #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <Eigen/Geometry> #include <boost/format.hpp> // for formating strings #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/voxel_grid.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/filters/statistical_outlier_removal.h> int main( int argc, char** argv ) { vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图 vector<Eigen::Isometry3d> poses; // 相机位姿 ifstream fin("./data/pose.txt"); if (!fin) { cerr<<"cannot find pose file"<<endl; return 1; } for ( int i=0; i<5; i++ ) { boost::format fmt( "./data/%s/%d.%s" ); //图像文件格式 colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() )); depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像 double data[7] = {0}; for ( int i=0; i<7; i++ ) { fin>>data[i]; } Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); Eigen::Isometry3d T(q); T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] )); poses.push_back( T ); } // 计算点云并拼接 // 相机内参 double cx = 325.5; double cy = 253.5; double fx = 518.0; double fy = 519.0; double depthScale = 1000.0; cout<<"正在将图像转换为点云..."<<endl; // 定义点云使用的格式：这里用的是XYZRGB typedef pcl::PointXYZRGB PointT; typedef pcl::PointCloud<PointT> PointCloud; // 新建一个点云 PointCloud::Ptr pointCloud( new PointCloud ); for ( int i=0; i<5; i++ ) { PointCloud::Ptr current( new PointCloud ); cout<<"转换图像中: "<<i+1<<endl; cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i]; Eigen::Isometry3d T = poses[i]; for ( int v=0; v<color.rows; v++ ) for ( int u=0; u<color.cols; u++ ) { unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值 if ( d==0 ) continue; // 为0表示没有测量到 if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉 Eigen::Vector3d point; point[2] = double(d)/depthScale; point[0] = (u-cx)*point[2]/fx; point[1] = (v-cy)*point[2]/fy; Eigen::Vector3d pointWorld = T*point; PointT p ; p.x = pointWorld[0]; p.y = pointWorld[1]; p.z = pointWorld[2]; p.b = color.data[ v*color.step+u*color.channels() ]; p.g = color.data[ v*color.step+u*color.channels()+1 ]; p.r = color.data[ v*color.step+u*color.channels()+2 ]; current->points.push_back( p ); } // depth filter and statistical removal PointCloud::Ptr tmp ( new PointCloud ); pcl::StatisticalOutlierRemoval<PointT> statistical_filter; statistical_filter.setMeanK(50); statistical_filter.setStddevMulThresh(1.0); statistical_filter.setInputCloud(current); statistical_filter.filter( *tmp ); (*pointCloud) += *tmp; } pointCloud->is_dense = false; cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl; // voxel filter pcl::VoxelGrid<PointT> voxel_filter; voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution 体素滤波器以后的点云地图分辨啦（参数含义为每立方厘米有一个点） PointCloud::Ptr tmp ( new PointCloud ); voxel_filter.setInputCloud( pointCloud ); voxel_filter.filter( *tmp ); tmp->swap( *pointCloud ); cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl; pcl::io::savePCDFileBinary("map.pcd", *pointCloud ); return 0; }
结果及分析;
正在将图像转换为点云... 转换图像中: 1 转换图像中: 2 转换图像中: 3 转换图像中: 4 转换图像中: 5 点云共有895060个点. 滤波之后，点云共有448175个点.
a: 由于滤波方法的实现，左（未加滤波）右图中可以看到异常点的差别
b: 体素滤波器，可以看到点云数量明显减少，且在当前分辨率参数下，视觉上感受不到差异。
点云总结：
可以有RGB-D图像高效生成，不需要额外的处理
点云地图是“基础的”或“初级的”，更接近于传感器读取的原始数据，。具有一些基本功能，通常用于调试和基本的显示，不变直接用于应用程序
从点云地图出发，可构建占据网格地图，以供导航算法查询是否可以通过；利用泊松重建方法，通过基本的点云重建物体的网格地图，得到物体的表面信息。
也可通过Surfel表达物体的表面，以面元作为地图的基本单位。
3.2 八叉树地图
在导航中比较常用，本身具有较好的压缩性能，灵活的，又能随时更新的地图形式（相比点云，规模大，无法处理运动物体的特点）
3.2.1） 八叉树相比于点云地图节省空间解释：
在八叉树地图中，节点存储它是否被占据的信息，然而，当某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点。实际当中，由于物体经常连在一起，空白的地方也会常常连在一起，所以大多数八叉树节点无须展开到叶子层面。
3.2.2） 八叉树节点存储是否被占据信息表达
采用概率对数值（Log-odds）动态的建模了地图中的障碍物信息。变换公式为：
其中，x表示占据概率0~1，y为存储的概率对数值。
当y=0时，对应x为0.5，随着不断观测到“占据”时，y值不断增大，此时x逐渐增大，接近于1。
当查询某点是否被占据时，读取节点值y,通过反变换得到概率信息。
3.2.3） 八叉树更新规律
在深度值对应的空间点上观察到了一个占据数据，并且，从相机光心出发到这个点的线段上，应该是没有物体的（否则会被遮挡）。
3.2.4） 实现
a: octomap 提供了许多种八叉树：带地图的，带占据信息的，亦可自定义节点携带哪些信息；
此处使用最基本的八叉树地图（octomap::OcTree）
b: 先生成点云信息，然后讲点云信息存入八叉树地图，并给相机光心位置信息（使用octomap提供的内部点云结构，相比PCL点云信息稍微简单，只携带点的空间位置信息）
c: 根据存储的点云信息，更新内部的占据概率，最后保存为压缩后的八叉树地图。
d: 使用可视化程序octovis查看存储的八叉树地图文件.bt。
e: 在可视化程序中，可调节地图的分辨率
f: 提供额外功能： 方便地查询任意点的占据概率，以此设计在地图中进行导航的方法
g: 利用相同的图片信息生成的文件，八叉树地图文件大小明显小于上一小节生成的点云文件，可有效的建模较大的场景
// 计算点云并拼接 // 相机内参 double cx = 325.5; double cy = 253.5; double fx = 518.0; double fy = 519.0; double depthScale = 1000.0; cout<<"正在将图像转换为 Octomap ..."<<endl; // octomap tree octomap::OcTree tree( 0.05 ); // 参数为分辨率 for ( int i=0; i<5; i++ ) { cout<<"转换图像中: "<<i+1<<endl; cv::Mat color = colorImgs[i]; cv::Mat depth = depthImgs[i]; Eigen::Isometry3d T = poses[i]; octomap::Pointcloud cloud; // the point cloud in octomap for ( int v=0; v<color.rows; v++ ) for ( int u=0; u<color.cols; u++ ) { unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值 if ( d==0 ) continue; // 为0表示没有测量到 if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉 Eigen::Vector3d point; point[2] = double(d)/depthScale; point[0] = (u-cx)*point[2]/fx; point[1] = (v-cy)*point[2]/fy; Eigen::Vector3d pointWorld = T*point; // 将世界坐标系的点放入点云 cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] ); } // 将点云存入八叉树地图，给定原点，这样可以计算投射线 tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) ); } // 更新中间节点的占据信息并写入磁盘 tree.updateInnerOccupancy(); cout<<"saving octomap ... "<<endl; tree.writeBinary( "octomap.bt" );
4. TSDF地图和Fusion系列
4.1） 之前地图模型总结
4.1.1）以定位为主体，地图拼接作为后续加工步骤存在于SLAM框架中。该框架成为主流的原因：定位算法可以满足实时性要求，而地图的加工可以在关键帧处进行处理，无须实时响应。
4.1.2） 并没有对稠密地图进行优化——“鬼影”现象
4.2）实时三维重建
希望重建的地图结果是光滑的，完整的，符合对地图的认知，基于此，出现了一种以“建图”为主体，而定位处于次要地位的的做法——实时三维重建
4.2.1）特点：
由于三维重建把重建准确地图作为主要目标，所以基本都需要利用GPU进行加速，甚至需要非常高级的GPU或多个GPU进行并行加速。
SLAM是朝轻量级，小型化方向发展，有些方案甚至放弃了建图和回环检测部分，只保留了视觉里程计；而实施重建在朝大规模、大型动态场景的重建方向发展。
利用RGB-D图像进行实时重建形成了一个重要的发展方向陆续产生了Kinect Fusion，Dynamic Fusion， Elastic Fusion， Fusion4D，Volumn Deform等成果。
4.3） TSDF（Truncated Signed Distance Function）地图
是一种网格式的地图。
参考链接