前言
大家好，時隔多年之后，我又開始了博客旅程。經歷了很多事情之后呢，我發現自己的想法真的很簡單：好好讀書做課題，閑下來時寫寫博客，服務大家。所以我會繼續寫SLAM相關的博客。如果你覺得它對你有幫助，那是最好不過的啦！寫作過程中得到了許多熱心讀者的幫助與鼓勵，有些讀者還成了要好的朋友，在此向大家致謝啦！關於SLAM，讀者也會有很多問題。由於我個人精力和學力都有限，無法一一回答，向大家說聲抱歉！有些共同的問題，我肯定會在博客里介紹的！
前兩天剛從珠海開會回來，與中山大學的同學們聚在一起玩耍，很開心！
《一起做》系列已經結束，事實上它是我以前探索過程中的一些總結。雖然仍然有很多令人不滿意的地方，不過相信讀了那個系列，讀者應該對SLAM的流程有一定的了解了。尤其是通過代碼，你能知道許多論文里沒講清楚的細節。在這之后，我現在有兩個規划。一是對目前流行的SLAM程序做一個介紹，沿着《視覺SLAM實戰》往下寫；二是介紹一些好用的開源工具/庫，寫成一個《SLAM拾萃》。我覺得這兩部分內容，對讀者了解SLAM會有較大的幫助。當然，如果你對我的博客有任何建議，可以在下方評論或給我發郵件。
本篇是《SLAM拾萃》第一篇，介紹一個建圖工具：octomap。和往常一樣，我會介紹它的原理、安裝與使用方式，並提供例程供讀者學習。必要時也會請小蘿卜過來吐槽。（小蘿卜真是太好用了，它可以代替讀者提很多問題。）
什么是octomap？
RGBD SLAM的目的有兩個：估計機器人的軌跡，並建立正確的地圖。地圖有很多種表達方式，比如特征點地圖、網格地圖、拓撲地圖等等。在《一起做》系列中，我們使用的地圖形式主要是點雲地圖。在程序中，我們根據優化后的位姿，拼接點雲，最后構成地圖。這種做法很簡單，但有一些明顯的缺陷：
- 地圖形式不緊湊。
點雲地圖通常規模很大，所以一個pcd文件也會很大。一張640$\times$480的圖像，會產生30萬個空間點，需要大量的存儲空間。即使經過一些濾波之后，pcd文件也是很大的。而且討厭之處在於，它的“大”並不是必需的。點雲地圖提供了很多不必要的細節。對於地毯上的褶皺、陰暗處的影子，我們並不特別關心這些東西。把它們放在地圖里是浪費空間。 - 處理重疊的方式不夠好。
在構建點雲時，我們直接按照估計位姿拼在了一起。在位姿存在誤差時，會導致地圖出現明顯的重疊。例如一個電腦屏變成了兩個，原本方的邊界變成了多邊形。對重疊地區的處理方式應該更好一些。 - 難以用於導航
說起地圖的用處，第一就是導航啦！有了地圖，就可以指揮機器人從A點到B點運動，豈不是很方便的事？但是，給你一張點雲地圖，是否有些傻眼了呢？我至少得知道哪些地方可通過，哪些地方不可通過，才能完成導航呀！光有點是不夠的！
octomap就是為此而設計的！親，你沒有看錯，它可以優雅地壓縮、更新地圖，並且分辨率可調！它以八叉樹（octotree，后面會講）的形式存儲地圖，相比點雲，能夠省下大把的空間。octomap建立的地圖大概是這樣子的：（從左到右是不同的分辨率）
由於八叉樹的原因，它的地圖像是很多個小方塊組成的（很像minecraft）。當分辨率較高時，方塊很小；分辨率較低時，方塊很大。每個方塊表示該格被占據的概率。因此你可以查詢某個方塊或點“是否可以通過”，從而實現不同層次的導航。簡而言之，環境較大時采用較低分辨率，而較精細的導航可采用較高分辨率。
小蘿卜：師兄你這是介紹嗎？真像廣告啊……
octomap原理
本段會講一些數學知識。如果你想“跑跑程序看效果”，可以跳過本段。
- 八叉樹的表達
八叉樹，也就是傳說中有八個子節點的樹！是不是很厲害呢？至於為什么要分成八個子節點，想象一下一個正方形的方塊的三個面各切一刀，不就變成八塊了嘛！如果你想象不出來，請看下圖：
實際的數據結構呢，就是一個樹根不斷地往下擴，每次分成八個枝，直到葉子為止。葉子節點代表了分辨率最高的情況。例如分辨率設成0.01m，那么每個葉子就是一個1cm見方的小方塊了呢！
每個小方塊都有一個數描述它是否被占據。在最簡單的情況下，可以用0－1兩個數表示（太簡單了所以沒什么用）。通常還是用0～1之間的浮點數表示它被占據的概率。0.5表示未確定，越大則表示被占據的可能性越高，反之亦然。由於它是八叉樹，那么一個節點的八個孩子都有一定的概率被占據或不被占據啦！（下圖是一棵八叉樹）
用樹結構的好處時：當某個節點的子結點都“占據”或“不占據”或“未確定”時，就可以把它給剪掉！換句話說，如果沒必要進一步描述更精細的結構（孩子節點）時，我們只要一個粗方塊（父節點）的信息就夠了。這可以省去很多的存儲空間。因為我們不用存一個“全八叉樹”呀！
2. 八叉樹的更新
在八叉樹中，我們用概率來表達一個葉子是否被占據。為什么不直接用0－1表達呢？因為在對環境的觀測過程中，由於噪聲的存在，某個方塊有時可能被觀測到是“占據”的，過了一會兒，在另一些方塊中又是“不占據”的。有時“占據”的時候多，有時“不占據”的時候多。這一方面可能是由於環境本身有動態特征（例如桌子被挪走了），另一方面（多數時候）可能是由於噪聲。根據八叉樹的推導，假設$t＝1，\ldots, T$時刻，觀測的數據為$z_1, \ldots, z_T$，那么第$n$個葉子節點記錄的信息為：$$P(n|z_{1:T}) = \left[ 1+ \frac{1-P(n|z_T)}{P(n|z_T)} \frac{1-P(n|z_{1:T-1})}{P(n|z_{1:T-1})} \frac{P(n)}{1-P(n)} \right]^{-1} \quad \quad (1)$$
小蘿卜：哇！又一個好長的式子！這說的是啥師兄？
師兄：哎，寫論文非得把一些簡單的事情寫得很復雜。為了解釋這東西，先講一下 logit 變換。該變換把一個概率$p$變換到全實數空間$R$上： $$ \alpha = logit(p) = log \left( \frac{p}{1-p} \right) $$
這是一個可逆變換，反之有： $$ p = logit^{-1} (\alpha) = \frac{1} { 1+ exp(-\alpha) }. $$
$\alpha$叫做log-odds。我們把用$L()$葉子節點的log-odds，那么(1)就可以寫成： $$ L( n| z_{1:T} ) = L(n|z_{1:T-1}) + L(n|z_T) $$
小蘿卜：哦！這個我就懂了！每新來一個就直接加到原來的上面，是吧？
師兄：對，此外還要加一個最大最小值的限制。最后轉換回原來的概率即可。
八叉樹中的父親節點占據概率，可以根據孩子節點的數值進行計算。比較簡單的是取平均值或最大值。如果把八叉樹按照占據概率進行渲染，不確定的方塊渲染成透明的，確定占據的渲染成不透明的，就能看到我們平時見到的那種東西啦！
octomap本身的數學原理還是簡單的。不過它的可視化做的比較好。下面我們來講講如何下載、安裝八叉樹程序，並給出幾個小的例程。
安裝octomap
octomap的網頁見：https://octomap.github.io
它的github源碼在：https://github.com/OctoMap/octomap
它還有ROS下的安裝方式：http://wiki.ros.org/octomap
在開發過程中，可能需要不斷地查看它的API文檔。你可以自己用doxygen生成一個，或者查看在線文檔：http://octomap.github.io/octomap/doc/
為了保持簡潔，我們不要求讀者安裝ROS，僅介紹單獨的octomap。我的編譯環境是ubuntu 14.04。ubuntu系列的應該都不會有太大問題。
1. 編譯octomap
新建一個目錄，拷貝octomap代碼。如果沒有git請安裝git：sudo apt-get install git
-
git clone https://github.com/OctoMap/octomap
git會把代碼拷貝到當前目錄/octomap下。進入該目錄，參照README.md進行安裝。編譯方式和普通的cmake程序一樣，如果你學過《一起做》就應該很熟悉了：
1 mkdir build 2 cd build 3 cmake .. 4 make
事實上，octomap的代碼主要含兩個模塊：本身的octomap和可視化工具octovis。octovis依賴於qt4和qglviewer，所以如果你沒有裝這兩個依賴，請安裝它們：sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev
如果編譯沒有給出任何警告，恭喜你編譯成功！
- 使用octovis查看示例地圖
在bin/文件夾中，存放着編譯出來可執行文件。為了直觀起見，我們直接看一個示例地圖：
bin/octovis octomap/share/data/geb079.bt
octovis會打開這個地圖並顯示。它的UI是長這樣的。你可以玩玩菜單里各種東西（雖然也不多，我就不一一介紹UI怎么玩了），能看出這是一層樓的掃描圖。octovis是一個比較實用的工具，你生成的各種octomap地圖都可以用它來看。（所以你可以把octovis放到/usr/local/bin/下，省得以后還要找。）
例程1：轉換pcd到octomap
GUI玩夠了吧？僅僅會用UI是不夠滴，現在讓我們開始編代碼使用octomap這個庫吧！
我為你准備了三個小例程。在前兩個中，我會教你如何將一個pcd格式的點雲地圖轉換為octomap地圖。后一個中，我會講講如何根據g2o優化的軌跡，以類似slam的方式，把幾個RGBD圖像拼接出一個octomap。這對你研究SLAM會有一些幫助。所有的代碼與數據都可以在我的github上找到。有關編譯的信息，我寫在這個代碼的Readme中了，請在編譯前看一眼如何編譯這些代碼。
源代碼如下：src/pcd2octomap.cpp 這份代碼將命令行參數1作為輸入文件，參數2作為輸出文件，把輸入的pcd格式點雲轉換成octomap格式的點雲。通過這個例子，你可以學會如何創建一個簡單的OcTree對象並往里面添加新的點。
1 #include <iostream> 2 #include <assert.h> 3 4 //pcl 5 #include <pcl/io/pcd_io.h> 6 #include <pcl/point_types.h> 7 8 //octomap 9 #include <octomap/octomap.h> 10 using namespace std; 11 12 int main( int argc, char** argv ) 13 { 14 if (argc != 3) 15 { 16 cout<<"Usage: pcd2octomap <input_file> <output_file>"<<endl; 17 return -1; 18 } 19 20 string input_file = argv[1], output_file = argv[2]; 21 pcl::PointCloud<pcl::PointXYZRGBA> cloud; 22 pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud ); 23 24 cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl; 25 26 //聲明octomap變量 27 cout<<"copy data into octomap..."<<endl; 28 // 創建八叉樹對象，參數為分辨率，這里設成了0.05 29 octomap::OcTree tree( 0.05 ); 30 31 for (auto p:cloud.points) 32 { 33 // 將點雲里的點插入到octomap中 34 tree.updateNode( octomap::point3d(p.x, p.y, p.z), true ); 35 } 36 37 // 更新octomap 38 tree.updateInnerOccupancy(); 39 // 存儲octomap 40 tree.writeBinary( output_file ); 41 cout<<"done."<<endl; 42 43 return 0; 44 }
這個代碼是相當直觀的。在編譯之后，它會產生一個可執行文件，叫做pcd2octomap，放在代碼根目錄的bin/文件夾下。你可以在代碼根目錄下這樣調：
1 bin/pcd2octomap data/sample.pcd data/sample.bt
它會把data文件夾下的sample.pcd（一個示例pcd點雲），轉換成一個data/sample.bt的octomap文件。你可以比較下pcd點雲與octomap的區別。下圖是分別調用這些顯示命令的結果。
1 pcl_viewer data/sample.pcd 2 octovis data/sample.ot
這個octomap里只存儲了點的空間信息，而沒有顏色信息。我按照高度給它染色了，否則它應該就是灰色的。通過octomap，我們能查看每個小方塊是否可以通行，從而實現導航的工作。
以下是對代碼的一些注解：
注1：有關如何讀取pcd文件，你可以參見pcl官網的tutorial。不過這件事情十分簡單，所以我相信你也能直接看懂。
注2：31行采用了C++11標准的for循環，它會讓代碼看起來稍微簡潔一些。如果你的編譯器比較老而不支持c++11，你可以自己將它改成傳統的for循環的樣式。
注3：octomap存儲的文件后綴名是.bt（二進制文件）和.ot（普通文件），前者相對更小一些。不過octomap文件普遍都很小，所以也不差這么些容量。如果你存成了其他后綴名，octovis可能認不出來。
例程2：加入色彩信息
第一個示例中，我們將pcd點雲轉換為octomap。但是pcd點雲是有顏色信息的，能否在octomap中也保存顏色信息呢？答案是可以的。octomap提供了ColorOcTree類，能夠幫你存儲顏色信息。下面我們就來做一個保存顏色信息的示例。代碼見：src/pcd2colorOctomap.cpp
1 #include <iostream> 2 #include <assert.h> 3 4 //pcl 5 #include <pcl/io/pcd_io.h> 6 #include <pcl/point_types.h> 7 8 //octomap 9 #include <octomap/octomap.h> 10 #include <octomap/ColorOcTree.h> 11 12 using namespace std; 13 14 int main( int argc, char** argv ) 15 { 16 if (argc != 3) 17 { 18 cout<<"Usage: pcd2colorOctomap <input_file> <output_file>"<<endl; 19 return -1; 20 } 21 22 string input_file = argv[1], output_file = argv[2]; 23 pcl::PointCloud<pcl::PointXYZRGBA> cloud; 24 pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud ); 25 26 cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl; 27 28 //聲明octomap變量 29 cout<<"copy data into octomap..."<<endl; 30 // 創建帶顏色的八叉樹對象，參數為分辨率，這里設成了0.05 31 octomap::ColorOcTree tree( 0.05 ); 32 33 for (auto p:cloud.points) 34 { 35 // 將點雲里的點插入到octomap中 36 tree.updateNode( octomap::point3d(p.x, p.y, p.z), true ); 37 } 38 39 // 設置顏色 40 for (auto p:cloud.points) 41 { 42 tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b ); 43 } 44 45 // 更新octomap 46 tree.updateInnerOccupancy(); 47 // 存儲octomap, 注意要存成.ot文件而非.bt文件 48 tree.write( output_file ); 49 cout<<"done."<<endl; 50 51 return 0; 52 }
大部分代碼和剛才是一樣的，除了把OcTree改成ColorOcTree，以及調用integrateNodeColor來混合顏色之外。這段代碼會編譯出pcd2colorOctomap這個程序，完成帶顏色的轉換。不過，后綴名改成了.ot文件。
1 bin/pcd2colorOctomap data/sample.pcd data/sample.ot
顏色信息能夠更好地幫助我們辨認結果是否正確，給予一個直觀的印象。是不是好看了一些呢？
例程3：更好的拼接與轉換
前兩個例程中，我們都是對單個pcd文件進行了處理。實際做slam時，我們需要拼接很多幀的octomap。為了做這樣一個示例，我從自己的實驗數據中取出了一小段。這一小段總共含有五張圖像（因為github並不適合傳大量數據），它們存放在data/rgb_index和data/dep_index下。我的slam程序估計了這五個關鍵幀的位置，放在data/trajectory.txt中。它的格式是：幀編號 x y z qx qy qz qw （位置＋姿態四元數）。事實上它是從一個g2o文件中拷出來的。你可以用g2o_viewer data/result_after.g2o來看整個軌跡。
54 -0.228993 0.00645704 0.0287837 -0.0004327 -0.113131 -0.0326832 0.993042
144 -0.50237 -0.0661803 0.322012 -0.00152174 -0.32441 -0.0783827 0.942662
230 -0.970912 -0.185889 0.872353 -0.00662576 -0.278681 -0.0736078 0.957536
313 -1.41952 -0.279885 1.43657 -0.00926933 -0.222761 -0.0567118 0.973178
346 -1.55819 -0.301094 1.6215 -0.02707 -0.250946 -0.0412848 0.966741
現在我們要做的事，就是根據trajectory.txt里記錄的信息，把幾個RGBD圖拼成一個octomap。這也是所謂的用octomap來建圖。我寫了一個示例，不知道你能否讀懂呢？src/joinMap.cpp
1 #include <iostream> 2 #include <vector> 3 4 // octomap 5 #include <octomap/octomap.h> 6 #include <octomap/ColorOcTree.h> 7 #include <octomap/math/Pose6D.h> 8 9 // opencv 用於圖像數據讀取與處理 10 #include <opencv2/core/core.hpp> 11 #include <opencv2/imgproc/imgproc.hpp> 12 #include <opencv2/highgui/highgui.hpp> 13 14 // 使用Eigen的Geometry模塊處理3d運動 15 #include <Eigen/Core> 16 #include <Eigen/Geometry> 17 18 // pcl 19 #include <pcl/common/transforms.h> 20 #include <pcl/point_types.h> 21 22 // boost.format 字符串處理 23 #include <boost/format.hpp> 24 25 using namespace std; 26 27 // 全局變量：相機矩陣 28 // 更好的寫法是存到參數文件中，但為方便起見我就直接這樣做了 29 float camera_scale = 1000; 30 float camera_cx = 325.5; 31 float camera_cy = 253.5; 32 float camera_fx = 518.0; 33 float camera_fy = 519.0; 34 35 int main( int argc, char** argv ) 36 { 37 // 讀關鍵幀編號 38 ifstream fin( "./data/keyframe.txt" ); 39 vector<int> keyframes; 40 vector< Eigen::Isometry3d > poses; 41 // 把文件 ./data/keyframe.txt 里的數據讀取到vector中 42 while( fin.peek() != EOF ) 43 { 44 int index_keyframe; 45 fin>>index_keyframe; 46 if (fin.fail()) break; 47 keyframes.push_back( index_keyframe ); 48 } 49 fin.close(); 50 51 cout<<"load total "<<keyframes.size()<<" keyframes. "<<endl; 52 53 // 讀關鍵幀姿態 54 // 我的代碼中使用了Eigen來存儲姿態，類似的，也可以用octomath::Pose6D來做這件事 55 fin.open( "./data/trajectory.txt" ); 56 while( fin.peek() != EOF ) 57 { 58 int index_keyframe; 59 float data[7]; // 三個位置加一個姿態四元數x,y,z, w,ux,uy,uz 60 fin>>index_keyframe; 61 for ( int i=0; i<7; i++ ) 62 { 63 fin>>data[i]; 64 cout<<data[i]<<" "; 65 } 66 cout<<endl; 67 if (fin.fail()) break; 68 // 注意這里的順序。g2o文件四元數按 qx, qy, qz, qw來存 69 // 但Eigen初始化按照qw, qx, qy, qz來做 70 Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); 71 Eigen::Isometry3d t(q); 72 t(0,3) = data[0]; t(1,3) = data[1]; t(2,3) = data[2]; 73 poses.push_back( t ); 74 } 75 fin.close(); 76 77 // 拼合全局地圖 78 octomap::ColorOcTree tree( 0.05 ); //全局map 79 80 // 注意我們的做法是先把圖像轉換至pcl的點雲，進行姿態變換，最后存儲成octomap 81 // 因為octomap的顏色信息不是特別方便處理，所以采用了這種迂回的方式 82 // 所以，如果不考慮顏色，那不必轉成pcl點雲，而可以直接使用octomap::Pointcloud結構 83 84 for ( size_t i=0; i<keyframes.size(); i++ ) 85 { 86 pcl::PointCloud<pcl::PointXYZRGBA> cloud; 87 cout<<"converting "<<i<<"th keyframe ..." <<endl; 88 int k = keyframes[i]; 89 Eigen::Isometry3d& pose = poses[i]; 90 91 // 生成第k幀的點雲，拼接至全局octomap上 92 boost::format fmt ("./data/rgb_index/%d.ppm" ); 93 cv::Mat rgb = cv::imread( (fmt % k).str().c_str() ); 94 fmt = boost::format("./data/dep_index/%d.pgm" ); 95 cv::Mat depth = cv::imread( (fmt % k).str().c_str(), -1 ); 96 97 // 從rgb, depth生成點雲，運算方法見《一起做》第二講 98 // 第一次遍歷用於生成空間點雲 99 for ( int m=0; m<depth.rows; m++ ) 100 for ( int n=0; n<depth.cols; n++ ) 101 { 102 ushort d = depth.ptr<ushort> (m) [n]; 103 if (d == 0) 104 continue; 105 float z = float(d) / camera_scale; 106 float x = (n - camera_cx) * z / camera_fx; 107 float y = (m - camera_cy) * z / camera_fy; 108 pcl::PointXYZRGBA p; 109 p.x = x; p.y = y; p.z = z; 110 111 uchar* rgbdata = &rgb.ptr<uchar>(m)[n*3]; 112 uchar b = rgbdata[0]; 113 uchar g = rgbdata[1]; 114 uchar r = rgbdata[2]; 115 116 p.r = r; p.g = g; p.b = b; 117 cloud.points.push_back( p ); 118 } 119 // 將cloud旋轉之后插入全局地圖 120 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp( new pcl::PointCloud<pcl::PointXYZRGBA>() ); 121 pcl::transformPointCloud( cloud, *temp, pose.matrix() ); 122 123 octomap::Pointcloud cloud_octo; 124 for (auto p:temp->points) 125 cloud_octo.push_back( p.x, p.y, p.z ); 126 127 tree.insertPointCloud( cloud_octo, 128 octomap::point3d( pose(0,3), pose(1,3), pose(2,3) ) ); 129 130 for (auto p:temp->points) 131 tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b ); 132 } 133 134 tree.updateInnerOccupancy(); 135 tree.write( "./data/map.ot" ); 136 137 cout<<"done."<<endl; 138 139 return 0; 140 141 }
大部分需要解釋的地方，我都在程序里寫了注解。我用了一種稍微有些迂回的方式：先把圖像轉成pcl的點雲，變換后再放到octotree中。這種做法的原因是比較便於處理顏色，因為我希望做出帶有顏色的地圖。如果你不關心顏色，完全可以不用pcl，直接用octomap自帶的octomap::pointcloud來完成這件事。
insertPointCloud會比單純的插入點更好一些。octomap里的pointcloud是一種射線的形式，只有末端才存在被占據的點，中途的點則是沒被占據的。這會使一些重疊地方處理的更好。
最后，五幀數據拼接出來的點雲大概長這樣：
可能並不是特別完整，畢竟我們只用了五張圖。這些數據來自於nyud數據集的dining_room序列，一個比較完整的圖應該是這樣的：
至少是比純粹點雲好些了吧？好了，關於例程就介紹到這里。如果你准備使用octomap，這僅僅是個入門。你需要去查看它的文檔，了解它的類結構，以及一些重要類的使用、實現方式。
《SLAM拾萃》第一講，octomap，就為大家介紹到這里啦。最近我發現自己寫東西，講東西都越來越長，所以請原諒我越來越啰嗦的寫作和說話風格。希望它能幫助你！我們下講再見！
如果你覺得我的博客有幫助，可以進行幾塊錢的小額贊助，幫助我把博客寫得更好。（雖然我也是從別處學來的這招……）
小蘿卜：師兄你學壞了啊！
參考文獻
[1]. OctoMap: An efficient probabilistic 3D mapping framework based on octrees, Hornung, Armin and Wurm, Kai M and Bennewitz, Maren and Stachniss, Cyrill and Burgard, Wolfram, Autonomous Robots, 2013.
[2]. OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems, Wurm, Kai M and Hornung, Armin and Bennewitz, Maren and Stachniss, Cyrill and Burgard, Wolfram, ICRA 2010.