Octomap使用总结
安装与使用
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工程实现时与八叉树节点概率更新相关的几个重要参数设置函数。
- 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查看默认的参数设定。
- setClampingThresMax/setClampingThresMin。这两个函数决定了一个体元执行log-odd更新的阈值范围。也就是说某一个占据体元的概率值爬升到0.97（对应的log-odd为3.5）或者空闲体元的概率值下降到0.12（对应的log-odd为-2）便不再进行logodd更新计算。可以调用getClampingThresMax/getClampingThresMaxLog、getClampingThresMin/getClampingThresMinLog查看默认的参数设定。
- setOccupancyThres。这个函数定义了octomap判定某一个体元属于占据状态的阈值（isNodeOccupied函数），默认是0.5，一般情况下我们将其设定为0.7。
由于log-odd与概率值之间可以相互转化，因此在工程实现时octomap八叉树节点类OcTreeNode存储的数值是log-odd数值，并不是概率值。
Octomap基本数据类型解析
octomap::Vector3
octomap自定义的基本数据类型，API文档解释如下：
This class represents a three-dimensional vector.
The three-dimensional vector can be used to represent a translation in three-dimensional space or to represent the attitude of an object using Euler angle.
也就是说，这个数据结构给我们提供了octomap命名空间下DIY的三维向量表达，它可以表达三维空间中的一个平移向量，也可以表达以欧
拉角形式表示的三维物体。另外，这里需要注意到octomath::Vector3的提供了多个与向量运算相关的函数，例如相对于坐标系的变换
实现函数、向量归一化函数、向量点积函数等。
octomap::point3d
typedef octomath::Vector3 octomap::point3d显然，point3d是Octomap自定义的类型别名，原始的数据结构还得查看octomath::Vector3
octomap::OcTreeNode
API文档解释如下：
Nodes to be used in OcTree. They represent 3d occupancy grid cells. "value" stores their log-odds occupancy.
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_typeoctomap::KeySet
typedef unordered_ns::unordered_set 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 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 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::Ptr& cloud)
{
// Remove NaN values, if any.
std::vector 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::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: "<::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();
}