SLAM拾萃(1)：octomap

本篇是《SLAM拾萃》第一篇，介紹一個建圖工具：octomap。和往常一樣，我會介紹它的原理、安裝與使用方式，並提供例程供讀者學習。必要時也會請小蘿蔔過來吐槽。（小蘿蔔真是太好用了，它可以代替讀者提很多問題。）

---

 什麼是octomap？

　　RGBD SLAM的目的有兩個：估計機器人的軌跡，並建立正確的地圖。地圖有很多種表達方式，比如特徵點地圖、網格地圖、拓撲地圖等等。在《一起做》系列中，我們使用的地圖形式主要是點雲地圖。在程序中，我們根據優化後的位姿，拼接點雲，最後構成地圖。這種做法很簡單，但有一些明顯的缺陷：

• 地圖形式不緊湊。
  　　點雲地圖通常規模很大，所以一個pcd文件也會很大。一張640××480的圖像，會產生30萬個空間點，需要大量的存儲空間。即使經過一些濾波之後，pcd文件也是很大的。而且討厭之處在於，它的“大”並不是必需的。點雲地圖提供了很多不必要的細節。對於地毯上的褶皺、陰暗處的影子，我們並不特別關心這些東西。把它們放在地圖裏是浪費空間。
• 處理重疊的方式不夠好。
  　　在構建點雲時，我們直接按照估計位姿拼在了一起。在位姿存在誤差時，會導致地圖出現明顯的重疊。例如一個電腦屏變成了兩個，原本方的邊界變成了多邊形。對重疊地區的處理方式應該更好一些。
• 難以用於導航
  　　說起地圖的用處，第一就是導航啦！有了地圖，就可以指揮機器人從A點到B點運動，豈不是很方便的事？但是，給你一張點雲地圖，是否有些傻眼了呢？我至少得知道哪些地方可通過，哪些地方不可通過，才能完成導航呀！光有點是不夠的！

　　octomap就是爲此而設計的！親，你沒有看錯，它可以優雅地壓縮、更新地圖，並且分辨率可調！它以八叉樹（octotree，後面會講）的形式存儲地圖，相比點雲，能夠省下大把的空間。octomap建立的地圖大概是這樣子的：（從左到右是不同的分辨率）

　　

　　由於八叉樹的原因，它的地圖像是很多個小方塊組成的（很像minecraft）。當分辨率較高時，方塊很小；分辨率較低時，方塊很大。每個方塊表示該格被佔據的概率。因此你可以查詢某個方塊或點“是否可以通過”，從而實現不同層次的導航。簡而言之，環境較大時採用較低分辨率，而較精細的導航可採用較高分辨率。

　　小蘿蔔：師兄你這是介紹嗎？真像廣告啊……

---

 octomap原理

　　本段會講一些數學知識。如果你想“跑跑程序看效果”，可以跳過本段。

1. 八叉樹的表達

　　八叉樹，也就是傳說中有八個子節點的樹！是不是很厲害呢？至於爲什麼要分成八個子節點，想象一下一個正方形的方塊的三個面各切一刀，不就變成八塊了嘛！如果你想象不出來，請看下圖：

 

　　實際的數據結構呢，就是一個樹根不斷地往下擴，每次分成八個枝，直到葉子爲止。葉子節點代表了分辨率最高的情況。例如分辨率設成0.01m，那麼每個葉子就是一個1cm見方的小方塊了呢！

　　

　　每個小方塊都有一個數描述它是否被佔據。在最簡單的情況下，可以用0－1兩個數表示（太簡單了所以沒什麼用）。通常還是用0～1之間的浮點數表示它被佔據的概率。0.5表示未確定，越大則表示被佔據的可能性越高，反之亦然。由於它是八叉樹，那麼一個節點的八個孩子都有一定的概率被佔據或不被佔據啦！（下圖是一棵八叉樹）

　　用樹結構的好處時：當某個節點的子結點都“佔據”或“不佔據”或“未確定”時，就可以把它給剪掉！換句話說，如果沒必要進一步描述更精細的結構（孩子節點）時，我們只要一個粗方塊（父節點）的信息就夠了。這可以省去很多的存儲空間。因爲我們不用存一個“全八叉樹”呀！

　　2.　八叉樹的更新

小蘿蔔：哦！這個我就懂了！每新來一個就直接加到原來的上面，是吧？

　　師兄：對，此外還要加一個最大最小值的限制。最後轉換回原來的概率即可。

　　八叉樹中的父親節點佔據概率，可以根據孩子節點的數值進行計算。比較簡單的是取平均值或最大值。如果把八叉樹按照佔據概率進行渲染，不確定的方塊渲染成透明的，確定佔據的渲染成不透明的，就能看到我們平時見到的那種東西啦！

　　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

1. 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

   如果編譯沒有給出任何警告，恭喜你編譯成功！

2. 使用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中了，請在編譯前看一眼如何編譯這些代碼。

　　源代碼地址：https://github.com/gaoxiang12/octomap_tutor

　　源代碼如下：src/pcd2octomap.cpp 這份代碼將命令行參數1作爲輸入文件，參數2作爲輸出文件，把輸入的pcd格式點雲轉換成octomap格式的點雲。通過這個例子，你可以學會如何創建一個簡單的OcTree對象並往裏面添加新的點。　　

#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//octomap 
#include <octomap/octomap.h>
using namespace std;

int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2octomap <input_file> <output_file>"<<endl;
        return -1;
    }

    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    //聲明octomap變量
    cout<<"copy data into octomap..."<<endl;
    // 創建八叉樹對象，參數爲分辨率，這裏設成了0.05
    octomap::OcTree tree( 0.05 );

    for (auto p:cloud.points)
    {
        // 將點雲裏的點插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }

    // 更新octomap
    tree.updateInnerOccupancy();
    // 存儲octomap
    tree.writeBinary( output_file );
    cout<<"done."<<endl;

    return 0;
}

這個代碼是相當直觀的。在編譯之後，它會產生一個可執行文件，叫做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

#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//octomap 
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>

using namespace std;

int main( int argc, char** argv )
{
    if (argc != 3)
    {
        cout<<"Usage: pcd2colorOctomap <input_file> <output_file>"<<endl;
        return -1;
    }

    string input_file = argv[1], output_file = argv[2];
    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZRGBA> ( input_file, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    //聲明octomap變量
    cout<<"copy data into octomap..."<<endl;
    // 創建帶顏色的八叉樹對象，參數爲分辨率，這裏設成了0.05
    octomap::ColorOcTree tree( 0.05 );

    for (auto p:cloud.points)
    {
        // 將點雲裏的點插入到octomap中
        tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
    }

    // 設置顏色
    for (auto p:cloud.points)
    {
        tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
    }

    // 更新octomap
    tree.updateInnerOccupancy();
    // 存儲octomap, 注意要存成.ot文件而非.bt文件
    tree.write( output_file );
    cout<<"done."<<endl;

    return 0;
}

　大部分代碼和剛纔是一樣的，除了把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

#include <iostream>
#include <vector>

// octomap 
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap/math/Pose6D.h>

// opencv 用於圖像數據讀取與處理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// 使用Eigen的Geometry模塊處理3d運動
#include <Eigen/Core>
#include <Eigen/Geometry> 

// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>

// boost.format 字符串處理
#include <boost/format.hpp>

using namespace std;

// 全局變量：相機矩陣
// 更好的寫法是存到參數文件中，但爲方便起見我就直接這樣做了
float camera_scale  = 1000;
float camera_cx     = 325.5;
float camera_cy     = 253.5;
float camera_fx     = 518.0;
float camera_fy     = 519.0;

int main( int argc, char** argv )
{
    // 讀關鍵幀編號
    ifstream fin( "./data/keyframe.txt" );
    vector<int> keyframes;
    vector< Eigen::Isometry3d > poses;
    // 把文件 ./data/keyframe.txt 裏的數據讀取到vector中
    while( fin.peek() != EOF )
    {
        int index_keyframe;
        fin>>index_keyframe;
        if (fin.fail()) break;
        keyframes.push_back( index_keyframe );
    }
    fin.close();

    cout<<"load total "<<keyframes.size()<<" keyframes. "<<endl;

    // 讀關鍵幀姿態
    // 我的代碼中使用了Eigen來存儲姿態，類似的，也可以用octomath::Pose6D來做這件事
    fin.open( "./data/trajectory.txt" );
    while( fin.peek() != EOF )
    {
        int index_keyframe;
        float data[7]; // 三個位置加一個姿態四元數x,y,z, w,ux,uy,uz
        fin>>index_keyframe;
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
            cout<<data[i]<<" ";
        }
        cout<<endl;
        if (fin.fail()) break;
        // 注意這裏的順序。g2o文件四元數按 qx, qy, qz, qw來存
        // 但Eigen初始化按照qw, qx, qy, qz來做
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d t(q);
        t(0,3) = data[0]; t(1,3) = data[1]; t(2,3) = data[2];
        poses.push_back( t );
    }
    fin.close();

    // 拼合全局地圖
    octomap::ColorOcTree tree( 0.05 ); //全局map

    // 注意我們的做法是先把圖像轉換至pcl的點雲，進行姿態變換，最後存儲成octomap
    // 因爲octomap的顏色信息不是特別方便處理，所以採用了這種迂迴的方式
    // 所以，如果不考慮顏色，那不必轉成pcl點雲，而可以直接使用octomap::Pointcloud結構
    
    for ( size_t i=0; i<keyframes.size(); i++ )
    {
        pcl::PointCloud<pcl::PointXYZRGBA> cloud; 
        cout<<"converting "<<i<<"th keyframe ..." <<endl;
        int k = keyframes[i];
        Eigen::Isometry3d& pose = poses[i];

        // 生成第k幀的點雲，拼接至全局octomap上
        boost::format fmt ("./data/rgb_index/%d.ppm" );
        cv::Mat rgb = cv::imread( (fmt % k).str().c_str() );
        fmt = boost::format("./data/dep_index/%d.pgm" );
        cv::Mat depth = cv::imread( (fmt % k).str().c_str(), -1 );

        // 從rgb, depth生成點雲，運算方法見《一起做》第二講
        // 第一次遍歷用於生成空間點雲
        for ( int m=0; m<depth.rows; m++ )
            for ( int n=0; n<depth.cols; n++ )
            {
                ushort d = depth.ptr<ushort> (m) [n];
                if (d == 0)
                    continue;
                float z = float(d) / camera_scale;
                float x = (n - camera_cx) * z / camera_fx;
                float y = (m - camera_cy) * z / camera_fy;
                pcl::PointXYZRGBA p;
                p.x = x; p.y = y; p.z = z;

                uchar* rgbdata = &rgb.ptr<uchar>(m)[n*3];
                uchar b = rgbdata[0];
                uchar g = rgbdata[1];
                uchar r = rgbdata[2];

                p.r = r; p.g = g; p.b = b;
                cloud.points.push_back( p ); 
            }
        // 將cloud旋轉之後插入全局地圖
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp( new pcl::PointCloud<pcl::PointXYZRGBA>() );
        pcl::transformPointCloud( cloud, *temp, pose.matrix() );

        octomap::Pointcloud cloud_octo;
        for (auto p:temp->points)
            cloud_octo.push_back( p.x, p.y, p.z );
        
        tree.insertPointCloud( cloud_octo, 
                octomap::point3d( pose(0,3), pose(1,3), pose(2,3) ) );

        for (auto p:temp->points)
            tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );
    }
    
    tree.updateInnerOccupancy();
    tree.write( "./data/map.ot" );

    cout<<"done."<<endl;
    
    return 0;

}

大部分需要解釋的地方，我都在程序裏寫了註解。我用了一種稍微有些迂迴的方式：先把圖像轉成pcl的點雲，變換後再放到octotree中。這種做法的原因是比較便於處理顏色，因爲我希望做出帶有顏色的地圖。如果你不關心顏色，完全可以不用pcl，直接用octomap自帶的octomap::pointcloud來完成這件事。

　　insertPointCloud會比單純的插入點更好一些。octomap裏的pointcloud是一種射線的形式，只有末端才存在被佔據的點，中途的點則是沒被佔據的。這會使一些重疊地方處理的更好。

　　最後，五幀數據拼接出來的點雲大概長這樣：　　

　　可能並不是特別完整，畢竟我們只用了五張圖。這些數據來自於nyud數據集的dining_room序列，一個比較完整的圖應該是這樣的：

　　至少是比純粹點雲好些了吧？好了，關於例程就介紹到這裏。如果你準備使用octomap，這僅僅是個入門。你需要去查看它的文檔，瞭解它的類結構，以及一些重要類的使用、實現方式。

---

　　《SLAM拾萃》第一講，octomap，就爲大家介紹到這裏啦。最近我發現自己寫東西，講東西都越來越長，所以請原諒我越來越囉嗦的寫作和說話風格。希望它能幫助你！我們下講再見！