激光雷达障碍物检测与追踪实战——基于欧几里德聚类的激光雷达障碍物检测_激光雷达障碍物检测算法需要处理什么

最新推荐文章于 2024-09-10 09:00:16 发布

2401_84975843

最新推荐文章于 2024-09-10 09:00:16 发布

阅读量1.5k

点赞数 12

文章标签： c语言 c++ 学习

本文链接：https://blog.csdn.net/2401_84975843/article/details/138983472

版权

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

        {
            cloud_segments_array[1]->points.push\_back(current_point);
        }
        else if (origin_distance < clustering_ranges_[2])
        {
            cloud_segments_array[2]->points.push\_back(current_point);
        }
        else if (origin_distance < clustering_ranges_[3])
        {
            cloud_segments_array[3]->points.push\_back(current_point);
        }
        else
        {
            cloud_segments_array[4]->points.push\_back(current_point);
        }
    }

    std::vector<std::thread> thread\_vec(cloud_segments_array.size());
    for (unsigned int i = 0; i < cloud_segments_array.size(); i++)
    {
        std::promise<std::vector<pcl::PointIndices>> promiseObj;
        std::shared_future<std::vector<pcl::PointIndices>> futureObj = promiseObj.get\_future();
        thread_vec[i] = std::thread(&EuclideanCluster::clusterIndicesMultiThread, this, cloud_segments_array[i], std::ref(clustering_distances_[i]), std::ref(promiseObj));
        clusterIndices = futureObj.get();
        for (int j = 0; j < clusterIndices.size(); j++)
        {
            //每次聚类得出的indices为输入点云对应的索引
            pcl::PointCloud<pcl::PointXYZI>::Ptr temp\_cluster(new pcl::PointCloud<pcl::PointXYZI>);
            pcl::copyPointCloud(\*cloud_segments_array[i], clusterIndices[j], \*temp_cluster);
            \*outCloudPtr += \*temp_cluster;
            points_vector.push\_back(temp_cluster);
        }
    }
    for (int i = 0; i < thread_vec.size(); i++)
    {
        thread_vec[i].join();
    }
}

}


## 计算包围框


先介绍下包围框  
 ![在这里插入图片描述](https://img-blog.csdnimg.cn/01e6b5dea7ac4c9bb7f1cd56d118d4b3.png#pic_center)


包围框的定义：


* 包围框指一个简单几何空间，在三维点云中，里面包含一系列点集，为点集构建包围框能够有效提取障碍物几何属性给跟踪模块作为观测值。  
 包围框分类：
* 轴对齐包围框：包围框的三条边与坐标轴平行，无方向，目标有旋转时会增大间隙
* 方向包围框：根据目标几何形状来决定包围框大小和方向，也就是计算最小外接斜矩形，常用方法：PCA算法和L-shape拟合
* 点云凸包：最小凸多边形也叫凸包，就是将最外层点链接起来，包含所有的点集，常用算法Granham扫描法


此代码流程：


1. 聚类并计算障碍物中心，和Bounding Box  
 五个点云分别使用不同的半径阈值进行欧几里德聚类，对聚类完以后的一个个点云簇，我们计算其形心作为该障碍物的中心，同时计算点云簇的长宽高，从而确定一个能够将点云簇包裹的三维Bounding Box
2. 凸多边形拟合，优化bounding box的中心和长宽
3. 计算bounding box朝向  
 采用拟合出的凸多边型的顶点计算点云轮廓最小外界矩形，是一个Box2D结构，包含最小外接矩形的中心、宽高、旋转角度（水平轴（x轴）逆时针旋转，与碰到的矩形的第一条边的夹角）

void BoundingBox::SetCloud(std_msgs::Header header, const pcl::PointCloudpcl::PointXYZI::Ptr in, bool in_estimate_pose)
{

// extract pointcloud using the indices
// calculate min and max points
pcl::PointCloudpcl::PointXYZRGB::Ptr currentCluster(new pcl::PointCloudpcl::PointXYZRGB);
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();
float average_x = 0, average_y = 0, average_z = 0;

for (int i = 0; i < in->points.size(); i++)
{
// fill new colored cluster point by point
pcl::PointXYZRGB p;
p.x = in->points[i].x;
p.y = in->points[i].y;
p.z = in->points[i].z;

average_x += p.x;
average_y += p.y;
average_z += p.z;
centroid_.x += p.x;
centroid_.y += p.y;
centroid_.z += p.z;
currentCluster->points.push\_back(p);

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
minPoint_.x = min_x;
minPoint_.y = min_y;
minPoint_.z = min_z;
maxPoint_.x = max_x;
maxPoint_.y = max_y;
maxPoint_.z = max_z;

// calculate centroid, average
if (in->points.size() > 0)
{
centroid_.x /= in->points.size();
centroid_.y /= in->points.size();
centroid_.z /= in->points.size();

average_x /= in->points.size();
average_y /= in->points.size();
average_z /= in->points.size();

}

averagePoint_.x = average_x;
averagePoint_.y = average_y;
averagePoint_.z = average_z;

// calculate bounding box
float length_ = maxPoint_.x - minPoint_.x;
float width_ = maxPoint_.y - minPoint_.y;
float height_ = maxPoint_.z - minPoint_.z;

boundingBox_.header = header;

boundingBox_.pose.position.x = minPoint_.x + length_ / 2;
boundingBox_.pose.position.y = minPoint_.y + width_ / 2;
boundingBox_.pose.position.z = minPoint_.z + height_ / 2;

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

// pose estimation
double rz = 0;

std::vectorcv::Point2f points;
for (unsigned int i = 0; i < currentCluster->points.size(); i++)
{
cv::Point2f pt;
pt.x = currentCluster->points[i].x;
pt.y = currentCluster->points[i].y;
points.push_back(pt);
}

std::vectorcv::Point2f hull;
cv::convexHull(points, hull);

polygon_.header = header;
for (size_t i = 0; i < hull.size() + 1; i++)
{
geometry_msgs::Point32 point;
point.x = hull[i % hull.size()].x;
point.y = hull[i % hull.size()].y;
point.z = minPoint_.z;
polygon_.polygon.points.push_back(point);
}

if (in_estimate_pose)
{
cv::RotatedRect box = minAreaRect(hull);
rz = box.angle * 3.14 / 180;
boundingBox_.pose.position.x = box.center.x;
boundingBox_.pose.position.y = box.center.y;
boundingBox_.dimensions.x = box.size.width;
boundingBox_.dimensions.y = box.size.height;
}

// set bounding box direction
tf::Quaternion quat = tf::createQuaternionFromRPY(0.0, 0.0, rz);

/** \brief convert Quaternion to Quaternion msg*/
tf::quaternionTFToMsg(quat, boundingBox_.pose.orientation);

currentCluster->width = currentCluster->points.size();
currentCluster->height = 1;
currentCluster->is_dense = true;

// Get EigenValues, eigenvectors
if (currentCluster->points.size() > 3)
{
pcl::PCApcl::PointXYZ currentClusterPca;
pcl::PointCloudpcl::PointXYZ::Ptr current_cluster_mono(new pcl::PointCloudpcl::PointXYZ);

pcl::copyPointCloud(\*currentCluster, \*current_cluster_mono);

currentClusterPca.setInputCloud(current_cluster_mono);
eigenVectors_ = currentClusterPca.getEigenVectors();
eigenValues_ = currentClusterPca.getEigenValues();

}

validCluster_ = true;
pointCloud_ = currentCluster;
}


4.对可能的bounding\_box进行合并

void BoundingBox::getBoundingBox(std_msgs::Header header,
std::vector<pcl::PointCloudpcl::PointXYZI::Ptr> &points_vector,
autoware_msgs::CloudClusterArray &inOutClusters)
{
std::vector Clusters;
for (int i = 0; i < points_vector.size(); i++)
{
// pcl::PointCloudpcl::PointXYZI::Ptr temp_cluster(new pcl::PointCloudpcl::PointXYZI);
// pcl::copyPointCloud(*in, it->indices, *temp_cluster);
// *outCloudPtr += *temp_cluster;

BoundingBoxPtr cluster(new BoundingBox());
cluster->SetCloud(header, points_vector[i], inEstimatePose_);
Clusters.push\_back(cluster);

}

// Clusters can be merged or checked in here
// check for mergable clusters
std::vector midClusters;
std::vector finalClusters;

if (Clusters.size() > 0)
checkAllForMerge(header, Clusters, midClusters, clusterMergeThreshold_, inEstimatePose_);
else
midClusters = Clusters;

if (midClusters.size() > 0)
checkAllForMerge(header, midClusters, finalClusters, clusterMergeThreshold_, inEstimatePose_);
else
finalClusters = midClusters;

// Get final PointCloud to be published
for (unsigned int i = 0; i < Clusters.size(); i++)
{
if (Clusters[i]->validCluster_)
{
autoware_msgs::CloudCluster cloudCluster;
Clusters[i]->ToROSMessage(header, cloudCluster);
inOutClusters.clusters.push_back(cloudCluster);
}
}

inOutClusters.header = header;
}




![img](https://img-blog.csdnimg.cn/img_convert/8ff40eb5536d8d9ca8e50b0e84526318.png)
![img](https://img-blog.csdnimg.cn/img_convert/189d95f1359b782362002213d74625cb.png)

**网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。**

**[需要这份系统化的资料的朋友，可以添加戳这里获取](https://bbs.csdn.net/topics/618668825)**


**一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！**

181)]
[外链图片转存中...(img-oQ9AmWJK-1715884623181)]

**网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。**

**[需要这份系统化的资料的朋友，可以添加戳这里获取](https://bbs.csdn.net/topics/618668825)**


**一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！**