octomap的简介
裝載自高翔博士的博客：https://www.cnblogs.com/gaoxiang12/p/5041142.html
什么是octomap？
RGBD SLAM的目的有兩個(gè)：估計(jì)機(jī)器人的軌跡，并建立正確的地圖。地圖有很多種表達(dá)方式，比如特征點(diǎn)地圖、網(wǎng)格地圖、拓?fù)涞貓D等等。在《一起做》系列中，我們使用的地圖形式主要是點(diǎn)云地圖。在程序中，我們根據(jù)優(yōu)化后的位姿，拼接點(diǎn)云，最后構(gòu)成地圖。這種做法很簡(jiǎn)單，但有一些明顯的缺陷：
地圖形式不緊湊。
點(diǎn)云地圖通常規(guī)模很大，所以一個(gè)pcd文件也會(huì)很大。一張640×
480的圖像，會(huì)產(chǎn)生30萬(wàn)個(gè)空間點(diǎn)，需要大量的存儲(chǔ)空間。即使經(jīng)過(guò)一些濾波之后，pcd文件也是很大的。而且討厭之處在于，它的“大”并不是必需的。點(diǎn)云地圖提供了很多不必要的細(xì)節(jié)。對(duì)于地毯上的褶皺、陰暗處的影子，我們并不特別關(guān)心這些東西。把它們放在地圖里是浪費(fèi)空間。
處理重疊的方式不夠好。
在構(gòu)建點(diǎn)云時(shí)，我們直接按照估計(jì)位姿拼在了一起。在位姿存在誤差時(shí)，會(huì)導(dǎo)致地圖出現(xiàn)明顯的重疊。例如一個(gè)電腦屏變成了兩個(gè)，原本方的邊界變成了多邊形。對(duì)重疊地區(qū)的處理方式應(yīng)該更好一些。
難以用于導(dǎo)航
說(shuō)起地圖的用處，第一就是導(dǎo)航啦！有了地圖，就可以指揮機(jī)器人從A點(diǎn)到B點(diǎn)運(yùn)動(dòng)，豈不是很方便的事？但是，給你一張點(diǎn)云地圖，是否有些傻眼了呢？我至少得知道哪些地方可通過(guò)，哪些地方不可通過(guò)，才能完成導(dǎo)航呀！光有點(diǎn)是不夠的！
octomap就是為此而設(shè)計(jì)的！親，你沒(méi)有看錯(cuò)，它可以優(yōu)雅地壓縮、更新地圖，并且分辨率可調(diào)！它以八叉樹(shù)（octotree，后面會(huì)講）的形式存儲(chǔ)地圖，相比點(diǎn)云，能夠省下大把的空間。octomap建立的地圖大概是這樣子的：（從左到右是不同的分辨率）
由于八叉樹(shù)的原因，它的地圖像是很多個(gè)小方塊組成的（很像minecraft）。當(dāng)分辨率較高時(shí)，方塊很小；分辨率較低時(shí)，方塊很大。每個(gè)方塊表示該格被占據(jù)的概率。因此你可以查詢某個(gè)方塊或點(diǎn)“是否可以通過(guò)”，從而實(shí)現(xiàn)不同層次的導(dǎo)航。簡(jiǎn)而言之，環(huán)境較大時(shí)采用較低分辨率，而較精細(xì)的導(dǎo)航可采用較高分辨率。
小蘿卜：師兄你這是介紹嗎？真像廣告啊……
octomap原理
本段會(huì)講一些數(shù)學(xué)知識(shí)。如果你想“跑跑程序看效果”，可以跳過(guò)本段。
八叉樹(shù)的表達(dá)
八叉樹(shù)，也就是傳說(shuō)中有八個(gè)子節(jié)點(diǎn)的樹(shù)！是不是很厲害呢？至于為什么要分成八個(gè)子節(jié)點(diǎn)，想象一下一個(gè)正方形的方塊的三個(gè)面各切一刀，不就變成八塊了嘛！如果你想象不出來(lái)，請(qǐng)看下圖：
實(shí)際的數(shù)據(jù)結(jié)構(gòu)呢，就是一個(gè)樹(shù)根不斷地往下擴(kuò)，每次分成八個(gè)枝，直到葉子為止。葉子節(jié)點(diǎn)代表了分辨率最高的情況。例如分辨率設(shè)成0.01m，那么每個(gè)葉子就是一個(gè)1cm見(jiàn)方的小方塊了呢！
每個(gè)小方塊都有一個(gè)數(shù)描述它是否被占據(jù)。在最簡(jiǎn)單的情況下，可以用0－1兩個(gè)數(shù)表示（太簡(jiǎn)單了所以沒(méi)什么用）。通常還是用0～1之間的浮點(diǎn)數(shù)表示它被占據(jù)的概率。0.5表示未確定，越大則表示被占據(jù)的可能性越高，反之亦然。由于它是八叉樹(shù)，那么一個(gè)節(jié)點(diǎn)的八個(gè)孩子都有一定的概率被占據(jù)或不被占據(jù)啦！（下圖是一棵八叉樹(shù)）
用樹(shù)結(jié)構(gòu)的好處時(shí)：當(dāng)某個(gè)節(jié)點(diǎn)的子結(jié)點(diǎn)都“占據(jù)”或“不占據(jù)”或“未確定”時(shí)，就可以把它給剪掉！換句話說(shuō)，如果沒(méi)必要進(jìn)一步描述更精細(xì)的結(jié)構(gòu)（孩子節(jié)點(diǎn)）時(shí)，我們只要一個(gè)粗方塊（父節(jié)點(diǎn)）的信息就夠了。這可以省去很多的存儲(chǔ)空間。因?yàn)槲覀儾挥么嬉粋€(gè)“全八叉樹(shù)”呀！
2. 八叉樹(shù)的更新
在八叉樹(shù)中，我們用概率來(lái)表達(dá)一個(gè)葉子是否被占據(jù)。為什么不直接用0－1表達(dá)呢？因?yàn)樵趯?duì)環(huán)境的觀測(cè)過(guò)程中，由于噪聲的存在，某個(gè)方塊有時(shí)可能被觀測(cè)到是“占據(jù)”的，過(guò)了一會(huì)兒，在另一些方塊中又是“不占據(jù)”的。有時(shí)“占據(jù)”的時(shí)候多，有時(shí)“不占據(jù)”的時(shí)候多。這一方面可能是由于環(huán)境本身有動(dòng)態(tài)特征（例如桌子被挪走了），另一方面（多數(shù)時(shí)候）可能是由于噪聲。根據(jù)八叉樹(shù)的推導(dǎo)，假設(shè)t＝1，…,T
時(shí)刻，觀測(cè)的數(shù)據(jù)為z1,…,zT，那么第n個(gè)葉子節(jié)點(diǎn)記錄的信息為：
P(n|z1:T)=[1+1−P(n|zT)P(n|zT)1−P(n|z1:T−1)P(n|z1:T−1)P(n)1−P(n)]−1(1)
小蘿卜：哇！又一個(gè)好長(zhǎng)的式子！這說(shuō)的是啥師兄？
師兄：哎，寫(xiě)論文非得把一些簡(jiǎn)單的事情寫(xiě)得很復(fù)雜。為了解釋這東西，先講一下 logit 變換。該變換把一個(gè)概率p
變換到全實(shí)數(shù)空間R上：
α=logit(p)=log(p1−p)
這是一個(gè)可逆變換，反之有：
p=logit−1(α)=11+exp(−α).
α
叫做log-odds。我們把用L()葉子節(jié)點(diǎn)的log-odds，那么(1)就可以寫(xiě)成：
L(n|z1:T)=L(n|z1:T−1)+L(n|zT)
小蘿卜：哦！這個(gè)我就懂了！每新來(lái)一個(gè)就直接加到原來(lái)的上面，是吧？
師兄：對(duì)，此外還要加一個(gè)最大最小值的限制。最后轉(zhuǎn)換回原來(lái)的概率即可。
八叉樹(shù)中的父親節(jié)點(diǎn)占據(jù)概率，可以根據(jù)孩子節(jié)點(diǎn)的數(shù)值進(jìn)行計(jì)算。比較簡(jiǎn)單的是取平均值或最大值。如果把八叉樹(shù)按照占據(jù)概率進(jìn)行渲染，不確定的方塊渲染成透明的，確定占據(jù)的渲染成不透明的，就能看到我們平時(shí)見(jiàn)到的那種東西啦！
octomap本身的數(shù)學(xué)原理還是簡(jiǎn)單的。不過(guò)它的可視化做的比較好。下面我們來(lái)講講如何下載、安裝八叉樹(shù)程序，并給出幾個(gè)小的例程。
安裝octomap
octomap的網(wǎng)頁(yè)見(jiàn)：https://octomap.github.io
它的github源碼在：https://github.com/OctoMap/octomap
它還有ROS下的安裝方式：http://wiki.ros.org/octomap
在開(kāi)發(fā)過(guò)程中，可能需要不斷地查看它的API文檔。你可以自己用doxygen生成一個(gè)，或者查看在線文檔：http://octomap.github.io/octomap/doc/
為了保持簡(jiǎn)潔，我們不要求讀者安裝ROS，僅介紹單獨(dú)的octomap。我的編譯環(huán)境是ubuntu 14.04。ubuntu系列的應(yīng)該都不會(huì)有太大問(wèn)題。
1. 編譯octomap
新建一個(gè)目錄，拷貝octomap代碼。如果沒(méi)有g(shù)it請(qǐng)安裝git：sudo apt-get install git
git clone https://github.com/OctoMap/octomap
git會(huì)把代碼拷貝到當(dāng)前目錄/octomap下。進(jìn)入該目錄，參照README.md進(jìn)行安裝。編譯方式和普通的cmake程序一樣，如果你學(xué)過(guò)《一起做》就應(yīng)該很熟悉了：
1 mkdir build 2 cd build 3 cmake .. 4 make
事實(shí)上，octomap的代碼主要含兩個(gè)模塊：本身的octomap和可視化工具octovis。octovis依賴(lài)于qt4和qglviewer，所以如果你沒(méi)有裝這兩個(gè)依賴(lài)，請(qǐng)安裝它們：sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev
如果編譯沒(méi)有給出任何警告，恭喜你編譯成功！
使用octovis查看示例地圖
在bin/文件夾中，存放著編譯出來(lái)可執(zhí)行文件。為了直觀起見(jiàn)，我們直接看一個(gè)示例地圖：
bin/octovis octomap/share/data/geb079.bt
octovis會(huì)打開(kāi)這個(gè)地圖并顯示。它的UI是長(zhǎng)這樣的。你可以玩玩菜單里各種東西（雖然也不多，我就不一一介紹UI怎么玩了），能看出這是一層樓的掃描圖。octovis是一個(gè)比較實(shí)用的工具，你生成的各種octomap地圖都可以用它來(lái)看。（所以你可以把octovis放到/usr/local/bin/下，省得以后還要找。）
例程1：轉(zhuǎn)換pcd到octomap
GUI玩夠了吧？?jī)H僅會(huì)用UI是不夠滴，現(xiàn)在讓我們開(kāi)始編代碼使用octomap這個(gè)庫(kù)吧！
我為你準(zhǔn)備了三個(gè)小例程。在前兩個(gè)中，我會(huì)教你如何將一個(gè)pcd格式的點(diǎn)云地圖轉(zhuǎn)換為octomap地圖。后一個(gè)中，我會(huì)講講如何根據(jù)g2o優(yōu)化的軌跡，以類(lèi)似slam的方式，把幾個(gè)RGBD圖像拼接出一個(gè)octomap。這對(duì)你研究SLAM會(huì)有一些幫助。所有的代碼與數(shù)據(jù)都可以在我的github上找到。有關(guān)編譯的信息，我寫(xiě)在這個(gè)代碼的Readme中了，請(qǐng)?jiān)诰幾g前看一眼如何編譯這些代碼。
源代碼地址：https://github.com/gaoxiang12/octomap_tutor
源代碼如下：src/pcd2octomap.cpp 這份代碼將命令行參數(shù)1作為輸入文件，參數(shù)2作為輸出文件，把輸入的pcd格式點(diǎn)云轉(zhuǎn)換成octomap格式的點(diǎn)云。通過(guò)這個(gè)例子，你可以學(xué)會(huì)如何創(chuàng)建一個(gè)簡(jiǎn)單的OcTree對(duì)象并往里面添加新的點(diǎn)。
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 // 創(chuàng)建八叉樹(shù)對(duì)象，參數(shù)為分辨率，這里設(shè)成了0.05
29 octomap::OcTree tree( 0.05 );
30
31 for (auto p:cloud.points)
32 {
33 // 將點(diǎn)云里的點(diǎn)插入到octomap中
34 tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
35 }
36
37 // 更新octomap
38 tree.updateInnerOccupancy();
39 // 存儲(chǔ)octomap
40 tree.writeBinary( output_file );
41 cout<<"done."<<endl;
42
43 return 0;
44 }
這個(gè)代碼是相當(dāng)直觀的。在編譯之后，它會(huì)產(chǎn)生一個(gè)可執(zhí)行文件，叫做pcd2octomap，放在代碼根目錄的bin/文件夾下。你可以在代碼根目錄下這樣調(diào)：
1 bin/pcd2octomap data/sample.pcd data/sample.bt
它會(huì)把data文件夾下的sample.pcd（一個(gè)示例pcd點(diǎn)云），轉(zhuǎn)換成一個(gè)data/sample.bt的octomap文件。你可以比較下pcd點(diǎn)云與octomap的區(qū)別。下圖是分別調(diào)用這些顯示命令的結(jié)果。
1 pcl_viewer data/sample.pcd 2 octovis data/sample.ot
這個(gè)octomap里只存儲(chǔ)了點(diǎn)的空間信息，而沒(méi)有顏色信息。我按照高度給它染色了，否則它應(yīng)該就是灰色的。通過(guò)octomap，我們能查看每個(gè)小方塊是否可以通行，從而實(shí)現(xiàn)導(dǎo)航的工作。
以下是對(duì)代碼的一些注解：
注1：有關(guān)如何讀取pcd文件，你可以參見(jiàn)pcl官網(wǎng)的tutorial。不過(guò)這件事情十分簡(jiǎn)單，所以我相信你也能直接看懂。
注2：31行采用了C++11標(biāo)準(zhǔn)的for循環(huán)，它會(huì)讓代碼看起來(lái)稍微簡(jiǎn)潔一些。如果你的編譯器比較老而不支持c++11，你可以自己將它改成傳統(tǒng)的for循環(huán)的樣式。
注3：octomap存儲(chǔ)的文件后綴名是.bt（二進(jìn)制文件）和.ot（普通文件），前者相對(duì)更小一些。不過(guò)octomap文件普遍都很小，所以也不差這么些容量。如果你存成了其他后綴名，octovis可能認(rèn)不出來(lái)。
例程2：加入色彩信息
第一個(gè)示例中，我們將pcd點(diǎn)云轉(zhuǎn)換為octomap。但是pcd點(diǎn)云是有顏色信息的，能否在octomap中也保存顏色信息呢？答案是可以的。octomap提供了ColorOcTree類(lèi)，能夠幫你存儲(chǔ)顏色信息。下面我們就來(lái)做一個(gè)保存顏色信息的示例。代碼見(jiàn)：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 // 創(chuàng)建帶顏色的八叉樹(shù)對(duì)象，參數(shù)為分辨率，這里設(shè)成了0.05
31 octomap::ColorOcTree tree( 0.05 );
32
33 for (auto p:cloud.points)
34 {
35 // 將點(diǎn)云里的點(diǎn)插入到octomap中
36 tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
37 }
38
39 // 設(shè)置顏色
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 // 存儲(chǔ)octomap, 注意要存成.ot文件而非.bt文件
48 tree.write( output_file );
49 cout<<"done."<<endl;
50
51 return 0;
52 }
大部分代碼和剛才是一樣的，除了把OcTree改成ColorOcTree，以及調(diào)用integrateNodeColor來(lái)混合顏色之外。這段代碼會(huì)編譯出pcd2colorOctomap這個(gè)程序，完成帶顏色的轉(zhuǎn)換。不過(guò)，后綴名改成了.ot文件。
1 bin/pcd2colorOctomap data/sample.pcd data/sample.ot
顏色信息能夠更好地幫助我們辨認(rèn)結(jié)果是否正確，給予一個(gè)直觀的印象。是不是好看了一些呢？
例程3：更好的拼接與轉(zhuǎn)換
前兩個(gè)例程中，我們都是對(duì)單個(gè)pcd文件進(jìn)行了處理。實(shí)際做slam時(shí)，我們需要拼接很多幀的octomap。為了做這樣一個(gè)示例，我從自己的實(shí)驗(yàn)數(shù)據(jù)中取出了一小段。這一小段總共含有五張圖像（因?yàn)間ithub并不適合傳大量數(shù)據(jù)），它們存放在data/rgb_index和data/dep_index下。我的slam程序估計(jì)了這五個(gè)關(guān)鍵幀的位置，放在data/trajectory.txt中。它的格式是：幀編號(hào) x y z qx qy qz qw （位置＋姿態(tài)四元數(shù)）。事實(shí)上它是從一個(gè)g2o文件中拷出來(lái)的。你可以用g2o_viewer data/result_after.g2o來(lái)看整個(gè)軌跡。
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
現(xiàn)在我們要做的事，就是根據(jù)trajectory.txt里記錄的信息，把幾個(gè)RGBD圖拼成一個(gè)octomap。這也是所謂的用octomap來(lái)建圖。我寫(xiě)了一個(gè)示例，不知道你能否讀懂呢？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 用于圖像數(shù)據(jù)讀取與處理
10 #include <opencv2/core/core.hpp>
11 #include <opencv2/imgproc/imgproc.hpp>
12 #include <opencv2/highgui/highgui.hpp>
13
14 // 使用Eigen的Geometry模塊處理3d運(yùn)動(dòng)
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 // 全局變量：相機(jī)矩陣
28 // 更好的寫(xiě)法是存到參數(shù)文件中，但為方便起見(jiàn)我就直接這樣做了
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 // 讀關(guān)鍵幀編號(hào)
38 ifstream fin( "./data/keyframe.txt" );
39 vector<int> keyframes;
40 vector< Eigen::Isometry3d > poses;
41 // 把文件 ./data/keyframe.txt 里的數(shù)據(jù)讀取到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 // 讀關(guān)鍵幀姿態(tài)
54 // 我的代碼中使用了Eigen來(lái)存儲(chǔ)姿態(tài)，類(lèi)似的，也可以用octomath::Pose6D來(lái)做這件事
55 fin.open( "./data/trajectory.txt" );
56 while( fin.peek() != EOF )
57 {
58 int index_keyframe;
59 float data[7]; // 三個(gè)位置加一個(gè)姿態(tài)四元數(shù)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文件四元數(shù)按 qx, qy, qz, qw來(lái)存
69 // 但Eigen初始化按照qw, qx, qy, qz來(lái)做
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 // 注意我們的做法是先把圖像轉(zhuǎn)換至pcl的點(diǎn)云，進(jìn)行姿態(tài)變換，最后存儲(chǔ)成octomap
81 // 因?yàn)閛ctomap的顏色信息不是特別方便處理，所以采用了這種迂回的方式
82 // 所以，如果不考慮顏色，那不必轉(zhuǎn)成pcl點(diǎn)云，而可以直接使用octomap::Pointcloud結(jié)構(gòu)
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幀的點(diǎn)云，拼接至全局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生成點(diǎn)云，運(yùn)算方法見(jiàn)《一起做》第二講
98 // 第一次遍歷用于生成空間點(diǎn)云
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旋轉(zhuǎn)之后插入全局地圖
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 }
大部分需要解釋的地方，我都在程序里寫(xiě)了注解。我用了一種稍微有些迂回的方式：先把圖像轉(zhuǎn)成pcl的點(diǎn)云，變換后再放到octotree中。這種做法的原因是比較便于處理顏色，因?yàn)槲蚁Ｍ龀鰩в蓄伾牡貓D。如果你不關(guān)心顏色，完全可以不用pcl，直接用octomap自帶的octomap::pointcloud來(lái)完成這件事。
insertPointCloud會(huì)比單純的插入點(diǎn)更好一些。octomap里的pointcloud是一種射線的形式，只有末端才存在被占據(jù)的點(diǎn)，中途的點(diǎn)則是沒(méi)被占據(jù)的。這會(huì)使一些重疊地方處理的更好。
最后，五幀數(shù)據(jù)拼接出來(lái)的點(diǎn)云大概長(zhǎng)這樣：
可能并不是特別完整，畢竟我們只用了五張圖。這些數(shù)據(jù)來(lái)自于nyud數(shù)據(jù)集的dining_room序列，一個(gè)比較完整的圖應(yīng)該是這樣的：
至少是比純粹點(diǎn)云好些了吧？好了，關(guān)于例程就介紹到這里。如果你準(zhǔn)備使用octomap，這僅僅是個(gè)入門(mén)。你需要去查看它的文檔，了解它的類(lèi)結(jié)構(gòu)，以及一些重要類(lèi)的使用、實(shí)現(xiàn)方式。
《SLAM拾萃》第一講，octomap，就為大家介紹到這里啦。最近我發(fā)現(xiàn)自己寫(xiě)東西，講東西都越來(lái)越長(zhǎng)，所以請(qǐng)?jiān)徫以絹?lái)越啰嗦的寫(xiě)作和說(shuō)話風(fēng)格。希望它能幫助你！我們下講再見(jiàn)！
如果你覺(jué)得我的博客有幫助，可以進(jìn)行幾塊錢(qián)的小額贊助，幫助我把博客寫(xiě)得更好。（雖然我也是從別處學(xué)來(lái)的這招……）
小蘿卜：師兄你學(xué)壞了啊！
參考文獻(xiàn)
[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.
總結(jié)
以上是生活随笔為你收集整理的octomap的简介的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
- 上一篇： ping网关丢包_网络/摄像机丢包的原因
- 下一篇： 图Graph