主要內容
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）地圖
是一種網格式的地圖。
參考鏈接