安装与使用

Octomap采用八叉树数据结构存储三维环境的概率占据地图。关于它的基本介绍，可以移步高翔博客：SLAM拾萃（1）：octomap，包括“什么是Octomap”、“Octomap的原理”、“Octomap的安装”以及“以视觉SLAM为背景玩转Octomap系列”。

安装

高翔博客只对github源码版的Octomap做了介绍，并没有给出ROS下使用Octomap的教程，这里做一些补充介绍。

• Octomap github源码编译安装：高翔博客已经给出详细介绍，这里不再赘述。
• Octomap ros二进制软件包安装：http://wiki.ros.org/octomap
• Octomap ros功能包安装：http://wiki.ros.org/octomap_mapping

说明一下三种安装方式的区别和联系。

• github源码编译安装：如果你对Octomap的源码实现很感兴趣，那么你可以从Octomap的github仓库中将源码git clone到你的文件目录下，结合Octomap的官方API进行阅读。
• ros二进制软件包安装：这个安装方式可能具有一些迷惑性，事实上，如果你按照wiki页面的安装方式安装Octomap，它也还是一个独立版本（stand-alone library）而已，跟源码编译安装差不多，你只是省去了手动编译安装的过程，最后CMakeLists.txt文件有所区别而已。如果在CMakeLists.txt文件中将Octomap的包含路径打印出来，会是下面的结果：

message("${OCTOMAP_INCLUDE_DIRS}") 
# /opt/ros/kinetic/include  

message("${OCTOMAP_LIBRARIES}") 
# /opt/ros/kinetic/lib/liboctomap.so;/opt/ros/kinetic/lib/liboctomath.so  

• Octomap ros功能包安装：事实上，这种方式的安装你才真正会使用到ROS。这种方式可以让你不写任何一句代码就能从PCL点云数据获取Octomap，你需要做的只是roslaunch octomap_server节点，并将你自己PCL点云数据的topic remap一下即可。可以参考：octomap_server

可视化

除去离线的可视化工具octovis，Octomap也提供了rviz下实时显示的功能包：octomap_rviz_plugins。关于这个包的安装，当然也可以apt-get install，但是如果你在后面实际使用的时候，遇到rviz core dumped或者segmentation default之类令人莫名奇妙的rviz程序崩溃问题时，请重新采用源码编译安装的方式！这是博主debug了一天的领悟，具体原因不明，猜测是二进制安装方式没有提供正确可用的关于octomap rviz的.so文件，即便我自己的程序可以发布正确的octomap消息！

原理介绍

Octomap本身的理论基础并不复杂，概率更新只是一个简单的二值贝叶斯滤波过程，Octomap原文也给出了贝叶斯滤波的最终结果即递推的概率更新公式。对贝叶斯滤波的推导过程感兴趣的同学可以参考弗莱堡大学的课件Lecture 12：Grid Maps and Mapping with Known Poses或者阅读《概率机器人》ch4.2。

这里简单的提一下Octomap工程实现时与八叉树节点概率更新相关的几个重要参数设置函数。

1. setProHit/setProMiss。这两个函数决定了inverse sensor model的log-odd概率更新的具体参数，默认情况下占据三维体元被击中（Hit）的概率值为0.7对应的log-odd为0.85，空闲体元被穿越（traverse）的概率值为0.4对应的log-odd为-0.4。可以调用getProHit/getProHitLog、getProMiss/getProMissLog查看默认的参数设定。
2. setClampingThresMax/setClampingThresMin。这两个函数决定了一个体元执行log-odd更新的阈值范围。也就是说某一个占据体元的概率值爬升到0.97（对应的log-odd为3.5）或者空闲体元的概率值下降到0.12（对应的log-odd为-2）便不再进行logodd更新计算。可以调用getClampingThresMax/getClampingThresMaxLog、getClampingThresMin/getClampingThresMinLog查看默认的参数设定。
3. setOccupancyThres。这个函数定义了octomap判定某一个体元属于占据状态的阈值（isNodeOccupied函数），默认是0.5，一般情况下我们将其设定为0.7。
   

由于log-odd与概率值之间可以相互转化，因此在工程实现时octomap八叉树节点类OcTreeNode存储的数值是log-odd数值，并不是概率值。

Octomap基本数据类型解析

octomap::Vector3

octomap自定义的基本数据类型，API文档解释如下：

也就是说，这个数据结构给我们提供了octomap命名空间下DIY的三维向量表达，它可以表达三维空间中的一个平移向量，也可以表达以欧

拉角形式表示的三维物体。另外，这里需要注意到octomath::Vector3的提供了多个与向量运算相关的函数，例如相对于坐标系的变换

实现函数、向量归一化函数、向量点积函数等。

octomap::point3d

typedef octomath::Vector3 octomap::point3d

显然，point3d是Octomap自定义的类型别名，原始的数据结构还得查看octomath::Vector3

octomap::OcTreeNode

octomap::OcTreeKey

octomap自定义的基本数据类型，API文档解释如下：

OcTreeKey is a container class for internal key addressing.The keys count the number of cells (voxels) from the origin as discrete address of a voxel.

也就是说，这个数据结构是一个关键字，它可以实现对八叉树节点OcTreeNode的关键字查询（也就是下面我们即将提到的无序关联容器unordered_set，C++11标准）。

在2D激光SLAM中，我们通常可以生成一张二维栅格地图，依据地图分辨率将一个二维空间点映射成一个二维栅格点，利用Occupancy grid mapping with known poses实现二维栅格地图的概率更新。Octomap其实就是从二维空间扩展到三维空间中了，使用的是八叉树数据结构（回顾下Octomap是什么：采用八叉树数据结构存储三维环境的概率占据地图），OcTreeKey关键字查询的就是离散的三维体元所处的空间地址，OcTreeKey定义的成员变量也说明了这一点。

key_type k [3]

其中key_type是一个类型别名

typedef uint16_t octomap::key_type

octomap::KeySet

typedef unordered_ns::unordered_set<OcTreeKey, OcTreeKey::KeyHash> octomap::KeySet

API文档解释如下：

Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.

显然，KeySet是Octomap自定义的类型别名！它使用一个无序关联容器（std::unordered_set/boost::unordered_set）来存储一组

关键字集合，由哈希函数来组织。哈希函数也称散列函数，直观上，它是一个映射函数f，实现的功能为：内存中记

录的存储位置=f（关键字），关于哈希函数的更多介绍，可以自行百度。在Octomap中，关键字对应的哈希值由

KeyHash结构体实现。

// OcTreeKey::KeyHash: Provides a hash function on Keys
struct KeyHash
{
       size_t operator()(const OcTreeKey& key) const{
         // a simple hashing function 
   // explicit casts to size_t to operate on the complete range
   // constanst will be promoted according to C++ standard
         return size_t(key.k[0]) + 1447*size_t(key.k[1]) + 345637*size_t(key.k[2]);
       }
};

octomap::KeyRay

typedef std::vector< OcTreeKey >::const_iterator const_iterator
 
typedef std::vector< OcTreeKey >::iterator iterator

显然，KeyRay是octomap自定义的类型别名。

实际使用时，KeyRay用于保存单条光束在三维空间中raytracing的结果，KeySet收纳所有光束（也即点云数据）raytracing的结果。

octomap::KeyBoolMap

typedef unordered_ns::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> octomap::KeyBoolMap

KeyBoolMap::const_iterator octomap::OccupancyOcTreeBase< NODE >::changedKeysBegin( ) const

KeyBoolMap::const_iterator octomap::OccupancyOcTreeBase< NODE >::changedKeysEnd( )const

API文档解释如下：

Data structrure to efficiently track changed nodes as a combination of OcTreeKeys and a bool flag (to denote newly created nodes)

KeyBoolMap是一个无序关联容器，实现高效的关键字-值的查询。它同时定义了两个迭代器，方便对“changed nodes”

遍历。这个数据结构的设计具有一定的应用背景，这里先不做过多介绍。

octomap::PointCloud

官网的解释如下：

A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.

在数据结构的设计上，它包括了两个受保护的成员变量

pose6d 	current_inv_transform
point3d_collection  points

其中

typedef std::vector<octomath::Vector3> octomap::point3d_collection

这里，points变量又是一个自定义的类型别名，可以看出octomap::PointCloud使用了vector容器保存点云数据。于是，很自然的在程序中需要从PCL点云数据中提取Octomap点云数据结构时，可以使用push_back压入数据。另外，octomap点云类同时也定义了两个类型别名，方便我们使用迭代器访问vector中的点云数据

typedef point3d_collection::const_iterator octomap::Pointcloud::const_iterator
typedef point3d_collection::iterator octomap::Pointcloud::iterator

在成员函数的实现上，我们使用到了getPoint函数和rotate函数

point3d octomap::Pointcloud::getPoint(unsigned int i) const 
void octomap::Pointcloud::rotate  (double roll,double pitch,double yaw)

八叉树工程实现解析

八叉树基本类型

到这里，我们已经在上面将Octomap最基本的数据类型全部列出并加以解释，却唯独还没有解释八叉树类型OcTree的数据结构。其实，可以认为OcTree八叉树类是建立在以上所有数据结构基础上的顶层类，并不是指其它所有类型都派生于它，而是它提供了操作以上所有数据结构的的方法（函数），也就是核心的函数都由八叉树类型OcTree中提供。

通过阅读octomap的github源码，发现核心代码的实现在octomap::OccupancyOcTreeBase、octomap::OcTreeBase两个类中。

其中insertPointCloud函数由OccupancyOcTreeBase类实现。在目前的PC机上，github提供的源码实现是通过OpenMP并行计算完成的。当然，Octomap建图还经常使用在无人机上，受限于计算设备，机载处理器无法像在PC机上实现并行计算，因此ETH ASL实验室的工程人员专门为此开发了适合于无人机应用的Octomap建图程序包volumetric_mapping。

点云插入函数insertPointCloud函数解析

从实现流程上来看，空间点云raycasting到OcTree的过程大致分为三步

• 坐标系转换。需要使用pcl::transformPointCloud函数
• 光线追迹过程。需要使用computeRayKeys函数。强调一下，参数origin（光束起点）和参数end（传感器末端击中点）都是世界坐标系下的坐标！

bool octomap::OcTreeBaseImpl< OcTreeNode , AbstractOccupancyOcTree >::computeRayKeys(const point3d & origin,const point3d & end,KeyRay & ray ) const
// Traces a ray from origin to end (excluding), returning an OcTreeKey of all nodes traversed by the beam.

• 概率更新。需要使用updateNode函数

virtual OcTreeNode * octomap::OccupancyOcTreeBase< OcTreeNode >::updateNode	(const OcTreeKey & key,float log_odds_update,bool lazy_eval = false )		
// Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).

从工程实现来看，空间点云八叉树的实现基于C++11标准使用了大量C++ Primer中的数据结构，包括基本的顺序容器如Vector以及关联容器map、set（确切说是无序关联容器unordered_map和unordered_set），对C++ Primer掌握程度要求比较高。

查询相关函数解析

Octomap为我们提供了节点查询函数search、iterator以及光线追迹查询函数castRay，Octomap原作给出的原话：individual octree nodes can be accessed by searching for their coordinate. For efficient batch queries, our implementation provides iterators to traverse the octree analogous to a standard C++ container class. With these iterators, all nodes,leaf nodes, or leaf nodes in a certain bounding box can be queried or they can be filtered according to further criteria.

Ray intersection queries, i.e., casting a ray from an origin into a given direction until it hits an occupied volume,

are an important use-case for a 3D map in robotics. This kind of query is used for visibility checks or to localize with range sensors. Thus, we provide this functionality in the castRay(·) method.

• 批量查询迭代器：leaf_iterator。迭代器批量查询所有的叶子节点的状态，跳过所有的Inner nodes节点。可以查看官网给出的使用例程！
• 单节点查询：search、isNodeOccupied。isNodeOccupied函数：queries whether a node is occupied according to the tree's parameter for "occupancy"
• 光线追迹查询：castRay。castRay函数中参数origin（光束起点）是世界坐标系下sensor（可以是RGBD传感器、也可以是三维激光雷达）的位置；参数direction也就是光束的方向向量，只需要给出sensor model光线的方向向量即可，且没有必要对方向向量归一化，castRay函数在内部会为我们完成这件事，返回的光束末端点end是世界坐标系下的坐标表达。再次强调一下，上面我们提到的光线追迹computeRayKeys函数的参数origin（光束起点）和参数end（传感器末端击中点）都是世界坐标系下的表达！

virtual bool octomap::OccupancyOcTreeBase< OcTreeNode >::castRay(const point3d & origin,
                                                                 const point3d & direction,
                                                                 point3d & end,
                                                                 bool ignoreUnknownCells = false,
                                                                 double maxRange = -1.0 
                                                                 )const

实例

最后，以volumetric_mapping为例，给出串行计算模式下insertPointCloud函数的实现过程代码解析（代码略有改动！）

// T_G_sensor：4*4的齐次变换矩阵，存储相机相对于世界坐标系的实时位姿态
// cloud     ：PCL点云数据
void NbvPlannerROS::insertPointcloudIntoMapImpl(const Eigen::Matrix4f& T_G_sensor,const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& cloud)
{
  // Remove NaN values, if any.
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);

  pcl::transformPointCloud(*cloud, *cloud, T_G_sensor);
  const octomap::point3d p_G_sensor = octomap::point3d( T_G_sensor (0,3), T_G_sensor (1,3), T_G_sensor (2,3));

  // 关联容器也可以认为是“关联数组”，只是它相比数组可以使用关键字来查询相应的值。
  // 关于关联容器的讲述，见C++ Primer 374页和376页最重要！！！ 关于关联容器与顺序容器的区别，请见C++ Primer 381页和388页，注意到find函数和insert函数都是关联容器所独有的！！！关联容器并不具备与位置操作相关的push_back等函数
  // 我们这里的是无序关联容器，除了哈希管理操作之外，无序容器提供了与有序容器相同的操作，例如insert、find等
  octomap::KeySet free_cells, occupied_cells;  // 使用哈希函数组织的set API文档对该数据结构的定义：Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.
  int i = 0;
  for (pcl::PointCloud<pcl::PointXYZRGBA>::const_iterator it = cloud->begin();it != cloud->end(); ++it) 
  {
    const octomap::point3d p_G_point(it->x, it->y, it->z);
    
    // 区别：首先检查endpoint是否已经是occupied_cells的关键字成员了
    octomap::OcTreeKey key = m_octomap.coordToKey(p_G_point);

    if (occupied_cells.find(key) == occupied_cells.end())  // 如果给定关键字存在于set中则find返回的迭代器指向该关键字，否则返回尾后迭代器。find是关联容器特有的函数
    {
      castRay(p_G_sensor, p_G_point, &free_cells, &occupied_cells); 
      i++;
    }
  }
  cout<<"if find, time is: "<<i<<endl;

  updateOccupancy(&free_cells, &occupied_cells);

  for (pcl::PointCloud<pcl::PointXYZRGBA>::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
  {
    m_octomap.integrateNodeColor( it->x, it->y, it->z, it->r, it->g, it->b );
  }

  m_octomap.updateInnerOccupancy();
}

void NbvPlannerROS::castRay(const octomap::point3d& sensor_origin,
                           const octomap::point3d& point,
                           octomap::KeySet* free_cells,
                           octomap::KeySet* occupied_cells) const {
  // CHECK_NOTNULL(free_cells);
  // CHECK_NOTNULL(occupied_cells);

  if ( (point - sensor_origin).norm() <= 4.0 ) 
  {
    octomap::KeyRay key_ray;
    if (m_octomap.computeRayKeys(sensor_origin, point, key_ray)) 
    {
      free_cells->insert(key_ray.begin(), key_ray.end());  // key_ray.begin(), key_ray.end()体现的是顺序容器迭代器的基本用法;
    }
    octomap::OcTreeKey key;
    if (m_octomap.coordToKeyChecked(point, key)) 
    {
      occupied_cells->insert(key); //同样还是insert操作，只不过是insert操作的不同实现方式，见C++ Primer 384页
    }
  }
  else 
  {
    octomap::point3d new_end = sensor_origin + (point - sensor_origin).normalized() * 4.0;
    octomap::KeyRay key_ray;
    if (m_octomap.computeRayKeys(sensor_origin, new_end, key_ray))
    {
      free_cells->insert(key_ray.begin(), key_ray.end());
    }
  }
}

void NbvPlannerROS::updateOccupancy(octomap::KeySet* free_cells, octomap::KeySet* occupied_cells) 
{
  // CHECK_NOTNULL(free_cells);
  // CHECK_NOTNULL(occupied_cells);

  // Mark occupied cells.
  for (octomap::KeySet::iterator it = occupied_cells->begin(),end = occupied_cells->end();it != end; it++)  // （无序）关联容器迭代器的使用，见C++ Primer 382页
  {
    m_octomap.updateNode(*it, true);

    // Remove any occupied cells from free cells - assume there are far fewer
    // occupied cells than free cells, so this is much faster than checking on
    // every free cell.
    if (free_cells->find(*it) != free_cells->end()) // find操作，它是（无序）关联容器所特有的函数，访问容器中的指定元素。如果给定关键字存在于set中则find返回的迭代器指向该关键字，否则返回尾后迭代器
    {
      free_cells->erase(*it); // erase操作，删除容器中的指定元素。
    }
  }

  // Mark free cells.
  for (octomap::KeySet::iterator it = free_cells->begin(),end = free_cells->end();it != end; ++it) 
  {
    m_octomap.updateNode(*it, false);
  }
  // m_octomap.updateInnerOccupancy();
}