這里需要兩個包，一個是octomap的包，一個是放了怎么做這種圖的程序包。一定要把這兩個包放在同一目錄下，不要放在catkin工作空間，因為octomap這個包會使catkin工作空間catkin_make失敗。
1.編譯
安裝依賴
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev
git clone https://github.com/OctoMap/octomap
下面就是對這個包進行編譯，就是'
mkdir build
cd build
cmake ..
make
只所以新建一個build文件夾，是因為要在里面cmake的話，會把編譯過程中產生的緩存文件放在這個目錄，如果直接cmake .，會不太好整理文件。就像我每次重新編譯的時候，直接把build文件下的所以東西都刪掉就可以了。
2.查看示例地圖
bin/octovis octomap/share/data/geb079.bt
大概長這樣
這是一層樓的掃描圖。
3.把點雲地圖轉換成octomap
git clone https://github.com/gaoxiang12/octomap_tutor
編譯這個包，依然是
mkdir build
cd build
cmake ..
make
編譯之后會在bin目錄下生成幾個可執行程序。這個例子里的程序對應的是pcd2octomap源代碼在src/pcd2octomap.cpp里：
#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;
}
這個代碼也比較簡單.用的時候可以直接調用
bin/pcd2octomap data/sample.pcd data/sample.bt
這個程序的作用是把data下的點雲數據sample.pcd轉換成octovis格式的數據（它有兩種格式一個是bt,一個是ot,其它格式octovis識別不了），生成之后的文件是sample.bt,
比較結果
pcl_viewer data/sample.pcd
../octomap-devel/bin/octovis data/sample.bt
4.加入色彩信息
點雲是有色彩信息的，octomap也有色彩信息，是用ColorOcTree類來存儲色彩信息.用的程序是/bin/pcd2colorOctomap,源代碼在src目錄下。
#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;
}
這個程序是對之前的pcd2octomap做了一些修改，比如把OcTree改成ColorOcTree，以及調用integrateNodeColor來混合顏色等。
bin/pcd2colorOctomap data/sample.pcd data/sample.ot
../octomap-devel/bin/octovis data/sample.ot
效果如下
5.拼接與轉換
這里做的是把五張RGBD圖拼接成octomap,程序是joinMap，源代碼如下：
#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;
}
這個程序是把RGBD圖先轉換成點雲，再放到octotree,這樣做出來的圖是有顏色的。