个人博客：http://www.chenjianqu.com/
原文链接：http://www.chenjianqu.com/show-102.html
八叉树地图
八叉树地图(OctoMap)就是一种灵活的、压缩的、又能随时更新的地图。八叉树示意图如下：
一个大立方体不断地均匀分成八块，直到变成最小的方块为止。整个大方块可以看成是根节点，而最小的块可以看作是“叶子节点”。于是，在八叉树中，当我们由下一层节点往上走一层时，地图的体积就能扩大八倍。某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点。
八叉树的节点存储了它是否被占据的信息。可以用0表示空白，1表示占据。更好的选择是用概率形式表达某节点是否被占据的事情。比方说，用一个浮点数 x∈[0,1]来表达。这个x一开始取 0.5。如果不断观测到它被占据，那么让这个值不断增加；反之，如果不断观测到它是空白，那就让它不断减小即可，这样可以动态地建模了地图中的障碍物信息。X不断减小或增加可能会超出[0,1]的范围，因此更好的方式是使用概率对数值来描述：y = logit(x) = log(x/(1-x)) ，反变换为 x = logit-1(y) = exp(y)/(exp(y)+1) 。存储 y 来表达节点是否被占据。当不断观测到“占据”时，让 y增加一个值；否则就让 y 减小一个值。当查询概率时，再用逆 logit 变换，将 y 转换至概率即可。
假设我们在RGB-D 图像中观测到某个像素带有深度 d，这说明了一件事：我们在深度值对应的空间点上观察到了一个占据数据，并且，从相机光心出发，到这个点的线段上，应该是没有物体的（否则会被遮挡）。利用这个信息，可以很好地对八叉树地图进行更新，并且能处理运动的结构。
代码实现
使用的库为Octomap，地址为:https://github.com/OctoMap/octomap ，包含 octomap 地图与 octovis（一个可视化程序），二者都是 cmake 工程，分别编译安装即可。
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(octomaptest)
set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11" )
# opencv
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
# eigen
include_directories( "/usr/include/eigen3/" )
# pcl
find_package( PCL REQUIRED COMPONENT common io filters )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )
# octomap
find_package( octomap REQUIRED )
include_directories( ${OCTOMAP_INCLUDE_DIRS} )
add_executable( octomaptest main.cpp )
target_link_libraries( octomaptest ${OpenCV_LIBS} ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES} )
install(TARGETS octomaptest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <octomap/octomap.h> // for octomap
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
using namespace std;
int main( int argc, char** argv )
{
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("./data/pose.txt");
for ( int i=0; i<5; i++ )
{
//读取图片
boost::format fmt( "./data/%s/%d.%s" ); //图像文件格式
colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
//读取位姿
double data[7] = {0};
for ( int i=0; i<7; i++ )
fin>>data[i];
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
Eigen::Isometry3d T(q);
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
poses.push_back( T );
}
// 计算点云拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
cout<<"正在将图像转换为 Octomap ..."<<endl;
// octomap tree
octomap::OcTree tree( 0.02 ); // 参数为分辨率
for ( int i=0; i<5; i++ )
{
cout<<"转换图像中: "<<i+1<<endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
//octomap库自带的简单店运
octomap::Pointcloud cloud; // the point cloud in octomap
for ( int v=0; v<color.rows; v++ )
for ( int u=0; u<color.cols; u++ )
{
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
if ( d==0 ) continue; // 为0表示没有测量到
if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
Eigen::Vector3d point;
point[2] = double(d)/depthScale;
point[0] = (u-cx)*point[2]/fx;
point[1] = (v-cy)*point[2]/fy;
Eigen::Vector3d pointWorld = T*point;
// 将世界坐标系的点放入点云
cloud.push_back( pointWorld[0], pointWorld[1], pointWorld[2] );
}
// 将点云存入八叉树地图，给定原点，这样可以计算投射线
tree.insertPointCloud( cloud, octomap::point3d( T(0,3), T(1,3), T(2,3) ) );
}
// 更新中间节点的占据信息并写入磁盘
tree.updateInnerOccupancy();
cout<<"saving octomap ... "<<endl;
tree.writeBinary( "octomap.bt" );
return 0;
}
上述程序执行完了之后会生成八叉树地图octomap.bt，使用octovis即可查看地图，如下：