两种点云地面去除方法
目錄
1、基于角度分割的地面、非地面分割
1.1 PCL基本入門
1.1.1 在ROS項(xiàng)目中引入PCL庫
1.2 編寫節(jié)點(diǎn)進(jìn)行Voxel Grid Filter
1.2.1 驗(yàn)證效果
1.3 點(diǎn)云地面過濾
1.3.1 點(diǎn)云剪裁和過濾——去除過高和過近的點(diǎn)
1.3.2 角度微分和地面/非地面判斷——Ray Ground Filter
1.3.3 分割?
2、基于地面平面擬合的激光雷達(dá)地面分割方法和ROS實(shí)現(xiàn)
2.1 地面平面擬合（Ground Plane Fitting）
2.2 種子點(diǎn)集選取
2.3?平面模型
2.4 優(yōu)化平面主循環(huán)?
2.5 點(diǎn)云過濾
2.6 整體流程
? ? ? ? 本文參考自大佬博客：
1.?https://adamshan.blog.csdn.net/article/details/82901295?《無人駕駛汽車系統(tǒng)入門（二十四）——激光雷達(dá)的地面-非地面分割和pcl_ros實(shí)踐》
2.?https://adamshan.blog.csdn.net/article/details/84569000?《無人駕駛汽車系統(tǒng)入門（二十七）——基于地面平面擬合的激光雷達(dá)地面分割方法和ROS實(shí)現(xiàn)》
1、基于角度分割的地面、非地面分割
????????在無人駕駛的雷達(dá)感知中，將雷達(dá)點(diǎn)云地面分割出來是一步基本的操作，這一步操作主要能夠改善地面點(diǎn)對(duì)于地面以上的目標(biāo)的點(diǎn)云聚類的影響。
????????本文首先帶大家入門pcl_ros，首先我們使用pcl_ros編寫一個(gè)簡單的ros節(jié)點(diǎn)，對(duì)輸入點(diǎn)云進(jìn)行Voxel Grid Filter（體素濾波）。
1.1 PCL基本入門
? ? ? ? PCL是一個(gè)開源的點(diǎn)云處理庫，是在吸收了前人點(diǎn)云相關(guān)研究基礎(chǔ)上建立起來的大型跨平臺(tái)開源C++編程庫，它實(shí)現(xiàn)了大量點(diǎn)云相關(guān)的通用算法和高效數(shù)據(jù)結(jié)構(gòu)，包含點(diǎn)云獲取、濾波、分割、配準(zhǔn)、檢索、特征提取、識(shí)別、追蹤、曲面重建、可視化等大量開源代碼。支持多種操作系統(tǒng)平臺(tái)，可在Windows、Linux、Android、Mac OS X、部分嵌入式實(shí)時(shí)系統(tǒng)上運(yùn)行。如果說OpenCV是2D信息獲取與處理的結(jié)晶，那么PCL就在3D信息獲取與處理上具有同等地位。ROS kinetic完整版中本身已經(jīng)包含了pcl庫，同時(shí)ROS自帶的pcl_ros 包可以連接ROS和PCL庫。我們從一個(gè)簡單的Voxel Grid Filter的ROS節(jié)點(diǎn)實(shí)現(xiàn)來了解一下PCL在ROS中的基本用法，同時(shí)了解PCL中的一些基本數(shù)據(jù)結(jié)構(gòu)：
1.1.1 在ROS項(xiàng)目中引入PCL庫
????????在此我們假定讀者已經(jīng)自行安裝好ROS kinetic 的完整版，首先在我們的catkin workspace中新建一個(gè)package，我們將它命名為pcl_test,可以通過如下指令生成workspace和package：
cd ~ mkdir -p pcl_ws/src cd pcl_ws catkin_make source devel/setup.bash cd src catkin_create_pkg pcl_test roscpp sensor_msgs pcl_ros????????這樣，我們就新建了一個(gè)workspace，用于學(xué)習(xí)PCL，同時(shí)新建了一個(gè)名為pcl_test的package，這個(gè)ROS包依賴于roscpp，sensor_msgs, pcl_ros這幾個(gè)包，我們修改pcl_test包下的CMakeList文件以及package.xml配置文件，如下：
????????package.xml?文件：
CMakeList.txt?文件：
cmake_minimum_required(VERSION 2.8.3) project(pcl_test)add_compile_options(-std=c++11)find_package(catkin REQUIRED COMPONENTS pcl_ros roscpp sensor_msgs )catkin_package(INCLUDE_DIRS includeCATKIN_DEPENDS roscpp sensor_msgs pcl_ros )include_directories(include${catkin_INCLUDE_DIRS} ) link_directories(${PCL_LIBRARY_DIRS})add_executable(${PROJECT_NAME}_node src/pcl_test_node.cpp src/pcl_test_core.cpp)target_link_libraries(${PROJECT_NAME}_node${catkin_LIBRARIES}${PCL_LIBRARIES} )????????package.xml的內(nèi)容很簡單，實(shí)際上就是這個(gè)包的描述文件，build_depend?和?run_depend?兩個(gè)描述符分別指定了程序包編譯和運(yùn)行的依賴項(xiàng)，通常是所用到的庫文件的名稱。在這里我們指定了三個(gè)編譯和運(yùn)行時(shí)依賴項(xiàng)，分別是roscpp（編寫C++ ROS節(jié)點(diǎn)），sensor_msgs（定義了激光雷達(dá)的msg），pcl_ros（連接ROS和pcl庫）。
????????同樣的，在CMakeList中，我們通過find_package查找這三個(gè)包的路徑，然后將三個(gè)包添加到?CATKIN_DEPENDS, 在使用pcl庫前，需要將PCL庫的路徑鏈接，通過link_directories(${PCL_LIBRARY_DIRS})來完成，并在最后的target_link_libraries中添加${PCL_LIBRARIES}。
1.2 編寫節(jié)點(diǎn)進(jìn)行Voxel Grid Filter
????????接著我們?cè)趐cl_test/src目錄下新建pcl_test_node.cpp文件：
#include "pcl_test_core.h" // 自己寫的頭文件int main(int argc, char **argv) {ros::init(argc, argv, "pcl_test");ros::NodeHandle nh;PclTestCore core(nh);return 0; }????????此文件僅包含main函數(shù)，是節(jié)點(diǎn)的入口，編寫頭文件include/pcl_test_core.h:
#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/voxel_grid.h>#include <sensor_msgs/PointCloud2.h>class PclTestCore {private:ros::Subscriber sub_point_cloud_;ros::Publisher pub_filtered_points_;void point_cb(const sensor_msgs::PointCloud2ConstPtr& in_cloud);public:PclTestCore(ros::NodeHandle &nh);~PclTestCore();void Spin(); };以及pcl_test_core.cpp：
#include "pcl_test_core.h"PclTestCore::PclTestCore(ros::NodeHandle &nh) {sub_point_cloud_ = nh.subscribe("/velodyne_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); }? ? ? ? 這個(gè)節(jié)點(diǎn)的功能是訂閱來自/velodyne_points話題的點(diǎn)云數(shù)據(jù)，使用PCL內(nèi)置的Voxel Grid Filter對(duì)原始點(diǎn)云進(jìn)行降采樣，將降采樣的結(jié)構(gòu)發(fā)布到/filtered_points話題上。我們重點(diǎn)看回調(diào)函數(shù)PclTestCore::point_cb。在該回調(diào)函數(shù)中，我們首先定義了兩個(gè)點(diǎn)云指針，在PCL庫中，pcl::PointCloud<T>是最基本的一種數(shù)據(jù)結(jié)構(gòu)，它表示一塊點(diǎn)云數(shù)據(jù)（點(diǎn)的集合），我們可以指定點(diǎn)的數(shù)據(jù)結(jié)構(gòu)，在上述實(shí)例中，采用了pcl::PointXYZI這種類型的點(diǎn)。pcl::PointXYZI結(jié)構(gòu)體使用(x, y, z, intensity)這四個(gè)數(shù)值來描述一個(gè)三維度空間點(diǎn)。
????????intensity，即反射強(qiáng)度，是指激光雷達(dá)的激光發(fā)射器發(fā)射激光后收到的反射的強(qiáng)度，通常所說的16線，32線激光雷達(dá)，其內(nèi)部實(shí)際是并列縱排的多個(gè)激光發(fā)射器，通過電機(jī)自旋，產(chǎn)生360環(huán)視的點(diǎn)云數(shù)據(jù)，不同顏色的物體對(duì)激光的反射強(qiáng)度也是不同的，通常來說，白色物體的反射強(qiáng)度（intensity）最強(qiáng)，對(duì)應(yīng)的，黑色的反射強(qiáng)度最弱。
?
????????通常使用sensor_msgs/PointCloud2.h 做為點(diǎn)云數(shù)據(jù)的消息格式，可使用pcl::fromROSMsg和pcl::toROSMsg將sensor_msgs::PointCloud2與pcl::PointCloud<T>進(jìn)行轉(zhuǎn)換，這里的T就是pcl::PointXYZI。
????????為了使用Voxel Grid Filter對(duì)原始點(diǎn)云進(jìn)行降采樣，只需定義pcl::VoxelGrid并且指定輸入點(diǎn)云和leaf size，在本例中，我們使用leaf size為 0.2。Voxel Grid Filter將輸入點(diǎn)云使用0.2m*0.2m*0.2m的立方體進(jìn)行分割，使用小立方體的 形心（centroid） 來表示這個(gè)立方體的所有點(diǎn)，保留這些點(diǎn)作為降采樣的輸出。
?
1.2.1 驗(yàn)證效果
????????我們寫一個(gè)launch文件pcl_test.launch來啟動(dòng)這個(gè)節(jié)點(diǎn)：
<launch><node pkg="pcl_test" type="pcl_test_node" name="pcl_test_node" output="screen"/> </launch>????????回到workspace 目錄，使用catkin_make 編譯：
catkin_make????????啟動(dòng)這個(gè)節(jié)點(diǎn)：
roslaunch pcl_test pcl_test.launch????????新建終端，并運(yùn)行我們的測試bag（測試bag下載鏈接：https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w）
rosbag play --clock test.bag????????打開第三個(gè)終端，啟動(dòng)Rviz：
rosrun rviz rviz配置Rviz的Frame為velodyne，并且加載原始點(diǎn)云和過濾以后的點(diǎn)云的display：
?原始點(diǎn)云：
?降采樣之后的點(diǎn)云（即我們的節(jié)點(diǎn)的輸出）：
1.3 點(diǎn)云地面過濾
????????過濾地面是激光雷達(dá)感知中一步基礎(chǔ)的預(yù)處理操作，因?yàn)槲覀儹h(huán)境感知通常只對(duì)路面上的障礙物感興趣，且地面的點(diǎn)對(duì)于障礙物聚類容易產(chǎn)生影響，所以在做Lidar Obstacle Detection之前通常將地面點(diǎn)和非地面點(diǎn)進(jìn)行分離。在此文中我們介紹一種被稱為Ray Ground Filter的路面過濾方法，并且在ROS中實(shí)踐。
????????
1.3.1 點(diǎn)云剪裁和過濾——去除過高和過近的點(diǎn)
????????要分割地面和非地面，那么過高的區(qū)域首先就可以忽略不計(jì)，我們先對(duì)點(diǎn)云進(jìn)行高度的裁剪。我們實(shí)驗(yàn)用的bag在錄制的時(shí)候lidar的高度約為1.78米，我們剪裁掉1.28米以上的部分，代碼如下：
void PclTestCore::clip_above(double clip_height, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,const pcl::PointCloud<pcl::PointXYZI>::Ptr out) {pcl::ExtractIndices<pcl::PointXYZI> 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<pcl::PointIndices>(indices));cliper.setNegative(true); //ture to remove the indices 去除高于閾值的cliper.filter(*out); }????????為了消除車身自身的雷達(dá)反射的影響，我們對(duì)近距離的點(diǎn)云進(jìn)行過濾，仍然使用pcl::ExtractIndices進(jìn)行剪裁：
void PclTestCore::remove_close_pt(double min_distance, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,const pcl::PointCloud<pcl::PointXYZI>::Ptr out) {pcl::ExtractIndices<pcl::PointXYZI> cliper;cliper.setInputCloud(in);pcl::PointIndices indices; #pragma omp forfor (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){indices.indices.push_back(i);}}cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));cliper.setNegative(true); //ture to remove the indicescliper.filter(*out); }其中，#pragma omp for語法OpenMP的并行化語法，即希望通過OpenMP并行化執(zhí)行這條語句后的for循環(huán)，從而起到加速的效果。
1.3.2 角度微分和地面/非地面判斷——Ray Ground Filter
????????Ray Ground Filter算法的核心是以射線（Ray）的形式來組織點(diǎn)云。我們現(xiàn)在將點(diǎn)云的 (x, y, z)三維空間降到（x,y）平面來看，計(jì)算每一個(gè)點(diǎn)到車輛x正方向的平面夾角 θ, 我們對(duì)360度進(jìn)行微分，分成若干等份，每一份的角度為0.18度，這個(gè)微分的等份近似的可以看作一條射線，如下圖所示,圖中是一個(gè)激光雷達(dá)的縱截面的示意圖，雷達(dá)由下至上分布多個(gè)激光器，發(fā)出如圖所示的放射狀激光束，這些激光束在平地上即表現(xiàn)為，圖中的水平線即為一條射線：
?0.18度是VLP32C雷達(dá)的水平光束發(fā)散間隔。?
????????為了方便地對(duì)點(diǎn)進(jìn)行半徑和夾角的表示，我們使用如下數(shù)據(jù)結(jié)構(gòu)代替pcl::PointCloudXYZI：
struct PointXYZIRTColor {pcl::PointXYZI point;float radius; //cylindric coords on XY Planefloat theta; //angle deg on XY planesize_t radial_div; //index of the radial divsion to which this point belongs tosize_t concentric_div; //index of the concentric division to which this points belongs tosize_t original_index; //index of this point in the source pointcloud };typedef std::vector<PointXYZIRTColor> PointCloudXYZIRTColor;其中，radius表示點(diǎn)到lidar的水平距離(半徑)，即：
?theta是點(diǎn)相對(duì)于車頭正方向(即x方向)的夾角，計(jì)算公式為：
????????我們用radial_div和concentric_div分別描述角度微分和距離微分。對(duì)點(diǎn)云進(jìn)行水平角度微分之后，可得到：360/0.18 = 2000條射線，將這些射線中的點(diǎn)按照距離的遠(yuǎn)近進(jìn)行排序，如下所示：
//將同一根射線上的點(diǎn)按照半徑（距離）排序 #pragma omp forfor (size_t i = 0; i < radial_dividers_num_; i++){std::sort(out_radial_ordered_clouds[i].begin(), out_radial_ordered_clouds[i].end(),[](const PointXYZIRTColor &a, const PointXYZIRTColor &b) { return a.radius < b.radius; });}????????通過判斷射線中前后兩點(diǎn)的坡度是否大于我們事先設(shè)定的坡度閾值，從而判斷點(diǎn)是否為地面點(diǎn)。代碼如下：?
void PclTestCore::classify_pc(std::vector<PointCloudXYZIRTColor> &in_radial_ordered_clouds,pcl::PointIndices &out_ground_indices,pcl::PointIndices &out_no_ground_indices) {out_ground_indices.indices.clear(); // 地面點(diǎn)out_no_ground_indices.indices.clear(); // 非地面點(diǎn)#pragma omp for// sweep through each radial division 遍歷每一根射線for( size_t i=0; i < in_radial_ordered_clouds.size(); i ++ ){float prev_radius = 0.f;float prev_height = -SENSOR_HEIGHT;bool prev_ground = false;bool current_ground = false;// 遍歷每一個(gè)射線上的所有點(diǎn)for( size_t j=0; j<in_radial_ordered_clouds[i].size(); j ++ ){float points_distance = in_radial_ordered_clouds[i][j].radius - prev_radius; // 與前一個(gè)點(diǎn)的距離float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;float current_height = in_radial_ordered_clouds[i][j].point.z; // 當(dāng)前點(diǎn)的實(shí)際z值float general_height_threshold = tan(DEG2RAD(general_max_slope_)) * in_radial_ordered_clouds[i][j].radius;// for points which are very close causing the height threshold to be tiny, set a minimum value// 對(duì)于非常接近導(dǎo)致高度閾值很小的點(diǎn)，請(qǐng)?jiān)O(shè)置一個(gè)最小值if ( points_distance > concentric_divider_distance_ && height_threshold < min_height_threshold_ ){height_threshold = min_height_threshold_;}// check current point height against the LOCAL threshold (previous point) 對(duì)照局部閾值檢查當(dāng)前點(diǎn)高度（上一點(diǎn)）if ( current_height <= (prev_height+height_threshold) && current_height >= (prev_height-height_threshold) ){// check again using general geometry (radius from origin) if previous points wasn't groundif ( !prev_ground ) // 前一點(diǎn)不是地面點(diǎn)，需要進(jìn)一步判斷當(dāng)前點(diǎn){if (current_height <= (-SENSOR_HEIGHT+general_height_threshold) && current_height >= (-SENSOR_HEIGHT-general_height_threshold)){current_ground = true;}else{current_ground = false;}}else // 上一點(diǎn)是地面點(diǎn)，當(dāng)前點(diǎn)在上一點(diǎn)閾值高度范圍內(nèi)，所以這一點(diǎn)一定是地面點(diǎn){current_ground = true;}}else{// check if previous point is too far from previous one, if so classify again// 檢查當(dāng)前點(diǎn)是否離上一個(gè)點(diǎn)太遠(yuǎn)，如果是，重新分類if ( points_distance > reclass_distance_threshold_ &&(current_height <= (-SENSOR_HEIGHT+height_threshold) && current_height >= (-SENSOR_HEIGHT-height_threshold))){current_ground = true;}else{current_ground = false;}}// 最終判斷當(dāng)前點(diǎn)是否為地面點(diǎn)if ( current_ground ){out_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index);prev_ground = true;}else{out_no_ground_indices.indices.push_back(in_radial_ordered_clouds[i][j].original_index);prev_ground = false;}prev_radius = in_radial_ordered_clouds[i][j].radius;prev_height = in_radial_ordered_clouds[i][j].point.z;}}}????????這里有兩個(gè)重要參數(shù)，一個(gè)是local_max_slope_，是我們?cè)O(shè)定的同條射線上鄰近兩點(diǎn)的坡度閾值，一個(gè)是general_max_slope_，表示整個(gè)地面的坡度閾值，這兩個(gè)坡度閾值的單位為度（degree），我們通過這兩個(gè)坡度閾值以及當(dāng)前點(diǎn)的半徑（到lidar的水平距離）求得高度閾值，通過判斷當(dāng)前點(diǎn)的高度（即點(diǎn)的z值）是否在地面加減高度閾值范圍內(nèi)來判斷當(dāng)前點(diǎn)是為地面。
????????在地面判斷條件中，current_height <= (-SENSOR_HEIGHT + general_height_threshold) && current_height >= (-SENSOR_HEIGHT - general_height_threshold) 中SENSOR_HEIGHT表示lidar掛載的高度，-SNESOR_HEIGHT即表示水平地面。
1.3.3 分割?
????????我們使用上文中的bag來驗(yàn)證地面分割節(jié)點(diǎn)的工作效果。運(yùn)行bag并且運(yùn)行我們的節(jié)點(diǎn)，打開Rviz，加載兩個(gè)點(diǎn)云display，效果如下所示：
其中，紅色的點(diǎn)為我們分割出來的地面，來自于 /filtered_points_ground 話題，白色的點(diǎn)為非地面，來自于 /filtered_points_no_ground 話題。分割出非地面點(diǎn)云之后，我們就可以讓Lidar Detection的代碼工作在這個(gè)點(diǎn)云上了，從而排除了地面對(duì)于Lidar聚類以及Detection的影響。
?
2、基于地面平面擬合的激光雷達(dá)地面分割方法和ROS實(shí)現(xiàn)
? ? ? ? 上面是一種基于射線的地面過濾方法，此方法能夠很好的完成地面分割，但是存在幾點(diǎn)不足：
- 第一，存在少量噪點(diǎn)，不能徹底過濾出地面；
- 第二，非地面的點(diǎn)容易被錯(cuò)誤分類，造成非地面點(diǎn)缺失；
- 第三，對(duì)于目標(biāo)接近激光雷達(dá)盲區(qū)的情況，會(huì)出現(xiàn)誤分割，即將非地面點(diǎn)云分割為地面。
????????通過本文我們一起學(xué)習(xí)一種新的地面分割方法，出自論文：Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications，在可靠性、分割精度方面均優(yōu)于基于射線的地面分割方法。我們將其稱之為地面平面擬合方法。
2.1 地面平面擬合（Ground Plane Fitting）
????????我們采用平面模型（Plane Model）來擬合當(dāng)前的地面，通常來說，由于現(xiàn)實(shí)的地面并不是一個(gè)“完美的”平面，而且當(dāng)距離較大時(shí)激光雷達(dá)會(huì)存在一定的測量噪聲，單一的平面模型并不足以描述我們現(xiàn)實(shí)的地面。
????????要很好的完成地面分割，就必須要處理存在一定坡度變化的地面的情況（不能將這種坡度的變化視為非地面，不能因?yàn)槠露鹊拇嬖诙朐肼?#xff09;，一種簡單的處理方法就是沿著x方向（車頭的方向）將空間分割成若干個(gè)子平面，然后對(duì)每個(gè)子平面使用地面平面擬合算法（GPF）從而得到能夠處理陡坡的地面分割方法。
?
????????那么如何進(jìn)行平面擬合呢？其偽代碼如下：
?????????我們來詳細(xì)的了解這一流程：對(duì)于給定的點(diǎn)云 P ,分割的最終結(jié)果為兩個(gè)點(diǎn)云集合?：地面點(diǎn)云 和 ：非地面點(diǎn)云。該算法有四個(gè)重要的參數(shù)，分別是：
- ：進(jìn)行奇異值分解（singular value decomposition，SVD）的次數(shù)，也即進(jìn)行優(yōu)化擬合的次數(shù)；
- ：用于選取LPR的最低高度點(diǎn)的數(shù)量；
- ：?用于選取種子點(diǎn)的閾值，當(dāng)點(diǎn)云內(nèi)的點(diǎn)的高度小于LPR的高度加上此閾值時(shí)，我們將該點(diǎn)加入種子點(diǎn)集；
- ：平面距離閾值，我們會(huì)計(jì)算點(diǎn)云中每一個(gè)點(diǎn)到我們擬合的平面的正交投影的距離，而這個(gè)平面距離閾值，就是用來判定點(diǎn)是否屬于地面。
2.2 種子點(diǎn)集選取
????????我們首先選取一個(gè)種子點(diǎn)集(seed point set)，這些種子點(diǎn)來源于點(diǎn)云中高度（即z值）較小的點(diǎn)，種子點(diǎn)集被用于建立描述地面的初始平面模型，那么如何選取這個(gè)種子點(diǎn)集呢？
????????我們引入最低點(diǎn)代表（Lowest Point Representative, LPR）的概念。LPR就是? 個(gè)最低高度點(diǎn)的平均值，LPR 保證了平面擬合階段不受測量噪聲的影響。這個(gè)LPR被當(dāng)作是整個(gè)點(diǎn)云 P 的最低點(diǎn)，點(diǎn)云P中高度在閾值? 內(nèi)的點(diǎn)被當(dāng)作是種子點(diǎn)，由這些種子點(diǎn)構(gòu)成一個(gè)種子點(diǎn)集合。
????????種子點(diǎn)集的選取代碼如下：
void PlaneGroundFilter::extract_initial_seeds_(const pcl::PointCloud<VPoint> &p_sorted) {// LPR is the mean of low point representative.double sum = 0; //int cnt = 0; // 點(diǎn)云計(jì)數(shù)// Calculate the mean height value. 因?yàn)橐呀?jīng)排過序了，所以直接選取前最小的num_lpr個(gè)點(diǎn)即可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 0g_seeds_pc->clear();// iterate pointcloud, filter those height is less than lpr.height + th_seeds_. 得到初始地面種子點(diǎn)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]);}}// return seeds points }????????輸入是一個(gè)點(diǎn)云，這個(gè)點(diǎn)云內(nèi)的點(diǎn)已經(jīng)沿著z方向（即高度）做了排序，取?num_lpr_?個(gè)最小點(diǎn)，求得高度平均值?lpr_height（即LPR），選取高度小于?lpr_height + th_seeds_?的點(diǎn)作為種子點(diǎn)。
2.3?平面模型
????????接下來我們需要確定一個(gè)平面，點(diǎn)云中的點(diǎn)到這個(gè)平面的正交投影距離小于閾值? 則認(rèn)為該點(diǎn)屬于地面，否則屬于非地面，我們采用一個(gè)簡單的線性模型用于平面模型估計(jì)，如下：
ax+by+cz+d = 0?
也即：
其中，是所有點(diǎn)的均值。這個(gè)協(xié)方差矩陣 C? 描述了種子點(diǎn)集的散布情況，其三個(gè)奇異向量可以通過奇異值分解（Singular Value Decomposition,SVD）求得，這三個(gè)奇異向量描述了點(diǎn)集在三個(gè)主要方向的散布情況。由于是平面模型，垂直于平面的法向量n表示具有最小方差的方向，可以通過計(jì)算具有最小奇異值的奇異向量來求得。
????????那么在求得了 n 以后， d 可以通過代入種子點(diǎn)集的平均值?? (它代表屬于地面的點(diǎn)) 直接求得。整個(gè)平面模型計(jì)算代碼如下：
// 根據(jù)ax+by+cz+d = 0，擬合平面，最重要的是確定點(diǎn)與平面的投影距離閾值Th_dist，并以此判斷點(diǎn)是否在平面上 void PlaneGroundFilter::estimate_plane_(void) {// Create covariance matrix in single pass.// TODO: compare the efficiency.Eigen::Matrix3f cov;Eigen::Vector4f pc_mean;// 計(jì)算均值和協(xié)方差矩陣pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);// Singular Value Decomposition: SVD 奇異值分解JacobiSVD<MatrixXf> svd(cov, Eigen::DecompositionOptions::ComputeFullU);// use the least singular vector as normal. 取前三維作為主要方向：U矩陣m*m => m*r r=3 m是g_ground_pc點(diǎn)云的點(diǎn)數(shù)normal_ = (svd.matrixU().col(2));// mean ground seeds value.Eigen::Vector3f seeds_mean = pc_mean.head<3>(); // 要XYZIR前三維，XYZ// according to normal.T * [x, y, z] = -dd_ = -(normal_.transpose() * seeds_mean)(0, 0);// set distance threshold to 'th_dist - d'// 點(diǎn)云中的點(diǎn)到這個(gè)平面的正交投影距離小于閾值 Th_dist, 則認(rèn)為該點(diǎn)屬于地面，否則屬于非地面th_dist_d_ = th_dist_ - d_;// return the equation parameters. }2.4 優(yōu)化平面主循環(huán)?
????????在確定如何選取種子點(diǎn)集以及如何估計(jì)平面模型以后，我們來看一下整個(gè)算法的主循環(huán)，以下是主循環(huán)的代碼：
// 4. Extract init ground seeds. 提取初始地面點(diǎn)extract_initial_seeds_(laserCloudIn);g_ground_pc = g_seeds_pc;// 5. Ground plane fitter mainloopfor(int i=0; i<num_iter_; i ++){estimate_plane_(); // 用上面 估計(jì)的g_ground_pc點(diǎn)云 來擬合地平面g_ground_pc->clear(); // 準(zhǔn)備重新來得到地面點(diǎn)云，用以進(jìn)行下一次平面擬合//g_not_ground_pc->clear();// pointcloud to matrix 點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為矩陣存儲(chǔ) n*3維度表示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;}// ground plane model 所有點(diǎn)與地平面法線的點(diǎn)乘，得到與地平面的距離VectorXf result = points * normal_;// threshold filterfor (int r = 0; r < result.rows(); ++r){if( result[r] < th_dist_d_ ) // 距離小于閾值的，就劃分為地平面{g_all_pc->points[r].label = 1u; // mean ground 表示地面，在所有點(diǎn)云中進(jìn)行標(biāo)簽標(biāo)識(shí)，加以區(qū)分g_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨(dú)存放地面點(diǎn)云}else{g_all_pc->points[r].label = 0u; // mean not ground and non clusteredg_not_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨(dú)存放非地面點(diǎn)云}}}????????得到這個(gè)初始的平面模型以后，我們會(huì)計(jì)算點(diǎn)云中每一個(gè)點(diǎn)到該平面的正交投影的距離，即?points * normal_，并且將這個(gè)距離與設(shè)定的閾值（即th_dist_d_）比較，當(dāng)高度差小于此閾值，我們認(rèn)為該點(diǎn)屬于地面，當(dāng)高度差大于此閾值，則為非地面點(diǎn)。經(jīng)過分類以后的所有地面點(diǎn)被當(dāng)作下一次迭代的種子點(diǎn)集，迭代優(yōu)化。
2.5 點(diǎn)云過濾
????????下面我們編寫一個(gè)簡單的ROS節(jié)點(diǎn)實(shí)現(xiàn)這一地面分割算法。我們?nèi)匀徊捎玫?4篇博客（激光雷達(dá)的地面-非地面分割和pcl_ros實(shí)踐）中的bag進(jìn)行實(shí)驗(yàn)，這個(gè)bag來自Velodyne的VLP32C，在此算法的基礎(chǔ)上我們要進(jìn)一步濾除雷達(dá)過近處和過高處的點(diǎn)，因?yàn)槔走_(dá)安裝位置的原因，近處的車身反射會(huì)對(duì)Detection造成影響，需要濾除; 過高的目標(biāo)，如大樹、高樓，對(duì)于無人車的雷達(dá)感知意義也不大，我們對(duì)過近過高的點(diǎn)進(jìn)行切除，代碼如下：
// 去除過近或者過遠(yuǎn)的點(diǎn) void PlaneGroundFilter::remove_close_far_pt(const pcl::PointCloud<VPoint>::Ptr in,const pcl::PointCloud<VPoint>::Ptr out) {pcl::ExtractIndices<VPoint> cliper;pcl::PointIndices indices;#pragma omp forfor( 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<pcl::PointIndices>(indices));cliper.setNegative(true); // true to remove the indicescliper.filter(*out); }2.6 整體流程
? ? ? ? 主要流程都在PlaneGroundFilter::point_cb()函數(shù)中：
? ? ? ? 1. 數(shù)據(jù)預(yù)處理，將輸入的Msg消息轉(zhuǎn)換成pcl::pointcloud，并將點(diǎn)云全部存入g_all_pc中：
// 主要處理函數(shù) void PlaneGroundFilter::point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud_ptr) {// 1. Msg to pointcloud 數(shù)據(jù)類型轉(zhuǎn)換pcl::PointCloud<VPoint> laserCloudIn;pcl::fromROSMsg(*in_cloud_ptr, laserCloudIn);pcl::PointCloud<VPoint> laserCloudIn_org;pcl::fromROSMsg(*in_cloud_ptr, laserCloudIn_org);// For mark ground points and hold all points.SLRPointXYZIRL point;// 將輸入的所有點(diǎn)存放到g_all_pc中for(size_t i=0; i < laserCloudIn.points.size(); i ++){point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;point.intensity = laserCloudIn.points[i].intensity;point.ring = laserCloudIn.points[i].ring;point.label = 0u; // 0 means uncluster.g_all_pc->points.push_back(point);}? ? ? ? 2. 對(duì)點(diǎn)云按照z軸數(shù)值大小對(duì)輸入點(diǎn)云進(jìn)行排序：
// 2. Sort on Z-axis value. 按照z軸數(shù)值大小對(duì)輸入點(diǎn)云進(jìn)行排序sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_cmp);? ? ? ? 3. 對(duì)Z軸數(shù)據(jù)過小的點(diǎn)直接去除：
// 3. Error point removal 清除一部分錯(cuò)誤點(diǎn)// As there are some error mirror reflection under the ground,// here regardless point under 2* sensor_height// Sort point according to height, here uses z-axis in defaultpcl::PointCloud<VPoint>::iterator it = laserCloudIn.points.begin();for( int i=0; i < laserCloudIn.points.size(); i ++ ){if( laserCloudIn.points[i].z < -1.5*sensor_height_ ){it ++;}else{break;}}laserCloudIn.points.erase(laserCloudIn.points.begin(), it);? ? ? ? 4. 提取初始地面種子點(diǎn)
// 4. Extract init ground seeds. 提取初始地面點(diǎn)extract_initial_seeds_(laserCloudIn);g_ground_pc = g_seeds_pc;? ? ? ? 5. 地面去除主循環(huán)
// 5. Ground plane fitter mainloopfor(int i=0; i<num_iter_; i ++){estimate_plane_(); // 用上面 估計(jì)的g_ground_pc點(diǎn)云 來擬合地平面g_ground_pc->clear(); // 準(zhǔn)備重新來得到地面點(diǎn)云，用以進(jìn)行下一次平面擬合//g_not_ground_pc->clear();// pointcloud to matrix 點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為矩陣存儲(chǔ) n*3維度表示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;}// ground plane model 所有點(diǎn)與地平面法線的點(diǎn)乘，得到與地平面的距離VectorXf result = points * normal_;// threshold filterfor (int r = 0; r < result.rows(); ++r){if( result[r] < th_dist_d_ ) // 距離小于閾值的，就劃分為地平面{g_all_pc->points[r].label = 1u; // mean ground 表示地面，在所有點(diǎn)云中進(jìn)行標(biāo)簽標(biāo)識(shí)，加以區(qū)分g_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨(dú)存放地面點(diǎn)云}else{g_all_pc->points[r].label = 0u; // mean not ground and non clusteredg_not_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨(dú)存放非地面點(diǎn)云}}}? ? ? ? 6. 對(duì)去除地面點(diǎn)后的點(diǎn)云進(jìn)行后處理：
// 6. 對(duì)去除地面的點(diǎn)云進(jìn)行后處理pcl::PointCloud<VPoint>::Ptr final_no_ground(new pcl::PointCloud<VPoint>);post_process(g_not_ground_pc, final_no_ground);// ROS_INFO_STREAM("origin: "<<g_not_ground_pc->points.size()<<" post_process: "<<final_no_ground->points.size());// publish ground points 發(fā)布地面點(diǎn)云sensor_msgs::PointCloud2 ground_msg;pcl::toROSMsg(*g_ground_pc, ground_msg);ground_msg.header.stamp = in_cloud_ptr->header.stamp; // 時(shí)間戳ground_msg.header.frame_id = in_cloud_ptr->header.frame_id; // 幀Idpub_ground_.publish(ground_msg);// publish not ground points 發(fā)布非地面點(diǎn)云sensor_msgs::PointCloud2 groundless_msg;pcl::toROSMsg(*final_no_ground, groundless_msg);//pcl::toROSMsg(*g_not_ground_pc, groundless_msg);groundless_msg.header.stamp = in_cloud_ptr->header.stamp; // 時(shí)間戳groundless_msg.header.frame_id = in_cloud_ptr->header.frame_id; // 幀Idpub_no_ground_.publish(groundless_msg);// publish all points 發(fā)布所有點(diǎn)云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(); }總結(jié)
以上是生活随笔為你收集整理的两种点云地面去除方法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 2016 年 VR 行业回顾与展望
- 下一篇： POS机龙头联迪：一款产品的质量事故引发