个人博客：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 stringsusing 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即可查看地图，如下：