文章目录

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 topic
output_grid_map：栅格地图保存的ros bag名字
map_frame：栅格地图的frame名字，rviz默认的即是map
map_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

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔

grid map学习笔记3之详解grid_map_pcl库实现point cloud点云转换成grid map栅格地图

文章目录

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 重新测试和结果

相关文章

Go学习第六天

electron+vue3全家桶+vite项目搭建【25】使用electron-updater自动更新应用

vue2-组件和插件的区别

Qt QThread的moveToThread方法使用

kodi 电视遥控器不起作用，无法唤醒视频OSD

在美国，信用分数超过800，这是一个很好的分数吗？

PyCharm安装使用2023年教程，PyCharm与现流行所有编辑器对比。

为什么list.sort()比Stream().sorted()更快？

ORA-48913: Writing into trace file failed, file size limit [50000000] reached

LeetCode 热题 100 JavaScript--108. 将有序数组转换为二叉搜索树

C#利用自定义特性以及反射，来提大型项目的开发的效率

摄像头电池组和平衡车电池组

云安全攻防（四）之云原生技术

JNI之Java实现远程打印

vim粘贴内容格式混乱解决方法

原型模式（C++）

SpringCloud Gateway获取请求响应body大小

外部排序算法总结

高翔《自动驾驶中的SLAM技术》代码详解 — 第6章 2D SLAM

Redis—全局命令