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進行加速。