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工程实现时与八叉树节点概率更新相关的几个重要参数设置函数。

  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。

Octomap使用总结_第1张图片

由于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_type

octomap::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使用总结_第2张图片

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

Octomap使用总结_第3张图片

其中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_mapunordered_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"
  • 光线追迹查询:castRaycastRay函数中参数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();
}


你可能感兴趣的:(Octomap使用总结)