octomap包的安装与编译 - 想晴天

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

发表于 2016-09-30 14:19 想晴天阅读(3738) 评论(0) 编辑收藏举报