地图结构 | 详解八叉树Octomap原理与Rviz可视化

0 专栏介绍

附C++/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 点云地图的局限性

在提到八叉树的概念前，先简要介绍点云图在三维导航问题中的局限性

• 数据量大：三维点云地图通常包含大量的点数据，对存储能力提出了较高的要求；
• 实时性差：点云地图的构建和更新需要时间，无法实时地反映环境的变化。在动态环境下，实时性差可能导致导航算法无法准确感知障碍物的位置和形状；
• 处理复杂性高：点云地图的处理和匹配算法相对复杂，需要耗费大量计算资源才能进行高精度的定位和导航；
• 数据密度不均匀：点云地图中的点分布并不均匀，有些区域可能较为稀疏，而其他区域则可能过于密集。这种不均匀性会导致导航算法在某些区域难以准确感知环境，影响导航的可靠性和精度；
• …

类似于二维平面中栅格地图的概念，我们在三维空间中引入八叉树

2 八叉树基本原理

八叉树(Octree)将三维空间建模为一个立方体，不断将其分成同等大小的八个子立方体直至达到建模的最高精度，在数据结构层面表达为八叉树，如图所示。

八叉树的叶子节点是对三维空间的最小建模单位，称为体素(Voxel)，因此八叉树支持分辨率调节。八叉树的占据概率更新与栅格地图相同，详见地图结构 | 图解占据栅格地图原理(附Matlab建图实验)

八叉树的每个体素亦有三种状态：

• 占据
• 空闲
• 未知

当树结构某个节点的所有子节点都是同一状态时，可以只存储该节点信息而剪去其子节点，实现八叉树压缩以节省内存。此外，八叉树在地图拼接、更新也比点云地图更有优势。

接下来从代码层面更深入地理解八叉树是什么结构。

3 Octovis可视化

首先安装octomap包

git clone https://github.com/OctoMap/octomap

并编译安装

cd octomap
mkdir build
cd build
cmake ..
make

接着使用octovis工具进行可视化

bin/octovis octomap/share/data/geb079.bt

右边的拉条用来调节八叉树分辨率，如下所示(从左往右分别是0.08、0.32、0.64$m$)，这就是八叉树比点云图方便的地方之一

4 点云转化octomap

点云转化为八叉树的原理是，提取点云坐标，接着在八叉树中更新相应位置的节点，核心代码如下

// 从pcd中提取点云
pcl::PointCloud<pcl::PointXYZRGBA> cloud;
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (input_file, cloud);

// 创建八叉树对象，参数为分辨率
octomap::OcTree tree(0.05);

// 用点云更新八叉树
for (auto p:cloud.points)
   tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
tree.updateInnerOccupancy();

// 存储八叉树
tree.writeBinary( output_file );

转化结果如下所示

• 点云：

  pcl_viewer data/sample.pcd
  

• 八叉树：

  bin/pcd2octomap data/sample.pcd data/test.bt
  octovis data/test.bt
  

如果想保留点云中的色彩信息，就创建带颜色的八叉树

pcl::PointCloud<pcl::PointXYZRGBA> cloud;
pcl::io::loadPCDFile<pcl::PointXYZRGBA> (input_file, cloud);

// 创建带颜色的八叉树对象，参数为分辨率
octomap::ColorOcTree tree(0.05);

// 用点云更新八叉树
for (auto p:cloud.points)
    tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );


// 设置颜色
for (auto p:cloud.points)
    tree.integrateNodeColor( p.x, p.y, p.z, p.r, p.g, p.b );

tree.updateInnerOccupancy();

// 存储octomap, 注意要存成.ot文件而非.bt文件
tree.write(output_file);

5 ROS Rviz可视化

Rviz可视化八叉树的整体流程很简单：

• 数据源发布点云数据
• octomap_server接收点云数据并转化为八叉树
• Rviz可视化八叉树

发布点云数据采用以下节点

int main(int argc, char **argv)
{
	std::string pcd_path, frame_id, topic;
	int hz;

	ros::init(argc, argv, "publish_point_cloud");
	ros::NodeHandle nh("~");

	nh.param<std::string>("pcd_path", pcd_path, "test.pcd");
	nh.param<std::string>("frame_id", frame_id, "camera");
	nh.param<std::string>("topic", topic, "/point_cloud/output");
	nh.param<int>("hz", hz, 5);

	ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>(topic, 10);

	pcl::PointCloud<pcl::PointXYZ> cloud;
	sensor_msgs::PointCloud2 output;
	pcl::io::loadPCDFile(pcd_path, cloud);
	pcl::toROSMsg(cloud, output);

	output.header.stamp = ros::Time::now();
	output.header.frame_id = frame_id;

	ros::Rate loop_rate(hz);
	while (ros::ok())
	{
		pcl_pub.publish(output);
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}

可以在Rviz中先可视化点云，订阅/point_cloud/output话题即可

接着将点云数据映射到octomap_server的点云输入接口

<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
  
  <param name="resolution" value="0.05" />
  
  <param name="frame_id" type="string" value="camera" />
  
  <param name="sensor_model/max_range" value="100.0" />
  <param name="latch" value="true" />
  
  <param name="pointcloud_max_z" value="1000" />
  <param name="pointcloud_min_z" value="0" />
  
  <remap from="/cloud_in" to="/point_cloud/output" />
node>

接着即可在Rviz中可视化八叉树

八叉树可视化工具用的是官方插件，Rviz插件的使用参考ROS从入门到精通2-4：Rviz插件制作案例(以多点导航插件为例)

完整工程代码请联系下方博主名片获取

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系