文章目录
- 0 引言
- 1 grid_map_pcl示例
- 1.1 主要文件
- 1.2 示例数据
- 1.3 启动文件
- 1.4 配置文件
- 1.5 主要实现流程
- 1.6 启动示例
- 1.7 示例结果
- 2 D435i 点云生成栅格地图
- 2.1 D435i 点云文件
- 2.2 修改启动文件
- 2.3 测试和结果
- 2.4 修改配置文件
- 2.5 重新测试和结果
0 引言
grid map学习笔记1已经编译安装并测试了相关的demo示例，grid map学习笔记2进一步熟悉了该库相关的定义，本文在此基础上，重点学习grid_map_pcl，熟悉该子库在输入点云的前提下，如何转换成grid map格式的栅格地图，保存栅格地图，可视化栅格地图，以及相关的配置文件。
grid_map_pcl是一个用于在PointCloud Library (PCL)中进行栅格地图处理的库。它提供了一些功能，能够将点云数据映射到栅格地图中，以便进行高效的处理和分析。
-
创建和管理栅格地图：
使用grid_map_pcl，可以创建一个栅格地图，并在其中存储点云数据。栅格地图是一个二维数据结构，类似于栅格地图，但每个栅格可以存储多个属性值。可以使用grid_map库的函数来创建、管理和访问栅格地图。 -
将点云数据映射到栅格地图：
grid_map_pcl提供了函数来将点云数据映射到栅格地图中。可以使用fromPCLPointCloud2函数将PCL的点云数据转换为grid_map格式，也可以将点云数据添加到栅格地图中的特定层。 -
从栅格地图中提取点云数据：
可以使用toPCLPointCloud2函数将栅格地图中的数据转换回PCL的点云格式，以便进行进一步的处理或保存。 -
进行栅格地图的其他处理：
grid_map_pcl还提供了其他一些功能，如栅格地图的滤波、插值、裁剪等。可以使用这些功能对栅格地图进行进一步的处理和分析。
👉 grid map github：https://github.com/ANYbotics/grid_map
本文系统环境：
- Ubuntu18.04
- ROS-melodic
- grid map
- D435i相机和驱动
1 grid_map_pcl示例
1.1 主要文件
首先grid map学习笔记1已经一起编译安装了grid_map_pcl，可参考上文完成编译。
其次grid_map_pcl文件夹下所有文件的数目录如下：
.
├── CHANGELOG.rst
├── CMakeLists.txt
├── config
│ ├── elevation_map.bag
│ ├── parameters.yaml
│ ├── plane_noisy.pcd
├── doc
│ ├── forest.png
│ ├── indoor.png
│ └── outdoor.png
├── include
│ └── grid_map_pcl
│ ├── GridMapPclConverter.hpp
│ ├── grid_map_pcl.hpp
│ ├── GridMapPclLoader.hpp
│ ├── helpers.hpp
│ ├── PclLoaderParameters.hpp
│ └── PointcloudProcessor.hpp
├── launch
│ └── grid_map_pcl_loader_node.launch
├── package.xml
├── README.md
├── src
│ ├── GridMapPclConverter.cpp
│ ├── GridMapPclLoader.cpp
│ ├── grid_map_pcl_loader_node.cpp
│ ├── helpers.cpp
│ ├── PclLoaderParameters.cpp
│ └── PointcloudProcessor.cpp
└── test
├── GridMapPclLoaderTest.cpp
├── HelpersTest.cpp
├── PointcloudCreator.cpp
├── PointcloudCreator.hpp
├── PointcloudProcessorTest.cpp
├── test_data
│ ├── parameters.yaml
│ └── plane_noisy.pcd
├── test_grid_map_pcl.cpp
├── test_helpers.cpp
└── test_helpers.hpp
其中主要的文件及文件夹简介如下：
CMakeLists.txt：grid_map_pcl库的CMakeLists.txt文件，catkin_make编译时需要依据该文件来寻找依赖库（pcl等），也确定编译后的执行文件等等。config文件夹：该文件夹是grid_map_pcl库的配置文件夹，比如parameters.yaml是主要的配置文件，后文会详细介绍该文件；其他两个是示例的点云输入文件和栅格地图输出bag包。doc文件夹：该文件夹是grid_map_pcl库的说明文档，里边有三种图片，直观展示了grid_map_pcl库把点云生成栅格地图。include文件夹：该文件夹是grid_map_pcl库的头文件，GridMapPclConverter.hpp声明了点云和栅格地图数据的转换函数等；GridMapPclLoader.hpp声明了点云和栅格地图的导入函数；PclLoaderParameters.hpp声明了点云相关的参数导入函数；helpers.hpp声明了各种类似中间件的函数，比如栅格地图topic函数，栅格地图保存函数等。src文件夹：该文件夹是grid_map_pcl库的主要函数实现文件，对应include文件夹的头文件，是各个头文件中声明函数的的具体实现代码。launch文件夹：grid_map_pcl_loader_node.launch是具体的点云生成栅格地图并保存示例的启动文件。test文件夹：该文件夹是grid_map_pcl库的测试代码，类似src文件夹中的主要函数实现代码。
1.2 示例数据
示例文件是grid_map_pcl/config/plane_noisy.pcd点云文件，因为已经安装过pcl，可直接用pcl_viewer来打开查看：
# plane_noisy.pcd 文件目录下新开终端
pcl_viewer plane_noisy.pcd
1.3 启动文件
grid_map_pcl_loader_node.launch 启动文件的内容，主要内容简介如下：
folder_path：配置文件、输入文件及输出文件的相对路径pcd_filename：点云文件的名字map_rosbag_topic：栅格地图的ros topicoutput_grid_map：栅格地图保存的ros bag名字map_frame：栅格地图的frame名字，rviz默认的即是mapmap_layer_name：该层栅格地图的名字grid_map_pcl_loader_node：该示例启动的主要代码文件名为grid_map_pcl_loader_node.cpp
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="folder_path" default="$(find grid_map_pcl)/config"/>
<arg name="pcd_filename" default="plane_noisy.pcd" />
<arg name="map_rosbag_topic" default="grid_map" />
<arg name="output_grid_map" default="elevation_map.bag" />
<arg name="map_frame" default="map" />
<arg name="map_layer_name" default="elevation" />
<arg name="prefix" default=""/>
<arg name="set_verbosity_to_debug" default="false"/>
<node name="grid_map_pcl_loader_node" pkg="grid_map_pcl"
type="grid_map_pcl_loader_node" output="screen" launch-prefix="$(arg prefix)">
<rosparam file="$(find grid_map_pcl)/config/parameters.yaml" />
<param name="folder_path" type="string" value="$(arg folder_path)" />
<param name="pcd_filename" type="string" value="$(arg pcd_filename)" />
<param name="map_rosbag_topic" type="string" value="$(arg map_rosbag_topic)" />
<param name="output_grid_map" type="string" value="$(arg output_grid_map)" />
<param name="map_frame" type="string" value="$(arg map_frame)" />
<param name="map_layer_name" type="string" value="$(arg map_layer_name)" />
<param name="set_verbosity_to_debug" type="bool" value="$(arg set_verbosity_to_debug)" />
</node>
</launch>
1.4 配置文件
parameters.yaml配置文件的内容如下，详细的配置参数含义已经注释：
pcl_grid_map_extraction:
# 所需的线程数
num_processing_threads: 4
# 点云转换矩阵，平移和旋转（rpy）
cloud_transform:
translation:
x: 0.0
y: 0.0
z: 0.0
rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence
r: 0.0
p: 0.0
y: 0.0
# 聚类提取的参数设置：聚类阈值，聚类最小的点云数及最大点云数
cluster_extraction:
cluster_tolerance: 0.05
min_num_points: 4
max_num_points: 1000000
use_max_height_as_cell_elevation: false
# 移除异常值：是否移除及参数设置
outlier_removal:
is_remove_outliers: true
mean_K: 10
stddev_threshold: 1.0
# 降采样设置：是否降采样，体素大小
downsampling:
is_downsample_cloud: true
voxel_size:
x: 0.02
y: 0.02
z: 0.02
# 栅格地图的设置：栅格单元的最小点云数和分辨率
grid_map:
min_num_points_per_cell: 4
resolution: 0.1
1.5 主要实现流程
grid_map_pcl_loader_node.cpp是该示例启动的main函数文件，详细的注释如下，主要流程是：ros 初始化->定义grid map发布的ros topic->导入数据和配置参数->转换点云为栅格地图->保存和发布栅格地图。
#include <ros/ros.h>
#include <grid_map_core/GridMap.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include "grid_map_pcl/GridMapPclLoader.hpp"
#include "grid_map_pcl/helpers.hpp"
namespace gm = ::grid_map::grid_map_pcl;
int main(int argc, char** argv) {
// ros 初始化
ros::init(argc, argv, "grid_map_pcl_loader_node");
ros::NodeHandle nh("~");
gm::setVerbosityLevelToDebugIfFlagSet(nh); // 是否设置为调试级别
// 定义grid map发布的ros topic
ros::Publisher gridMapPub;
gridMapPub = nh.advertise<grid_map_msgs::GridMap>("grid_map_from_raw_pointcloud", 1, true);
grid_map::GridMapPclLoader gridMapPclLoader;
const std::string pathToCloud = gm::getPcdFilePath(nh); // 获取点云pcd导入的路径
gridMapPclLoader.loadParameters(gm::getParameterPath()); // 获取配置参数的路径并导入参数
gridMapPclLoader.loadCloudFromPcdFile(pathToCloud); // 导入点云文件
// 初始化点云到栅格地图，添加点云转换的栅格地图层 等
gm::processPointcloud(&gridMapPclLoader, nh);
// 定义并生成栅格地图，添加frame id
grid_map::GridMap gridMap = gridMapPclLoader.getGridMap();
gridMap.setFrameId(gm::getMapFrame(nh));
// 保存栅格地图到 ros bag文件中
gm::saveGridMap(gridMap, nh, gm::getMapRosbagTopic(nh));
// publish grid map，持续发布生成的栅格地图topic
grid_map_msgs::GridMap msg;
grid_map::GridMapRosConverter::toMessage(gridMap, msg);
gridMapPub.publish(msg);
// run
ros::spin();
return EXIT_SUCCESS;
}
1.6 启动示例
执行以下命令启动：
# source激活环境
source ~/grid_map_ws/devel/setup.bash
roslaunch grid_map_pcl grid_map_pcl_loader_node.launch
1.7 示例结果
启动后，终端会打印出如下信息：
process[grid_map_pcl_loader_node-1]: started with pid [13328]
[ INFO] [1690873305.071903062]: Preprocessing of the pointcloud started
[ INFO] [1690873305.168204888]: Preprocessing and filtering finished
[ INFO] [1690873305.168273120]: Grid map dimensions: 2.5 x 1.3
[ INFO] [1690873305.168291046]: Grid map resolution: 0.1
[ INFO] [1690873305.168300955]: Grid map num cells: 25 x 13
[ INFO] [1690873305.168307533]: Initialized map geometry
[ INFO] [1690873305.168315524]: Initialization took: 0.096 sec
[ INFO] [1690873305.169005291]: Started adding layer: elevation
[ INFO] [1690873305.182350266]: Finished adding layer: elevation
[ INFO] [1690873305.182377328]: Total time: 0.11 sec
[ INFO] [1690873305.187189276]: Saving grid map successful: true
这时会发现grid_map_pcl/config/elevation_map.bag路径下保存了栅格地图的ros bag；通过rosbag info elevation_map.bag 可查看保存的栅格数据信息：
path: elevation_map.bag
version: 2.0
duration: 0.0s
start: Jul 31 2023 16:46:17.52 (1690793177.52)
end: Jul 31 2023 16:46:17.52 (1690793177.52)
size: 13.3 KB
messages: 1
compression: none [1/1 chunks]
types: grid_map_msgs/GridMap [95681e052b1f73bf87b7eb984382b401]
topics: grid_map 1 msg : grid_map_msgs/GridMap
同时开启rviz，按照如下步骤添加生成的栅格地图topic，即可看到可视化的栅格地图：
2 D435i 点云生成栅格地图
通过第1章，已经熟悉了grid_map_pcl实现点云转换成栅格地图的步骤，那接下来就用自己的点云数据来生成栅格地图，接下里就用D435i相机来演示。
2.1 D435i 点云文件
默认已经安装了D435i相机驱动，启动前需要修改rs_camera.launch，打开点云：
<!-- rs_camera.launch 修改下行的false 为 true -->
<arg name="enable_pointcloud" default="true"/>
然后通过roslaunch realsense2_camera rs_camera.launch命令启动D435i相机，通过rostopic list即可查看到/camera/depth/color/points是D435i相机的点云topic。
接下来就是如何保存一帧点云topic的问题，可以直接使用或参考如下python代码，调用python-pcl（需安装pip install python-pcl）等库，主要流程是：订阅点云topic->转换为pcl库格式数据->保存成pcd点云文件->取消订阅。
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import pcl
class PointCloudSubscriber:
def __init__(self):
self.cloud = pcl.PointCloud_PointXYZRGB()
self.sub = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.callback)
def callback(self, data):
# Convert PointCloud2 message to PCL data
pc = point_cloud2.read_points(data)
self.cloud.from_list(list(pc))
# Save PCL data as PCD file
pcl.save(self.cloud, 'point_cloud_xyzrgb.pcd')
rospy.loginfo('PointCloud2 message saved as point_cloud.pcd')
# Unsubscribe from topic after saving one frame
self.sub.unregister()
if __name__ == '__main__':
rospy.init_node('pcl_subscriber', anonymous=True)
pcl_subscriber = PointCloudSubscriber()
rospy.spin()
执行完脚本后，即可在当前文件夹下保存一帧D435i相机的点云文件point_cloud_xyzrgb.pcd，可以用pcl_viewer point_cloud_xyzrgb.pcd来可视化查看点云：
备注：或者直接下载point_cloud_xyzrgb.pcd
2.2 修改启动文件
首先复制point_cloud_xyzrgb.pcd到grid_map_pcl/config/目录下，然后如下修改grid_map_pcl_loader_node.launch启动文件中的pcd_filename和output_grid_map。
<arg name="folder_path" default="$(find grid_map_pcl)/config"/>
<!-- <arg name="pcd_filename" default="plane_noisy.pcd" /> -->
<arg name="pcd_filename" default="point_cloud_xyzrgb.pcd" />
<arg name="map_rosbag_topic" default="grid_map" />
<!-- <arg name="output_grid_map" default="elevation_map.bag" /> -->
<arg name="output_grid_map" default="point_cloud_xyzrgb_map.bag" />
2.3 测试和结果
重新执行以下命令启动：
# source激活环境
source ~/grid_map_ws/devel/setup.bash
roslaunch grid_map_pcl grid_map_pcl_loader_node.launch
启动后，终端会打印出如下信息：
process[grid_map_pcl_loader_node-1]: started with pid [17862]
[ INFO] [1690875646.276651807]: Preprocessing of the pointcloud started
[ INFO] [1690875648.082437226]: Preprocessing and filtering finished
[ INFO] [1690875648.082582464]: Grid map dimensions: 10.3 x 3.9
[ INFO] [1690875648.082612676]: Grid map resolution: 0.1
[ INFO] [1690875648.082621679]: Grid map num cells: 103 x 39
[ INFO] [1690875648.082628487]: Initialized map geometry
[ INFO] [1690875648.082636410]: Initialization took: 1.806 sec
[ INFO] [1690875648.083387628]: Started adding layer: elevation
[ INFO] [1690875648.103188697]: Finished adding layer: elevation
[ INFO] [1690875648.103218881]: Total time: 1.826 sec
[ INFO] [1690875648.107564757]: Saving grid map successful: true
这时会发现grid_map_pcl/config/point_cloud_xyzrgb_map.bag路径下保存了栅格地图的ros bag；通过rosbag info point_cloud_xyzrgb_map.bag 可查看保存的栅格数据信息：
path: point_cloud_xyzrgb_map.bag
version: 2.0
duration: 0.0s
start: Jul 31 2023 18:03:42.42 (1690797822.42)
end: Jul 31 2023 18:03:42.42 (1690797822.42)
size: 27.7 KB
messages: 1
compression: none [1/1 chunks]
types: grid_map_msgs/GridMap [95681e052b1f73bf87b7eb984382b401]
topics: grid_map 1 msg : grid_map_msgs/GridMap
同时开启rviz，按照如下步骤添加生成的栅格地图topic，即可看到可视化的栅格地图：
2.4 修改配置文件
以上演示的仅仅是修改了启动文件，替换了输入的点云文件，但每个传感器的点云质量，密度等都不一样，所以也需要多次调试配置文件parameters.yaml来调节生成的栅格地图。
比如仅调节resolution参数，分别调节成 resolution: 0.5 和 resolution: 0.05，其他参数不变，对比生成的栅格地图。
# 栅格地图的设置：栅格单元的最小点云数和分辨率
grid_map:
min_num_points_per_cell: 4
# resolution: 0.1
resolution: 0.5
# resolution: 0.05
2.5 重新测试和结果
每次调整参数后，重新执行roslaunch grid_map_pcl grid_map_pcl_loader_node.launch生成栅格地图，如下图 resolution: 0.5 、 resolution: 0.1和 resolution: 0.05的结果：
| resolution | 0.5 | 0.1 | 0.05 |
|---|---|---|---|
| grid map |  |  |  |
Reference:
- https://github.com/anybotics/grid_map
⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔
