realsense ros 三维点云地面检测与障碍物聚类

 1.realsense点云坐标转换

  链接：机器人运动学坐标变换 - 百度文库

1.1、绕轴旋转矩阵

1.2、角度正负判断：

1.3、左乘右乘判断：

动坐标系和静坐标系

例子：静坐标系，选择左乘

realsense 采集的深度点云，转换为世界坐标系 

 浅析相机相关坐标系的相互转换（世界坐标系、相机坐标系、图像坐标系、像素坐标系、内参矩阵、外参矩阵、扭转因子）【相机标定&计算机视觉】_土豪gold的博客-CSDN博客

三维视觉基础之世界坐标系、相机坐标系、图像坐标系和像素坐标系之间的转换关系_jiangxing11的博客-CSDN博客

从世界坐标系到相机坐标系，旋转顺序任意

• 常规欧拉角      (Z-X-Z, X-Y-X, Y-Z-Y, Z-Y-Z, X-Z-X, Y-X-Y)

，之后再平移的过程。

三维空间坐标系变换-旋转矩阵_fireflychh的博客-CSDN博客_坐标变换矩阵

绕世界坐标系的Z轴顺时针旋转90度，在将旋转后的坐标系绕其x轴旋转90度，坐标系变换需右乘，逆正顺负。

则：R（Z，-90）×R（X，-90）

=（0 1 0；-1 0 0；0 0 1）×（1 0 0；0 0 1；0 -1 0）=（0 0 1 ；-1 0 0；0 -1 0）

Xw=Zc；Yw=-Xc;Zw=-Yc

进行滤波处理，选取一定高度和深度。

/**************************************************************************************
Function: coordinate transformation
Description: This function in order to  transform coordinate
Calls: none
Called By: PlaneGroundFilter::point_cb()
Input: 
	Point cloud of "/camera/depth/color/points" 
Output: 
    Point cloud of target coordinate system
Return: 
	pcl::PointCloud point cloud
Others: 
***************************************************************************************/
pcl::PointCloud PlaneGroundFilter::coordinate_transformation(const sensor_msgs::PointCloud2ConstPtr &incloud_ptr)
{
    // Container for original & filtered data
    pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
    pcl::PCLPointCloud2 cloud_filtered; //滤波后数据储存
    // Convert to PCL data type
    pcl_conversions::toPCL(*incloud_ptr, *cloud);
    // Perform the actual filtering
    pcl::VoxelGrid sor;//滤波
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1f, 0.1f, 0.1f);//0.01 1cm
    sor.filter (cloud_filtered);
    pcl::PointCloud laserCloudIn_trans;
    pcl::fromPCLPointCloud2(cloud_filtered,laserCloudIn_trans);
    pcl::PointCloud P_TransOut=laserCloudIn_trans;
    // For mark ground points and hold all points
    SLRPointXYZIRL point;
    for (size_t i = 0; i < laserCloudIn_trans.points.size(); i++)
    {   
     if(laserCloudIn_trans[i].z<6.0&&laserCloudIn_trans[i].y<2.0){
        P_TransOut.points[i].x=laserCloudIn_trans[i].z;
        point.x = laserCloudIn_trans[i].z;
        P_TransOut.points[i].y=-laserCloudIn_trans[i].x;
        point.y = -laserCloudIn_trans[i].x;      
        P_TransOut.points[i].z=-laserCloudIn_trans[i].y;
        point.z = -laserCloudIn_trans[i].y;       
        point.label = 0u; // 0 means uncluster
        g_all_pc->points.push_back(point);}
    }
    return P_TransOut;
}

2.地面检测算法

点云平面检测GitHub - muratkendir/PlaneDetectionwPCL: Uses some filters and running RANSAC algorithm to detect planes in any kind of point cloud data, finally saves as seperate point clouds

地面拟合法Run_based_segmentation/scanlinerun.cpp at master · VincentCheungM/Run_based_segmentation · GitHub

点云平面检测GitHub - muratkendir/PlaneDetectionwPCL: Uses some filters and running RANSAC algorithm to detect planes in any kind of point cloud data, finally saves as seperate point cloudspcl点云平面提取检测RANSAC_肥鼠路易的博客-CSDN博客_ransac平面检测
点云平面检测GitHub - muratkendir/PlaneDetectionwPCL: Uses some filters and running RANSAC algorithm to detect planes in any kind of point cloud data, finally saves as seperate point clouds

地面检测算法，平面拟合法

参考：无人驾驶汽车系统入门（二十七）——基于地面平面拟合的激光雷达地面分割方法和ROS实现_AdamShan的博客-CSDN博客​​​​​​

#include "plane_ground_filter_core.h"

/**************************************************************************************
Function: point_cmp;
Description: In order to sort the point cloud, compare the size of the Z coordinates of the two input values;
Calls: none;
Called By: none;
Input: 
	VPoint a, VPoint b;
Output: 
    pcl::PointCloud::Ptr;
Return: 
	true:a.zb.z;
Others: 
***************************************************************************************/
bool point_cmp(VPoint a, VPoint b)
{
    return a.z < b.z;
}
/**************************************************************************************
Function: PlaneGroundFilter;
Description: Class to create nodes and pass parameters;
Calls: none ;
Called By: This function is automatically executed when class members are built;
Input: 
	ros::NodeHandle &nh,
    nh.subscribe("/camera/depth/color/points", 10, &PlaneGroundFilter::point_cb, this);
Output: none;
Return: none;
Others: main
***************************************************************************************/
PlaneGroundFilter::PlaneGroundFilter(ros::NodeHandle &nh)
{
    std::string input_topic;
    nh.getParam("input_topic", input_topic);
    sub_point_cloud_ = nh.subscribe("/camera/depth/color/points", 10, &PlaneGroundFilter::point_cb, this);

    // init publisher
    std::string no_ground_topic, ground_topic, all_points_topic;

    nh.getParam("no_ground_point_topic", no_ground_topic);
    nh.getParam("ground_point_topic", ground_topic);
    nh.getParam("all_points_topic", all_points_topic);

    nh.getParam("clip_height", clip_height_);
    ROS_INFO("clip_height: %f", clip_height_);
    nh.getParam("sensor_height", sensor_height_);
    ROS_INFO("sensor_height: %f", sensor_height_);
    nh.getParam("min_distance", min_distance_);
    ROS_INFO("min_distance: %f", min_distance_);
    nh.getParam("max_distance", max_distance_);
    ROS_INFO("max_distance: %f", max_distance_);

    nh.getParam("sensor_model", sensor_model_);
    ROS_INFO("sensor_model: %d", sensor_model_);
    nh.getParam("num_iter", num_iter_);
    ROS_INFO("num_iter: %d", num_iter_);
    nh.getParam("num_lpr", num_lpr_);
    ROS_INFO("num_lpr: %d", num_lpr_);
    nh.getParam("th_seeds", th_seeds_);
    ROS_INFO("th_seeds: %f", th_seeds_);
    nh.getParam("th_dist", th_dist_);
    ROS_INFO("th_dist: %f", th_dist_);

    pub_ground_ = nh.advertise(ground_topic, 10);
    pub_no_ground_ = nh.advertise(no_ground_topic, 10);
    pub_all_points_ = nh.advertise(all_points_topic, 10);

    g_seeds_pc = pcl::PointCloud::Ptr(new pcl::PointCloud);
    g_ground_pc = pcl::PointCloud::Ptr(new pcl::PointCloud);
    g_not_ground_pc = pcl::PointCloud::Ptr(new pcl::PointCloud);
    g_all_pc = pcl::PointCloud::Ptr(new pcl::PointCloud);

    ros::spin();
}

PlaneGroundFilter::~PlaneGroundFilter() {}

void PlaneGroundFilter::Spin()
{
}
/**************************************************************************************
Function: clip_above;
Description: Remove points that are too high;
Calls: none;
Called By: none;
Input: 
	pcl::PointCloud::Ptr in,min_distance_,distance > max_distance_;
Output: 
    pcl::PointCloud::Ptr;
Return: 
	none;
Others: 
***************************************************************************************/
void PlaneGroundFilter::clip_above(const pcl::PointCloud::Ptr in,
                                   const pcl::PointCloud::Ptr out)
{
    pcl::ExtractIndices cliper;

    cliper.setInputCloud(in);
    pcl::PointIndices indices;
#pragma omp for
    for (size_t i = 0; i < in->points.size(); i++)
    {
        if (in->points[i].z > clip_height_)
        {
            indices.indices.push_back(i);
        }
    }
    cliper.setIndices(boost::make_shared(indices));
    cliper.setNegative(true); //ture to remove the indices
    cliper.filter(*out);
}
/**************************************************************************************
Function: remove_close_far_pt;
Description: Remove points that are too close and too far away;
Calls: none;
Called By: none;
Input: 
	pcl::PointCloud::Ptr in,min_distance_,distance > max_distance_;
Output: 
    pcl::PointCloud::Ptr;
Return: 
	none;
Others: 
***************************************************************************************/
void PlaneGroundFilter::remove_close_far_pt(const pcl::PointCloud::Ptr in,
                                            const pcl::PointCloud::Ptr out)
{
    pcl::ExtractIndices cliper;

    cliper.setInputCloud(in);
    pcl::PointIndices indices;
#pragma omp for 
    for (size_t i = 0; i < in->points.size(); i++)
    {
        double distance = sqrt(in->points[i].x * in->points[i].x + in->points[i].y * in->points[i].y);

        if ((distance < min_distance_) || (distance > max_distance_))
        {
            indices.indices.push_back(i);
        }
    }
    cliper.setIndices(boost::make_shared(indices));
    cliper.setNegative(true); //ture to remove the indices
    cliper.filter(*out);
}

/**************************************************************************************
Function: estimate_plane_;
Description: Fit the input point set to a plane;
Calls: none;
Called By: none;
Input: 
	g_ground_pc;
Output: 
    normal_ ,use the least singular vector as normal;
Return: 
	none;
Others: 
***************************************************************************************/
void PlaneGroundFilter::estimate_plane_(void)
{
    // 线性模型用于平面模型估计 ax+by+cz+d=0 即 normal.T * X.T = -d
    // 其中 normal = [a, b, c].T , X = [x, y, z].T

    // Create covarian matrix in single pass.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean;
    // 地面点集g_ground_pc, 求解协方差矩阵cov和点云均值pc_mean
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);

    // SVD奇异值分解协方差矩阵cov得到三个方向的奇异向量
    JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU);

    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2)); // 垂直于平面的法向量n, 表示具有最小方差的方向 (normal = [a, b, c].T)

    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>(); // 种子点集的平均值 (代表属于地面的点)

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose() * seeds_mean)(0, 0);

    // set distance threhold to `th_dist - d`
    th_dist_d_ = th_dist_ - d_;

    // return the equation parameters
}
/**************************************************************************************
Function: extract_initial_seeds_;
Description: Extract the initial ground point cloud according to the sorted point cloud;
Calls: none;
Called By: none;
Input: 
	sorted point cloud,pcl::PointCloud &p_sorted;
Output: 
    g_seeds_pc,return seeds points;
Return: 
	none;
Others: 
***************************************************************************************/
void PlaneGroundFilter::extract_initial_seeds_(const pcl::PointCloud &p_sorted)
{
    // LPR is the mean of low point representative
    // LPR: 最低点代表(num_lpr个最低高度点的平均值)
    double sum = 0;
    int cnt = 0;
    // Calculate the mean height value.
    for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; i++)
    {
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0
    ROS_INFO("I heard lpr_height: [%f]", lpr_height);
    g_seeds_pc->clear();
    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_
    for (int i = 0; i < p_sorted.points.size(); i++)
    {
        if (p_sorted.points[i].z < lpr_height + th_seeds_)//小于平均高度+种子阈值
        {
               g_seeds_pc->points.push_back(p_sorted.points[i]);
        }
    }
}
/**************************************************************************************
Function: post_process
Description: Point cloud after removing high points
Calls: 
       clip_above(),
       remove_close_far_pt;
Called By: none
Input: 
	pcl::PointCloud::Ptr in;
Output: 
    pcl::PointCloud::Ptr out;
Return: 
	none
Others: 
***************************************************************************************/
void PlaneGroundFilter::post_process(const pcl::PointCloud::Ptr in, const pcl::PointCloud::Ptr out)
{
    pcl::PointCloud::Ptr cliped_pc_ptr(new pcl::PointCloud);
    clip_above(in, cliped_pc_ptr);
    pcl::PointCloud::Ptr remove_close(new pcl::PointCloud);
    remove_close_far_pt(cliped_pc_ptr, out);
}
/**************************************************************************************
Function: coordinate transformation
Description: This function in order to  transform coordinate
Calls: none
Called By: PlaneGroundFilter::point_cb()
Input: 
	Point cloud of "/camera/depth/color/points" 
Output: 
    Point cloud of target coordinate system
Return: 
	pcl::PointCloud point cloud
Others: 
***************************************************************************************/
pcl::PointCloud PlaneGroundFilter::coordinate_transformation(const sensor_msgs::PointCloud2ConstPtr &incloud_ptr)
{
    // Container for original & filtered data
    pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
    pcl::PCLPointCloud2 cloud_filtered; //滤波后数据储存
    // Convert to PCL data type
    pcl_conversions::toPCL(*incloud_ptr, *cloud);
    // Perform the actual filtering
    pcl::VoxelGrid sor;//滤波
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1f, 0.1f, 0.1f);//0.01 1cm
    sor.filter (cloud_filtered);
    pcl::PointCloud laserCloudIn_trans;
    pcl::fromPCLPointCloud2(cloud_filtered,laserCloudIn_trans);
    pcl::PointCloud P_TransOut=laserCloudIn_trans;
    // For mark ground points and hold all points
    SLRPointXYZIRL point;
    for (size_t i = 0; i < laserCloudIn_trans.points.size(); i++)
    {   
     if(laserCloudIn_trans[i].z<6.0&&laserCloudIn_trans[i].y<2.0){
        P_TransOut.points[i].x=laserCloudIn_trans[i].z;
        point.x = laserCloudIn_trans[i].z;
        P_TransOut.points[i].y=-laserCloudIn_trans[i].x;
        point.y = -laserCloudIn_trans[i].x;      
        P_TransOut.points[i].z=-laserCloudIn_trans[i].y;
        point.z = -laserCloudIn_trans[i].y;       
        point.label = 0u; // 0 means uncluster
        g_all_pc->points.push_back(point);}
    }
    return P_TransOut;
}
/**************************************************************************************
Function: Subscription point cloud callback
Description: Process the received point cloud data
Calls: 
    pcl::VoxelGrid sor;Voxel filtering
    bool point_cmp(VPoint a, VPoint b)
    sort(laserCloudIn.points.begin(), laserCloudIn.points.end(), point_cmp);
    extract_initial_seeds_(laserCloudIn);
    post_process(g_not_ground_pc, final_no_ground);
Called By: 
    PlaneGroundFilter::PlaneGroundFilter(ros::NodeHandle &nh)
Input: 
	Point cloud of "/camera/depth/color/points"
Output: 
    pub_ground_.publish(ground_msg);
    pub_no_ground_.publish(groundless_msg);
    pub_all_points_.publish(all_points_msg);
Return: 
	none
Others: 
***************************************************************************************/
void PlaneGroundFilter::point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud_ptr)
{
    pcl::PointCloud laserCloudIn=coordinate_transformation(in_cloud_ptr),
                            laserCloudIn_org=laserCloudIn;
    sort(laserCloudIn.points.begin(), laserCloudIn.points.end(), point_cmp);//对laserCloudIn进行排序。排序方式为根据z的高度
    pcl::PointCloud::iterator it = laserCloudIn.points.begin();//创建容器迭代器，迭代器指向点云的开始，即最低点
    for (int i = 0; i < laserCloudIn.points.size(); i++){
        if (laserCloudIn.points[i].z < -2.0 * sensor_height_){ //如果点云的高度小于-1.5倍的安装高度，则迭代器加一
            it++;}
        else{
            break;}}
    if(it != laserCloudIn.points.begin()){
       laserCloudIn.points.erase(laserCloudIn.points.begin(), it);}//删除从开始到迭代器记录的值
    ROS_INFO("I heardz: [%f]", laserCloudIn.points[0].z);
    ROS_INFO("I heardendz: [%f]", laserCloudIn.points[50].z);
    ROS_INFO("I heardendsiz: [%d]", laserCloudIn.size());
    extract_initial_seeds_(laserCloudIn);
    g_ground_pc = g_seeds_pc; // 种子点集作为初始地面点集
    for (int i = 0; i < num_iter_; i++)
    {
        estimate_plane_();
        g_ground_pc->clear();
        g_not_ground_pc->clear();
        //pointcloud to matrix
        MatrixXf points(laserCloudIn_org.points.size(), 3);
        int j = 0;
        for (auto p : laserCloudIn_org.points){
            points.row(j++) << p.x, p.y, p.z;}
        VectorXf result = points * normal_; // 点云中每一个点到该平面的正交投影的距离
        // threshold filter
        for (int r = 0; r < result.rows(); r++)
        {
            if (result[r] < th_dist_d_&&laserCloudIn_org[r].z<-0.5) // 高度差小于阈值，认为该点属于地面
            {
                g_all_pc->points[r].label = 1u; // means ground
                g_ground_pc->points.push_back(laserCloudIn_org[r]); // 所有地面点被当作下一次迭代的种子点集
            }
            else
            {
                g_all_pc->points[r].label = 0u; // means not ground and non clusterred
                g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
        }
    }

    pcl::PointCloud::Ptr final_no_ground(new pcl::PointCloud);
    post_process(g_not_ground_pc, final_no_ground);
    
    // ROS_INFO_STREAM("origin: "<points.size()<<" post_process: "<points.size());

    // publish ground points
    sensor_msgs::PointCloud2 ground_msg;
    pcl::toROSMsg(*g_ground_pc, ground_msg);
    ground_msg.header.stamp = in_cloud_ptr->header.stamp;
    ground_msg.header.frame_id = in_cloud_ptr->header.frame_id;
    pub_ground_.publish(ground_msg);

    // publish not ground points
    sensor_msgs::PointCloud2 groundless_msg;
    pcl::toROSMsg(*final_no_ground, groundless_msg);
    groundless_msg.header.stamp = in_cloud_ptr->header.stamp;
    groundless_msg.header.frame_id = in_cloud_ptr->header.frame_id;
    pub_no_ground_.publish(groundless_msg);

    // publish all points
    sensor_msgs::PointCloud2 all_points_msg;
    pcl::toROSMsg(*g_all_pc, all_points_msg);
    all_points_msg.header.stamp = in_cloud_ptr->header.stamp;
    all_points_msg.header.frame_id = in_cloud_ptr->header.frame_id;
    pub_all_points_.publish(all_points_msg);
    g_all_pc->clear();
}

 全部点与地面点

 

 原始点

3.KD树聚类

无人驾驶汽车系统入门（二十五）——基于欧几里德聚类的激光雷达点云分割及ROS实现_AdamShan的博客-CSDN博客

#include "euclidean_cluster_core.h"

EuClusterCore::EuClusterCore(ros::NodeHandle &nh)
{

    seg_distance_ = {0.5, 1.0, 2.5, 4.5};//分割距离 
    cluster_distance_ = {0.1, 0.2, 0.2, 0.3, 0.3}; // 聚类半径

    //sub_point_cloud_ = nh.subscribe("/filtered_points_no_ground", 5, &EuClusterCore::point_cb, this);
    sub_point_cloud_ = nh.subscribe("/points_no_ground", 5, &EuClusterCore::point_cb, this);

    pub_bounding_boxs_ = nh.advertise("/detected_bounding_boxs", 5);

    ros::spin();
}

EuClusterCore::~EuClusterCore() {}

void EuClusterCore::publish_cloud(const ros::Publisher &in_publisher,
                                  const pcl::PointCloud::Ptr in_cloud_to_publish_ptr,
                                  const std_msgs::Header &in_header)
{
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
    cloud_msg.header = in_header;
    in_publisher.publish(cloud_msg);
}
/**************************************************************************************
Function: voxel_grid_filer;
Description: Down-sampling;
Calls: none;
Called By: none;
Input: 
	pcl::PointCloud::Ptr in，double leaf_size
Output: 
    pcl::PointCloud::Ptr out
Return: 
	none;
Others: 
***************************************************************************************/
void EuClusterCore::voxel_grid_filer(pcl::PointCloud::Ptr in, pcl::PointCloud::Ptr out, double leaf_size)
{
    pcl::VoxelGrid filter;
    filter.setInputCloud(in);
    filter.setLeafSize(leaf_size, leaf_size, leaf_size);
    filter.filter(*out);
}
/**************************************************************************************
Function: cluster_segment;
Description: Cluster and calculate the obstacle center and bounding box;
Calls: none;
Called By: none;
Input: 
	pcl::PointCloud::Ptr in_pc
Output: 
    std::vector &obj_list
Return: 
	none;
Others: 
***************************************************************************************/
void EuClusterCore::cluster_segment(pcl::PointCloud::Ptr in_pc,
                                    double in_max_cluster_distance, std::vector &obj_list)
{
    //创建Kd树对象tree
    pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);

    //create 2d pc
    pcl::PointCloud::Ptr cloud_2d(new pcl::PointCloud);
    pcl::copyPointCloud(*in_pc, *cloud_2d);
    //make it flat
    for (size_t i = 0; i < cloud_2d->points.size(); i++)
    {
        cloud_2d->points[i].z = 0;
    }

    if (cloud_2d->points.size() > 0)
        tree->setInputCloud(cloud_2d); //创建点云索引向量，用于存储实际的点云信息

    std::vector cluster_indices;

    pcl::EuclideanClusterExtraction euclid;
    euclid.setInputCloud(cloud_2d);
    euclid.setClusterTolerance(in_max_cluster_distance); //设置近邻搜索的聚类半径
    euclid.setMinClusterSize(MIN_CLUSTER_SIZE); //设置一个聚类需要的最少点数
    euclid.setMaxClusterSize(MAX_CLUSTER_SIZE); //设置一个聚类需要的最大点数
    euclid.setSearchMethod(tree); //设置点云的搜索机制(KD tree)
    euclid.extract(cluster_indices); //从点云中提取聚类，并将点云索引保存在cluster_indices中

    //迭代访问点云索引cluster_indices
    for (size_t i = 0; i < cluster_indices.size(); i++)
    {
        // the structure to save one detected object
        Detected_Obj obj_info;

        float min_x = std::numeric_limits::max();
        float max_x = -std::numeric_limits::max();
        float min_y = std::numeric_limits::max();
        float max_y = -std::numeric_limits::max();
        float min_z = std::numeric_limits::max();
        float max_z = -std::numeric_limits::max();

        for (auto pit = cluster_indices[i].indices.begin(); pit != cluster_indices[i].indices.end(); ++pit)
        {
            //fill new colored cluster point by point
            pcl::PointXYZ p;
            p.x = in_pc->points[*pit].x;
            p.y = in_pc->points[*pit].y;
            p.z = in_pc->points[*pit].z;

            obj_info.centroid_.x += p.x;
            obj_info.centroid_.y += p.y;
            obj_info.centroid_.z += p.z;

            if (p.x < min_x)
                min_x = p.x;
            if (p.y < min_y)
                min_y = p.y;
            if (p.z < min_z)
                min_z = p.z;
            if (p.x > max_x)
                max_x = p.x;
            if (p.y > max_y)
                max_y = p.y;
            if (p.z > max_z)
                max_z = p.z;
        }

        //min, max points
        obj_info.min_point_.x = min_x;
        obj_info.min_point_.y = min_y;
        obj_info.min_point_.z = min_z;

        obj_info.max_point_.x = max_x;
        obj_info.max_point_.y = max_y;
        obj_info.max_point_.z = max_z;

        //calculate centroid, average 质心
        if (cluster_indices[i].indices.size() > 0)
        {
            obj_info.centroid_.x /= cluster_indices[i].indices.size();
            obj_info.centroid_.y /= cluster_indices[i].indices.size();
            obj_info.centroid_.z /= cluster_indices[i].indices.size();
        }

        //calculate bounding box 外接矩
        double length_ = obj_info.max_point_.x - obj_info.min_point_.x;
        double width_ = obj_info.max_point_.y - obj_info.min_point_.y;
        double height_ = obj_info.max_point_.z - obj_info.min_point_.z;

        obj_info.bounding_box_.header = point_cloud_header_;

        obj_info.bounding_box_.pose.position.x = obj_info.min_point_.x + length_ / 2;
        obj_info.bounding_box_.pose.position.y = obj_info.min_point_.y + width_ / 2;
        obj_info.bounding_box_.pose.position.z = obj_info.min_point_.z + height_ / 2;

        obj_info.bounding_box_.dimensions.x = ((length_ < 0) ? -1 * length_ : length_);
        obj_info.bounding_box_.dimensions.y = ((width_ < 0) ? -1 * width_ : width_);
        obj_info.bounding_box_.dimensions.z = ((height_ < 0) ? -1 * height_ : height_);

        obj_list.push_back(obj_info);
    }
}
/**************************************************************************************
Function: cluster_by_distance;
Description: The scanned point cloud is divided into five point clouds according to its distance to the radar;
             cluster the pointcloud according to the distance of the points using different thresholds (not only one for the entire pc)
             in this way, the points farther in the pc will also be clustered
             //0 => 0-0.5m  d=0.2
             //1 => 0.5-1.5 d=0.4
             //2 => 1.5-2.5 d=0.6
             //3 => 2.5-3.5 d=0.8
             //4 => >3.5    d=1.0
Calls: none;
Called By: none;
Input: 
	pcl::PointCloud::Ptr in_pc
Output: 
    std::vector &obj_list
Return: 
	none;
Others: 
***************************************************************************************/
void EuClusterCore::cluster_by_distance(pcl::PointCloud::Ptr in_pc, std::vector &obj_list)
{
    std::vector::Ptr> segment_pc_array(5);
    for (size_t i = 0; i < segment_pc_array.size(); i++)
    {
        pcl::PointCloud::Ptr tmp(new pcl::PointCloud);
        segment_pc_array[i] = tmp;
    }
    for (size_t i = 0; i < in_pc->points.size(); i++)
    {
        pcl::PointXYZ current_point;
        current_point.x = in_pc->points[i].x;
        current_point.y = in_pc->points[i].y;
        current_point.z = in_pc->points[i].z;

        float origin_distance = sqrt(pow(current_point.x, 2) + pow(current_point.y, 2));
        // 如果点的距离大于6m, 忽略该点
        if (origin_distance >= 8.5)
        {
            continue;
        }
        if (origin_distance < seg_distance_[0])
        {
            segment_pc_array[0]->points.push_back(current_point);
        }
        else if (origin_distance < seg_distance_[1])
        {
            segment_pc_array[1]->points.push_back(current_point);
        }
        else if (origin_distance < seg_distance_[2])
        {
            segment_pc_array[2]->points.push_back(current_point);
        }
        else if (origin_distance < seg_distance_[3])
        {
            segment_pc_array[3]->points.push_back(current_point);
        }
        else
        {
            segment_pc_array[4]->points.push_back(current_point);
        }
    }

    std::vector final_indices;
    std::vector tmp_indices;

    for (size_t i = 0; i < segment_pc_array.size(); i++)
    {
        cluster_segment(segment_pc_array[i], cluster_distance_[i], obj_list);
    }
}

void EuClusterCore::point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud_ptr)
{
    pcl::PointCloud::Ptr current_pc_ptr(new pcl::PointCloud);
    pcl::PointCloud::Ptr filtered_pc_ptr(new pcl::PointCloud);

    point_cloud_header_ = in_cloud_ptr->header;

    pcl::fromROSMsg(*in_cloud_ptr, *current_pc_ptr);
    // down sampling the point cloud before cluster
    //voxel_grid_filer(current_pc_ptr, filtered_pc_ptr, LEAF_SIZE);//降采样在地面检测时，已经采取
    filtered_pc_ptr=current_pc_ptr;
    std::vector global_obj_list;
    cluster_by_distance(filtered_pc_ptr, global_obj_list);

    jsk_recognition_msgs::BoundingBoxArray bbox_array;

    for (size_t i = 0; i < global_obj_list.size(); i++)
    {
        bbox_array.boxes.push_back(global_obj_list[i].bounding_box_);
    }
    bbox_array.header = point_cloud_header_;

    pub_bounding_boxs_.publish(bbox_array);
}

Autoware感知瞎学笔记（一）lidar_kf_contour_track_YP233的博客-程序员信息网 - 程序员信息网

Autoware L4无人驾驶，三维激光雷达数据从传感器出来后，经过传统的聚类+高精地图object filter，或者learning-based的方法之后，得到一个目标列表，此目标列表含有二维位置信息和角度信息，没有速度信息；同时基于每一帧的检测方法，可能会出现“目标消失”的问题，因此需要滤波器来track住这些目标，下面看Autoware是怎么处理这个问题的。
代码位置：autoware/core_perception/lidar_kf_contour_track

4. 3d box姿态显示

realsense 视觉范围较激光雷达范围较小，3d box 为矩形，无法勾勒出障碍物边界，进行二维投影，进行凹凸包提取

PCL计算ConvexHull凸包、ConcaveHull凹包_com1098247427的博客-CSDN博客

concaveHull

5.利用PCL实现点云的DBSCAN聚类

代码来源：

https://github.com/danielTobon43/DBScan-PCL-Optimized

博客园的一篇DBSCAN算法：

比较简陋，运行起来非常慢

https://www.cnblogs.com/zlian2016/p/5617527.html

激光雷达点云地面分割（附带有测试的激光电云bag包）

https://blog.csdn.net/yuxuan20062007/article/details/82926783

pcl形态学滤波器实现地面点分割

https://blog.csdn.net/hanshuobest/article/details/73557125

github上的一个算法：（运行起来很快）

https://github.com/buresu/DBSCAN

github上的基于PCL的聚类方法（2019.5.30）

https://github.com/yzrobot/adaptive_clustering
原文链接：https://blog.csdn.net/weixin_42718092/article/details/86221246

//

使用Kdtree加速的DBSCAN进行点云聚类 - 简书

cuda dbscan GitHub - xmba15/generic_dbscan: generic DBSCAN on CPU & GPU

https://github.com/NVIDIA-AI-IOT/cuda-pcl

6.Bounding Boxes

自动驾驶激光雷达物体检测技术 - 知乎