学习笔记：速腾聚创激光点云处理——地面分割和聚类

前言

激光雷达尝试中，已跑通，参数和效果还有待调整和验证
实践了大佬文章的代码
链接: link.
链接: link.
由于是新手小白也参考了下方文章
链接: link.

硬件和软件平台

系统：Ubuntu18.04+ros
硬件：速腾聚创Helios 32线激光雷达

点云的地面分割

1. filtered_points
   起初是根据大佬的文章一步一步自己建空间，建launch文件，由于不是velodyne雷达，需要在pcl_test_core.cpp里对雷达发出的数据名称进行更改，更改后如下：

#include "pcl_test_core.h"

PclTestCore::PclTestCore(ros::NodeHandle &nh){
    sub_point_cloud_ = nh.subscribe("/rslidar_points",10, &PclTestCore::point_cb, this);

    pub_filtered_points_ = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);

    ros::spin();
}

PclTestCore::~PclTestCore(){}

void PclTestCore::Spin(){
    
}

void PclTestCore::point_cb(const sensor_msgs::PointCloud2ConstPtr & in_cloud_ptr){
    pcl::PointCloud<pcl::PointXYZI>::Ptr current_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);

    pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr);

    pcl::VoxelGrid<pcl::PointXYZI> vg;

    vg.setInputCloud(current_pc_ptr);
    vg.setLeafSize(0.2f, 0.2f, 0.2f);
    vg.filter(*filtered_pc_ptr);

    sensor_msgs::PointCloud2 pub_pc;
    pcl::toROSMsg(*filtered_pc_ptr, pub_pc);

    pub_pc.header = in_cloud_ptr->header;

    pub_filtered_points_.publish(pub_pc);
}

更改并编译后能够跑通显示出 /filtered_points。

2. 地面点区分
   想实现地面点的区分，有两点需要注意的：
   第一是需要更改激光雷达参数，注意头文件include/pcl_test_core.h 跟之前做filter是不一样的，更改后如下：

#pragma once

#include <ros/ros.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>

#include <pcl/filters/extract_indices.h>

#include <sensor_msgs/PointCloud2.h>

#define CLIP_HEIGHT 0.2 //截取掉高于雷达自身0.2米的点 origin 0.2 change to 0.5
#define MIN_DISTANCE 0.2   
#define RADIAL_DIVIDER_ANGLE 0.18  
#define SENSOR_HEIGHT 1.1   //need chang to your height

#define concentric_divider_distance_ 0.01 //0.1 meters default
#define min_height_threshold_ 0.05
#define local_max_slope_ 8   //max slope of the ground between points, degree
#define general_max_slope_ 5 //max slope of the ground in entire point cloud, degree
#define reclass_distance_threshold_ 0.2

class PclTestCore
{

private:
  ros::Subscriber sub_point_cloud_;

  ros::Publisher pub_ground_, pub_no_ground_;

  struct PointXYZIRTColor
  {
    pcl::PointXYZI point;

    float radius; //cylindric coords on XY Plane
    float theta;  //angle deg on XY plane

    size_t radial_div;     //index of the radial divsion to which this point belongs to
    size_t concentric_div; //index of the concentric division to which this points belongs to

    size_t original_index; //index of this point in the source pointcloud
  };
  typedef std::vector<PointXYZIRTColor> PointCloudXYZIRTColor;

  size_t radial_dividers_num_;
  size_t concentric_dividers_num_;

  void point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud);

  void clip_above(double clip_height, const pcl::PointCloud<pcl::PointXYZI>::Ptr in, const pcl::PointCloud<pcl::PointXYZI>::Ptr out);

  void remove_close_pt(double min_distance, const pcl::PointCloud<pcl::PointXYZI>::Ptr in, const pcl::PointCloud<pcl::PointXYZI>::Ptr out);

  void XYZI_to_RTZColor(const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
                        PointCloudXYZIRTColor &out_organized_points,
                        std::vector<pcl::PointIndices> &out_radial_divided_indices,
                        std::vector<PointCloudXYZIRTColor> &out_radial_ordered_clouds);

  void classify_pc(std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds,
                   pcl::PointIndices &out_ground_indices,
                   pcl::PointIndices &out_no_ground_indices);

  void publish_cloud(const ros::Publisher &in_publisher,
                     const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud_to_publish_ptr,
                     const std_msgs::Header &in_header);

public:
  PclTestCore(ros::NodeHandle &nh);
  ~PclTestCore();
  void Spin();
};

第二是若之前在做第一步时有编译，需要将之前编译得到的devel和build文件都删掉，重新编译
最后能够在rviz中加载得到filtered_points_ground和/filtered_points_no_ground两个区分开的点云，在rviz中对两个topic分别设置颜色即可得到如下的图

3. 物体聚类
   物体聚类是直接下载的大佬的程序，编译通过后直接可以跑，最开始一直找不到BoundingBox，从rviz的topic中是一个不可加载的topic，一直以为是不是程序哪里设置错误跑出来没结果，最后发现是插件没装= =

sudo apt-get install ros-melodic-jsk-recognition-msgs 
sudo apt-get install ros-melodic-jsk-rqt-plugins 
sudo apt-get install ros-melodic-jsk-visualization

一通安装后，运行在rviz中通过By display type添加上BoundingBoxArray，然后在主界面rviz中选上topic好像是叫detected_bounding_box即可。

写在最后

目前只是对此方法进行了实施，就目前的极少量数据来看通过此方法的地面分割做的还是可以的，但是不知道在上坡和下坡的时候表现怎么样。
对于物体聚类来讲，就目前的极少量数据来看在遇到倾斜的物体时物体框选会有一定的问题，如下图所示，算法的框是在一个方向的，没有倾斜的框，这样的话在出现倾斜方向障碍物的时候就会对障碍物距离判断出现问题。

标签：速腾,const,pc,points,pcl,聚创,点云,ros,ptr
来源： https://blog.csdn.net/m0_53567875/article/details/120206355