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

最新推荐文章于 2025-11-15 17:52:44 发布

原创

最新推荐文章于 2025-11-15 17:52:44 发布 · 4.6k 阅读

51 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #leetcode

本文详细介绍了激光雷达点云数据的处理流程，包括坐标转换、地面检测和平面拟合，以及点云的KD树聚类和DBSCAN聚类算法。通过这些方法，可以有效地分割和识别点云中的地面和障碍物，为自动驾驶提供关键信息。

1.realsense点云坐标转换

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

1.1、绕轴旋转矩阵

1.2、角度正负判断：

1.3、左乘右乘判断：

动坐标系和静坐标系

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

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

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

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

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

，之后再平移的过程。

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

绕世界坐标系的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<VPoint> point cloud
Others: 
***************************************************************************************/
pcl::PointCloud<VPoint> 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<pcl::PCLPointCloud2> sor;//滤波
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.1f, 0.1f, 0.1f);//0.01 1cm
    sor.filter (cloud_filtered);
    pcl::PointCloud<VPoint> laserCloudIn_trans;
    pcl::fromPCLPointCloud2(cloud_filtered,laserCloudIn_trans);
    pcl::PointCloud<VPoint> 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 clouds pcl点云平面提取检测RANSAC_肥鼠路易的博客-优快云博客_ransac平面检测
 点云平面检测GitHub - muratkendir