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">
  <!-- resolution in meters per pixel -->
  <param name="resolution" value="0.05" />
  <!-- name of the fixed frame, needs to be "/map" for SLAM -->
  <param name="frame_id" type="string" value="camera" />
  <!-- max range / depth resolution of the kinect in meter -->
  <param name="sensor_model/max_range" value="100.0" />
  <param name="latch" value="true" />
  <!-- max/min height for occupancy map, should be in meters -->
  <param name="pointcloud_max_z" value="1000" />
  <param name="pointcloud_min_z" value="0" />
  <!-- topic from where pointcloud2 messages are subscribed -->
  <remap from="/cloud_in" to="/point_cloud/output" />
</node>