这里需要两个包，一个是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,这样做出来的图是有颜色的。