<基于欧几里德聚类的激光雷达点云分割及ROS实现> 笔记
本文實現參考Adam的博客 基于歐幾里德聚類的激光雷達點云分割及ROS實現
點云聚類在激光雷達環境感知中的作用
就無人車的環境感知而言，方案很多，根據使用的傳感器的不同，算法也截然不同，有單純基于圖像視覺的方法，也有基于激光雷達的方法，激光雷達以其穩定可靠、精度高并且能同時應用于定位和環境感知而被廣泛采用。激光雷達環境感知一般的流程為：
1、分割地面，從而減少地面的點對目標檢測的影響
2、點云聚類，將目標按照點的分布進行聚類，從而降低后續計算的計算量
3、模式識別，對分割得到的點云進行特征提取，使用提取出來的特征訓練分類器進行模式識別，近年來深度學習取得進展，也有不少使用深度神經網絡的端到端檢測算法。
4、目標追蹤，在完成模式識別以后我們實際上已經得到了目標障礙物的類別（是行人還是車輛還是別的）、障礙物的輪廓（一個3維的bounding box）、障礙物的位置（障礙物形心相對于激光雷達的xyz坐標）。為了方便規劃模塊完成對障礙物的預測，我們需要建立障礙物在前后幀（來自傳感器的前后兩次信號）的關系，即需要給障礙物一個ID，并且能夠持續追蹤這個障礙物，在目標追蹤中，我們前面介紹的卡爾曼濾波、擴展卡爾曼濾波和無損卡爾曼濾波被廣泛使用。
當然，根據使用的傳感器的不同，在點云聚類完成以后，對障礙物進一步的模式識別通常有兩種做法：
● 直接針對分割出來的點云進行模式識別
● 使用相機對目標進行模式識別（圖像），將相機和雷達進行聯合標定，將相機得到的檢測目標投射到3維的點云空間，融合圖像檢測和點云聚類的結果實現目標檢測和分類
目前來說，第一種方法依賴于密集的點云才能達到穩定可靠的效果，為了實現密集點云，通常使用高線激光雷達（如Velodyne的HDL-64），或者采用多雷達組合（單個32線雷達+多個16線雷達）來實現密集點云，這類方法要達到安全穩定的感知效果成本高昂。第二種方法依賴于圖像檢測的精度，由于深度神經網絡的發展，基于圖像的目標檢測已經非常穩定可靠了，但是，多路圖像的深度學習檢測依賴于強大的芯片，滿足車規級要求的深度學習芯片缺乏。
可見點云聚類是激光雷達環境感知中的重要步驟，實際上，在低速、簡單場景下，僅使用聚類已經能夠達到很好的障礙物感知效果了。
歐幾里德聚類
KD Tree
在介紹歐幾里德聚類之前我們首先理解歐幾里德聚類中使用的基本的數據結構——KD Tree（k-維樹）。k-維樹是在一個歐幾里德空間中組織點的基本數據結構，它本質上就是一個每個節點都未k維點的二叉樹。在PCL中，由于點云的三維屬性，所用到的K-維樹即為3維樹。在本文的代碼中，我們實際上僅使用了一個2維樹，我們將點云壓縮成了2維——即將所有點的z值（高度）設為0，這么做的原因在于一方面我們并不關心點云簇在z方向的搜索順序（兩個物體在z方向疊在一起我們可以將其視為一個障礙物），另一方面我們希望能夠加快我們的聚類速度以滿足無人車感知實時性的需求。此外，一個2維樹以更方便讀者理解KD Tree。使用二維樹對平面上的點進行劃分如下圖所示：
如上圖所示，我們使用一個二叉樹組織所有的點。點與點的距離表示其鄰近距離，二叉樹的所有非葉子節點可以視作用一個超平面把空間分割成兩個半空間。節點左邊的子樹代表在超平面左邊的點，節點右邊的子樹代表在超平面右邊的點。選擇超平面的方法如下：每個節點都與k維中垂直于超平面的那一維有關。因此，如果選擇按照x軸劃分，所有x值小于指定值的節點都會出現在左子樹，所有x值大于指定值的節點都會出現在右子樹。這樣，超平面可以用該x值來確定，其法線為x軸的單位向量。
歐幾里德聚類
對于歐幾里德聚類的具體算法流程，PCL官方文檔提供的如下偽代碼：
之所以使用KD Tree數據結構來組織點，實際上就是為了加速在聚類過程中的搜索速度。在該算法中，最重要的參數即為dth，它表示聚類的時候的半徑閾值。在這個半徑內整個球體內的點將被聚類成一個點云簇。此外，在PCL庫中，聚類方法還有兩個重要參數——最大和最小聚類點數閾值，當聚類的點云簇的點數在這個兩個閾值以內的情況下才會被返回。
基于歐幾里德聚類的障礙物檢測ROS實現
在上一篇博客中我們實現了一個簡單的地面-非地面分割ROS節點，這個節點訂閱來自 /velodyne_points 話題的點云數據，并且將分割完的點云分別發布到 /filtered_points_ground 和 /filtered_points_no_ground 兩個話題上，下面我們編寫一個歐幾里德聚類的節點，訂閱 /filtered_points_no_ground 話題，對路面以上的障礙物進行檢測。
使用Voxel Grid對點云降采樣
由于點云聚類的實時性要求，我們需要通過減少點云的密度來加速聚類。一種簡單的方法就是使用我們前文提到的Voxel Grid Filter對點云進行降采樣，代碼如下：
void EuClusterCore::voxel_grid_filer(pcl::PointCloud<pcl::PointXYZ>::Ptr in, pcl::PointCloud<pcl::PointXYZ>::Ptr out, double leaf_size)
{
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setInputCloud(in);
filter.setLeafSize(leaf_size, leaf_size, leaf_size);
filter.filter(*out);
}
需要注意的是，這里的Voxel Grid Filter的Leaf Size應該盡可能小，在實例中我們使用的Leaf Size為0.1m，過大的Leaf Size雖然會使得速度變快，但是聚類的結果相對會變得更差，尤其是對于反射較為微弱的物體（如遠處的行人）。
按距離分割點云
如上文所提，歐幾里德聚類最重要的參數是聚類半徑閾值，為了達到更好的聚類效果，我們在不同距離的區域使用不同的聚類半徑閾值，如下圖所示：
所以，我們首先將掃描的點云按照其到雷達的聚類切分成五個點云：
void EuClusterCore::cluster_by_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc, std::vector<Detected_Obj> &obj_list)
{
//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-15m d=0.5
//1 => 15-30 d=1
//2 => 30-45 d=1.6
//3 => 45-60 d=2.1
//4 => >60 d=2.6
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segment_pc_array(5);
for (size_t i = 0; i < segment_pc_array.size(); i++)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
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));
// 如果點的距離大于120m, 忽略該點
if (origin_distance >= 120)
{
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<pcl::PointIndices> final_indices;
std::vector<pcl::PointIndices> tmp_indices;
for (size_t i = 0; i < segment_pc_array.size(); i++)
{
cluster_segment(segment_pc_array[i], cluster_distance_[i], obj_list);
}
}
這里我們忽略了距離大于120m的點，原因在于一方面我們近期主要做的低速場景，對于遠距離的環境感知并無要求，此外我們采用的Velodyne VLP-32C雷達線束仍不密集，在遠處實際上反射已經非常微弱了，聚類效果不佳。
聚類并計算障礙物中心和Bounding Box
接下來我們正對這五個點云分別使用不同的半徑閾值進行歐幾里德聚類，對聚類完以后的一個個點云簇，我們計算其形心作為該障礙物的中心，同時計算點云簇的長寬高，從而確定一個能夠將點云簇包裹的三維Bounding Box，代碼如下：
void EuClusterCore::cluster_segment(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc,
double in_max_cluster_distance, std::vector<Detected_Obj> &obj_list)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
//create 2d pc
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
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<pcl::PointIndices> local_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> euclid;
euclid.setInputCloud(cloud_2d);
euclid.setClusterTolerance(in_max_cluster_distance);
euclid.setMinClusterSize(MIN_CLUSTER_SIZE);
euclid.setMaxClusterSize(MAX_CLUSTER_SIZE);
euclid.setSearchMethod(tree);
euclid.extract(local_indices);
for (size_t i = 0; i < local_indices.size(); i++)
{
// the structure to save one detected object
Detected_Obj obj_info;
float min_x = std::numeric_limits<float>::max();
float max_x = -std::numeric_limits<float>::max();
float min_y = std::numeric_limits<float>::max();
float max_y = -std::numeric_limits<float>::max();
float min_z = std::numeric_limits<float>::max();
float max_z = -std::numeric_limits<float>::max();
for (auto pit = local_indices[i].indices.begin(); pit != local_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 (local_indices[i].indices.size() > 0)
{
obj_info.centroid_.x /= local_indices[i].indices.size();
obj_info.centroid_.y /= local_indices[i].indices.size();
obj_info.centroid_.z /= local_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);
}
}
需要注意的是，我們放到 pcl::EuclideanClusterExtraction 是一個已經平面化的二維點云，這種做法能夠帶來速度的提升。這里我們定義了一個結構體 Detected_Obj ，用于存儲檢測到的障礙物的信息，內容如下：
struct Detected_Obj
{
jsk_recognition_msgs::BoundingBox bounding_box_;
pcl::PointXYZ min_point_;
pcl::PointXYZ max_point_;
pcl::PointXYZ centroid_;
};
最后，我們將檢測的障礙物的Bounding Box發布到 /detected_bounding_boxs 話題上：
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);
完整代碼見文末鏈接。
檢測結果
我們仍然使用上一篇博客中的rosbag來完成實踐，首先運行rosbag并且按空格暫停：
rosbag play test.bag
使用catkin_make編譯我們這個ROS節點，使用roslaunch運行我們上一篇文章中寫的節點和這個節點：
roslaunch pcl_test pcl_test.launch roslaunch euclidean_cluster euclidean_cluster.launch
啟動Rviz，繼續play rosbag，在Rviz中添加如下display：
其中，第三個為 jsk_rvize_plugins 中的BoundingBoxArray，添加方式如下：
得到的Detect效果：
我們放大看聚類的效果：
筆記
如果編譯過程中出現下面報錯，是ROS沒有安裝全，缺少 jsk_recognition_msgs包
Could not find a package configuration file provided by
"jsk_recognition_msgs" with any of the following names:
jsk_recognition_msgsConfig.cmake
jsk_recognition_msgs-config.cmake
Add the installation prefix of "jsk_recognition_msgs" to CMAKE_PREFIX_PATH
or set "jsk_recognition_msgs_DIR" to a directory containing one of the
above files. If "jsk_recognition_msgs" provides a separate development
package or SDK, be sure it has been installed.
解決方法：
sudo apt-get install ros-kinetic-jsk-recognition-msgs sudo apt-get install ros-kinetic-jsk-rviz-plugins
后記
本文的方法雖然能夠實現點云的聚類，但是受非道路元素的影響頗大，一種方法是采用高精度地圖徹底剔除不在可行駛區域上的點，這樣不僅聚類的計算量更小，同時能夠排除很多道旁障礙物（道旁的大樹，電線桿）的干擾。
完整代碼：https://gitee.com/itachi1121/lidar_euclidean_cluster.git
Talk is cheap, show me the code
總結
以上是生活随笔為你收集整理的<基于欧几里德聚类的激光雷达点云分割及ROS实现> 笔记的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： redis cluster集群
- 下一篇： 阿里矢量图简单css 用法