1. PCL point cloud learning
1.1. Introduction to PCL
/*
PCL(Point Cloud Library)作为我们接触的第一个外部库,可见其重要性。
PCL里实现了大量的处理点云相关的功能,实现了大量点云相关的通用算法和高效数据结构,
涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。
*/
/*
当我们要对激光雷达的数据进行处理时,我们也需要将ROS获取的雷达数据,
即 sensor_msgs::mgs::LaserScan转化为PCL点云库可以使用的数据
如`pcl::PointCloud<T>`.,这就是需要学习PCL的原因。
*/
1.2. Data conversion method
/*
通过上图我们可得知数据转换的方法步骤为
1、首先我们需要订阅激光雷达topic(如/scan)获取到sensor_msgs::LaserScan的message.然后使用ROS提供的 laser_geometry包将其转化为sensor_msgs::PointCloud2格式的message
2、将sensor_msgs::PointCloud2的message转换为PCL点云库所需的数据格式。
这里有两种方式
方式一:使用ROS提供的pcl_conversions包。
方式二:直接订阅之前转化的PointCloud2的数据。
这样可以自动完成两种类型的转换
*/
/****注******/
// scan的数据形式类似极坐标,PointCloud的数据形式类似笛卡尔坐标。这里需要有一个坐标转换。
X = R * cos(θ)
Y = R * sin(θ)
其中,(X, Y) 是笛卡尔坐标系中的点坐标,R 是极坐标中的距离,θ 是极坐标中的角度。
1.3. Use of PCL point cloud
// 使用PCL(Point Cloud Library)库来创建一个点云(PointCloud)对象,并初始化了一个名为cloud的指针,指向这个点云对象。
pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 创建点云发布者 这里需要注意其中 消息类型需要相同
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;
pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("dzj_scan", 10);
1.4. PassThrough Filtering in PCL
PassThrough Filtering is a basic filtering method in point cloud processing. It can be used to clip the point cloud according to the user-defined axial range and only retain points within the specified range. This is useful for removing points that are not within a specific area or extracting points in a region of interest.
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud); // cloud 是点云数据
pass.setFilterFieldName("z"); // 以 Z 轴为例
pass.setFilterLimits(min_limit, max_limit); // 设置滤波的范围
pass.filter(*cloud);
2. ROS2 performs circle fitting based on pcl point cloud data
2.1 Build workspace
mkdir -p ~/scan_get/src
cd ~/ros2_ws
colcon build
source install/setup.bash
2.2 Build ROS2 dependency package
Remember to enter the scr under workspace scan_get to create a function package.
Because subscribing to messages here requires two functional packages, rclcpp and std_msgs.
cd ~/scan_get/src
//这种方式需要手动添加add_executable()、ament_target_dependencies()、install(TARGETS DESTINATION lib/${PROJECT_NAME}) 内容
//ros2 pkg create my_lidar_listener --build-type ament_cmake --dependencies rclcpp sensor_msgs
//这种方式 --node-name lidar_subscriber 后面加上这句话就不用自己手动添加了。这种方式也不用手动创建cpp文件
ros2 pkg create my_lidar_listener --build-type ament_cmake --dependencies rclcpp sensor_msgs --node-name lidar_subscriber
2.3. Create CPP file
In the src directory of the function package my_lidar_listener, create a new C++ file lidar_listener.cpp.
The complete directory structure of the function package at this time is as follows
\src
└── my_lidar_listener
├── CMakeLists.txt
├── include
│ └── my_lidar_listener
├── package.xml
└── src
└── lidar_listener.cpp
2.4. Add the required dependencies in poackage.xml to ensure that the package can be compiled normally.
# 在 <depend> 标签中添加包所依赖的其他ROS2软件包。这些依赖项告诉ROS2构建系统,该包需要这些软件包来编译和运行。
<depend>pcl</depend>
<depend>pcl_conversions</depend>
2.5. Add ROS2 dependencies and build rules in CmakeList.txt
# 确保通过 find_package() 来查找所需的依赖项,并在构建时包含它们
find_package(PCL REQUIRED)
find_package(pcl_conversions REQUIRED)
# 将依赖项包含到构建中
ament_target_dependencies(lidar_listener
"rclcpp"
"sensor_msgs"
"PCL"
"pcl_conversions"
)
If the uppercase PCL in the above CmakeList.txt dependency is written as lowercase pcl, the following error will be reported
CMake Error at CMakeLists.txt:24 (find_package):
By not providing "Findpcl.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "pcl", but
CMake did not find one.
2.6. Data types that can be published in ros2->publish();
在ROS2中,publish()函数可以用来发布各种不同类型的ROS2消息。发布函数的参数应该是与发布器声明的消息类型相匹配的消息对象。
sensor_msgs/msg/Image | Message type used for image data, such as camera images. |
---|---|
sensor_msgs/msg/PointCloud2 | Message type used for point cloud data. |
geometry_msgs/msg/PoseStamped | Message type used to represent pose (position and orientation) information. |
nav_msgs/msg/Odometry | Message type used to represent odometer information for a robot or vehicle. |
std_msgs/msg/String | Message type used to post string data. |
custom ROS 2 messages | You can also create custom ROS 2 message types |
发布器的声明应该与你要发布的消息类型匹配。例如,如果你要发布激光扫描数据,你应该创建一个发布器,其消息类型为sensor_msgs/msg/LaserScan,然后使用该发布器的publish()函数来发布激光扫描数据。
The picture below shows a typical error
This requires data type conversion and conversion to msg under ros.
// 将PCL点云转换为PointCloud2消息
sensor_msgs::msg::PointCloud2 cloud_msg;
pcl::toROSMsg(*my_cloud, cloud_msg);
// 设置PointCloud2消息的头信息
cloud_msg.header = msg->header;
// 发布点云数据
pointcloud_publisher_->publish(cloud_msg);
3. Complete code
// 用来获取点云数据
#include <iostream>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// 用来得到云数据
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
// 用来 点云分割
#include <pcl/filters/passthrough.h>
// 用来实现 欧式距离聚类
#include <pcl/segmentation/extract_clusters.h>
// 用来拟合圆圈
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
// 画圆
#include <math.h>
using std::placeholders::_1;
class My_lidar_listener : public rclcpp::Node
{
public:
/********构造函数******************/
My_lidar_listener() : Node("laser_scan_to_pointcloud_converter")
{
RCLCPP_INFO(this->get_logger(), "scan话题订阅者创建完成");
/********创建订阅者*************************************/
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", 10, std::bind(&My_lidar_listener::lidarCallback, this, std::placeholders::_1));
/********创建发布者*************************************/
pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_coud", 10); //加::Ptr>报错
}
/********把雷达得到的 LaserScan数据 转换从成 点云的PointXYZ数据*******************************************/
pcl::PointCloud<pcl::PointXYZ>::Ptr My_scan_to_PointXYZ(const sensor_msgs::msg::LaserScan::SharedPtr msg, pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud);
/********定义函数对点云数据进行裁减**********************/
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_clipping(pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,float xlim_min,float xlim_max,float ylim_min,float ylim_max);
/********定义圆拟合函数,实现对圆的 圆心坐标 半径大小 输出*******************************************/
void My_circle_fitting(const pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,
double ¢er_x, double ¢er_y, double &radius);
private:
void lidarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
/**************将激光扫描数据转换为点云******************************/
// 使用PCL(Point Cloud Library)库来创建一个点云(PointCloud)对象,并初始化了一个名为cloud的指针,指向这个点云对象。//创建点云对象:使用PCL库创建一个空的点云对象
pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud(new pcl::PointCloud<pcl::PointXYZ>);
m_cloud = this->My_scan_to_PointXYZ(msg, m_cloud);
/// number = m_cloud->points.size(); 这将返回m_cloud中存储的点云点的数量
// x = m_cloud->points.at(1).x; 查看点云中点 1 的 x轴 的坐标/
// m_cloud->points.push_back()
/********对点云数据进行裁减******************/
m_cloud = this->pcl_clipping(m_cloud, -10, 10, -10, 10);
/********利于欧式距离进行聚类******************/
/*
这里需要根据实际的情况进行参数的调整
setClusterTolerance:表示两个点之间的最大距离,超过这个距离的点将不会被分到同一个群集中。
setMinClusterSize :表示 最小多少个点云才可以视为一个类的
setMaxClusterSize :表示 在一个点云中最大对少个点才可以视为一个类
*/
pcl::EuclideanClusterExtraction<pcl::PointXYZ> European_clustering;
European_clustering.setInputCloud(m_cloud);
European_clustering.setClusterTolerance(0.025); // 设置聚类的欧式距离阈值
European_clustering.setMinClusterSize(50); // 设置最小聚类点数
European_clustering.setMaxClusterSize(25000); // 设置最大聚类点数
// European_clustering.setSearchMethod(tree);
// 这个类用于存储欧氏聚类算法执行后的聚类结果。可以通过访问 cluster_indices 向量来获取每个簇的点的索引
std::vector<pcl::PointIndices> cluster_indices;
European_clustering.extract(cluster_indices);
// 看一个一共聚成了多少个类
// std::cout<<"点的个数"<<m_cloud->points.size()<<
// "聚成的类的个数"<<cluster_indices.size()<<std::endl;
// 下面这个操作是滤出一些离散的点 当点聚集的数量少于 setMinClusterSize 这些点就会被剔除
// 创建一个新的点云对象来存储所有的点云簇
pcl::PointCloud<pcl::PointXYZRGB>::Ptr clustered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//圆的参数
double center_x, center_y, radius;
// 遍历所有的点云簇
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
{
// 这个用来拟合圆
pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud_circle(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointXYZRGB color_point;
color_point.r = 0; // 红色
color_point.g = 100;
color_point.b = 200;
// 遍历当前点云簇中的所有点索引
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
{
// 从原始点云中将 点云簇 添加到新的点云对象中
pcloud_circle->points.push_back(m_cloud->points[*pit]);
// 从原始点云中将点添加到新的点云对象中
color_point.x = m_cloud->points[*pit].x;
color_point.y = m_cloud->points[*pit].y;
color_point.z = 0;
clustered_cloud->points.push_back(color_point);
}
// 设置 新点云簇 宽度和高度(如果有需要)
pcloud_circle->width = pcloud_circle->points.size();
pcloud_circle->height = 1; // 因为是无序点云,高度设置为1
/***********执行圆的拟合**************************************/
this->My_circle_fitting(pcloud_circle,center_x,center_y,radius); // 对点云簇进行圆形拟合 并判哪个是要识别对象 邦把圆心坐标和半径 通过引用的方式和传出来
// 把原有的数据簇给清空然后开始储存下一轮的数据
pcloud_circle->clear();
}
// 设置新点云的宽度和高度(如果有需要)
clustered_cloud->width = clustered_cloud->points.size();
clustered_cloud->height = 1; // 因为是无序点云,高度设置为1
/*********** 画圆圈 **************************************/
// 将圆上的点添加到PointCloud中,并设置颜色
for (double angle = 0; angle <= 2 * M_PI; angle += 0.01) {
pcl::PointXYZRGB point;
point.x = center_x + radius * cos(angle);
point.y = center_y + radius * sin(angle);
point.z = 0.0;
// 设置每个点的不同颜色
point.r = 255; // 红色
point.g = 0;
point.b = 0;
clustered_cloud->push_back(point);
}
/************将PointXYZ点云数据转换为PointCloud2消息类型********并把消息发布出去**********/
sensor_msgs::msg::PointCloud2 cloud_msg; //这将创建一个PointCloud2类型的ROS 2消息指针cloud_msg,用于存储转换后的点云数据。
pcl::toROSMsg(*clustered_cloud, cloud_msg); // 将ROS消息头转换为PCL消息头
cloud_msg.header = msg->header; // 设置PointCloud2消息的头信息
pointcloud_publisher_->publish(cloud_msg); // 发布点云数据
}
/************ 一些类的 实例化 *****************/
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;
};
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<My_lidar_listener>());
rclcpp::shutdown();
return 0;
}
/********把雷达得到的 LaserScan数据 转换从成 点云的PointXYZ数据*******************************************/
pcl::PointCloud<pcl::PointXYZ>::Ptr My_lidar_listener::My_scan_to_PointXYZ(const sensor_msgs::msg::LaserScan::SharedPtr my_msg, pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud)
{
// 获取激光扫描的相关信息
float angle_min = my_msg->angle_min;
float angle_increment = my_msg->angle_increment;
for (size_t i = 0; i < my_msg->ranges.size(); ++i)
{
float range = my_msg->ranges[i];
// 计算当前点的角度 //angle_increment 激光扫描角度的增量(弧度)
float angle = angle_min + i * angle_increment; //angle_min:激光扫描的起始角度(弧度)
// 计算当前点的坐标
pcl::PointXYZ point;
point.x = range * std::cos(angle);
point.y = range * std::sin(angle);
point.z = 0.0; // 在这个示例中,将所有点的z坐标设置为0
// 将点添加到点云中
my_cloud->push_back(point);
}
return my_cloud;
}
/********定义函数对点云数据进行裁减**********************/
pcl::PointCloud<pcl::PointXYZ>::Ptr My_lidar_listener::pcl_clipping(pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,float xlim_min,float xlim_max,float ylim_min,float ylim_max)
{
//创建x轴裁剪对象
pcl::PassThrough<pcl::PointXYZ> passX;
passX.setInputCloud(my_cloud);
passX.setFilterFieldName("x");
passX.setFilterLimits(xlim_min, xlim_max); //裁剪保留区域 -8m-8m
passX.filter(*my_cloud);
//创建y轴裁剪对象
pcl::PassThrough<pcl::PointXYZ> passY;
passY.setInputCloud(my_cloud);
passY.setFilterFieldName("y");
passY.setFilterLimits(ylim_min, ylim_max); //裁剪保留区域 -8m-8m
passY.filter(*my_cloud);
return my_cloud;
}
/**********圆形拟合***************************************/
void My_lidar_listener::My_circle_fitting(const pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,
double ¢er_x, double ¢er_y, double &radius)
{
// 使用体素网格降采样以加快计算(可选)
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud(my_cloud); /******************参数替换为自己的点云数据集********************/
vg.setLeafSize(0.01, 0.01, 0.01); // 设置体素网格的大小
vg.filter(*my_cloud); /******************参数替换为自己的点云数据集合********************/
// 创建分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
// 设置模型类型(这里使用圆模型)
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_CIRCLE2D); // 或者使用SACMODEL_CIRCLE3D
seg.setMethodType(pcl::SAC_RANSAC);
// 设置RANSAC的最大迭代次数和拟合阈值
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.01);
// 执行分割
seg.setInputCloud(my_cloud); /******************参数替换自己的点云数据集********************/
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
std::cerr << "Failed to estimate a circle for the given dataset." << std::endl;
return ;
}
float fitting_RMSE = 0;
float X_tall = 0;
float Y_tall = 0;
for(int point_number = 0; point_number < my_cloud->points.size(); point_number++ )
{
X_tall = X_tall + std::pow((my_cloud->points[point_number].x - coefficients->values[0]), 2);
Y_tall = Y_tall + std::pow((my_cloud->points[point_number].y - coefficients->values[1]), 2);
}
fitting_RMSE = std::sqrt(X_tall + Y_tall) / my_cloud->points.size();
if(coefficients->values[2] * 100 >= 18 && coefficients->values[2] * 100 <= 22 && fitting_RMSE < 0.030)
{
// coefficients->values[0] 是圆心x坐标
// coefficients->values[1] 是圆心y坐标
// coefficients->values[2] 是圆半径
// 这里需要加上逻辑判断,判断这个这次拟合出来的 圆 是否符合
std::cout << "拟合的圆形参数: " <<"圆心X坐标:"<< coefficients->values[0] * 100
<< ", 圆心Y坐标:" << coefficients->values[1] * 100
<<", 圆的半径:" << coefficients->values[2] * 100<< "cm"
<<"拟合的误差:" << fitting_RMSE <<std::endl;
center_x = coefficients->values[0] ;
center_y = coefficients->values[1] ;
radius = coefficients->values[2] ;
}
}