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