两种点云地面去除方法
目錄
1、基于角度分割的地面、非地面分割
1.1 PCL基本入門
1.1.1 在ROS項目中引入PCL庫
1.2 編寫節點進行Voxel Grid Filter
1.2.1 驗證效果
1.3 點云地面過濾
1.3.1 點云剪裁和過濾——去除過高和過近的點
1.3.2 角度微分和地面/非地面判斷——Ray Ground Filter
1.3.3 分割?
2、基于地面平面擬合的激光雷達地面分割方法和ROS實現
2.1 地面平面擬合（Ground Plane Fitting）
2.2 種子點集選取
2.3?平面模型
2.4 優化平面主循環?
2.5 點云過濾
2.6 整體流程
? ? ? ? 本文參考自大佬博客：
1.?https://adamshan.blog.csdn.net/article/details/82901295?《無人駕駛汽車系統入門（二十四）——激光雷達的地面-非地面分割和pcl_ros實踐》
2.?https://adamshan.blog.csdn.net/article/details/84569000?《無人駕駛汽車系統入門（二十七）——基于地面平面擬合的激光雷達地面分割方法和ROS實現》
1、基于角度分割的地面、非地面分割
????????在無人駕駛的雷達感知中，將雷達點云地面分割出來是一步基本的操作，這一步操作主要能夠改善地面點對于地面以上的目標的點云聚類的影響。
????????本文首先帶大家入門pcl_ros，首先我們使用pcl_ros編寫一個簡單的ros節點，對輸入點云進行Voxel Grid Filter（體素濾波）。
1.1 PCL基本入門
? ? ? ? PCL是一個開源的點云處理庫，是在吸收了前人點云相關研究基礎上建立起來的大型跨平臺開源C++編程庫，它實現了大量點云相關的通用算法和高效數據結構，包含點云獲取、濾波、分割、配準、檢索、特征提取、識別、追蹤、曲面重建、可視化等大量開源代碼。支持多種操作系統平臺，可在Windows、Linux、Android、Mac OS X、部分嵌入式實時系統上運行。如果說OpenCV是2D信息獲取與處理的結晶，那么PCL就在3D信息獲取與處理上具有同等地位。ROS kinetic完整版中本身已經包含了pcl庫，同時ROS自帶的pcl_ros 包可以連接ROS和PCL庫。我們從一個簡單的Voxel Grid Filter的ROS節點實現來了解一下PCL在ROS中的基本用法，同時了解PCL中的一些基本數據結構：
1.1.1 在ROS項目中引入PCL庫
????????在此我們假定讀者已經自行安裝好ROS kinetic 的完整版，首先在我們的catkin workspace中新建一個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????????這樣，我們就新建了一個workspace，用于學習PCL，同時新建了一個名為pcl_test的package，這個ROS包依賴于roscpp，sensor_msgs, pcl_ros這幾個包，我們修改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的內容很簡單，實際上就是這個包的描述文件，build_depend?和?run_depend?兩個描述符分別指定了程序包編譯和運行的依賴項，通常是所用到的庫文件的名稱。在這里我們指定了三個編譯和運行時依賴項，分別是roscpp（編寫C++ ROS節點），sensor_msgs（定義了激光雷達的msg），pcl_ros（連接ROS和pcl庫）。
????????同樣的，在CMakeList中，我們通過find_package查找這三個包的路徑，然后將三個包添加到?CATKIN_DEPENDS, 在使用pcl庫前，需要將PCL庫的路徑鏈接，通過link_directories(${PCL_LIBRARY_DIRS})來完成，并在最后的target_link_libraries中添加${PCL_LIBRARIES}。
1.2 編寫節點進行Voxel Grid Filter
????????接著我們在pcl_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函數，是節點的入口，編寫頭文件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); }? ? ? ? 這個節點的功能是訂閱來自/velodyne_points話題的點云數據，使用PCL內置的Voxel Grid Filter對原始點云進行降采樣，將降采樣的結構發布到/filtered_points話題上。我們重點看回調函數PclTestCore::point_cb。在該回調函數中，我們首先定義了兩個點云指針，在PCL庫中，pcl::PointCloud<T>是最基本的一種數據結構，它表示一塊點云數據（點的集合），我們可以指定點的數據結構，在上述實例中，采用了pcl::PointXYZI這種類型的點。pcl::PointXYZI結構體使用(x, y, z, intensity)這四個數值來描述一個三維度空間點。
????????intensity，即反射強度，是指激光雷達的激光發射器發射激光后收到的反射的強度，通常所說的16線，32線激光雷達，其內部實際是并列縱排的多個激光發射器，通過電機自旋，產生360環視的點云數據，不同顏色的物體對激光的反射強度也是不同的，通常來說，白色物體的反射強度（intensity）最強，對應的，黑色的反射強度最弱。
?
????????通常使用sensor_msgs/PointCloud2.h 做為點云數據的消息格式，可使用pcl::fromROSMsg和pcl::toROSMsg將sensor_msgs::PointCloud2與pcl::PointCloud<T>進行轉換，這里的T就是pcl::PointXYZI。
????????為了使用Voxel Grid Filter對原始點云進行降采樣，只需定義pcl::VoxelGrid并且指定輸入點云和leaf size，在本例中，我們使用leaf size為 0.2。Voxel Grid Filter將輸入點云使用0.2m*0.2m*0.2m的立方體進行分割，使用小立方體的 形心（centroid） 來表示這個立方體的所有點，保留這些點作為降采樣的輸出。
?
1.2.1 驗證效果
????????我們寫一個launch文件pcl_test.launch來啟動這個節點：
<launch><node pkg="pcl_test" type="pcl_test_node" name="pcl_test_node" output="screen"/> </launch>????????回到workspace 目錄，使用catkin_make 編譯：
catkin_make????????啟動這個節點：
roslaunch pcl_test pcl_test.launch????????新建終端，并運行我們的測試bag（測試bag下載鏈接：https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w）
rosbag play --clock test.bag????????打開第三個終端，啟動Rviz：
rosrun rviz rviz配置Rviz的Frame為velodyne，并且加載原始點云和過濾以后的點云的display：
?原始點云：
?降采樣之后的點云（即我們的節點的輸出）：
1.3 點云地面過濾
????????過濾地面是激光雷達感知中一步基礎的預處理操作，因為我們環境感知通常只對路面上的障礙物感興趣，且地面的點對于障礙物聚類容易產生影響，所以在做Lidar Obstacle Detection之前通常將地面點和非地面點進行分離。在此文中我們介紹一種被稱為Ray Ground Filter的路面過濾方法，并且在ROS中實踐。
????????
1.3.1 點云剪裁和過濾——去除過高和過近的點
????????要分割地面和非地面，那么過高的區域首先就可以忽略不計，我們先對點云進行高度的裁剪。我們實驗用的bag在錄制的時候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); }????????為了消除車身自身的雷達反射的影響，我們對近距離的點云進行過濾，仍然使用pcl::ExtractIndices進行剪裁：
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并行化執行這條語句后的for循環，從而起到加速的效果。
1.3.2 角度微分和地面/非地面判斷——Ray Ground Filter
????????Ray Ground Filter算法的核心是以射線（Ray）的形式來組織點云。我們現在將點云的 (x, y, z)三維空間降到（x,y）平面來看，計算每一個點到車輛x正方向的平面夾角 θ, 我們對360度進行微分，分成若干等份，每一份的角度為0.18度，這個微分的等份近似的可以看作一條射線，如下圖所示,圖中是一個激光雷達的縱截面的示意圖，雷達由下至上分布多個激光器，發出如圖所示的放射狀激光束，這些激光束在平地上即表現為，圖中的水平線即為一條射線：
?0.18度是VLP32C雷達的水平光束發散間隔。?
????????為了方便地對點進行半徑和夾角的表示，我們使用如下數據結構代替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表示點到lidar的水平距離(半徑)，即：
?theta是點相對于車頭正方向(即x方向)的夾角，計算公式為：
????????我們用radial_div和concentric_div分別描述角度微分和距離微分。對點云進行水平角度微分之后，可得到：360/0.18 = 2000條射線，將這些射線中的點按照距離的遠近進行排序，如下所示：
//將同一根射線上的點按照半徑（距離）排序 #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; });}????????通過判斷射線中前后兩點的坡度是否大于我們事先設定的坡度閾值，從而判斷點是否為地面點。代碼如下：?
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(); // 地面點out_no_ground_indices.indices.clear(); // 非地面點#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;// 遍歷每一個射線上的所有點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; // 與前一個點的距離float height_threshold = tan(DEG2RAD(local_max_slope_)) * points_distance;float current_height = in_radial_ordered_clouds[i][j].point.z; // 當前點的實際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// 對于非常接近導致高度閾值很小的點，請設置一個最小值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) 對照局部閾值檢查當前點高度（上一點）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 ) // 前一點不是地面點，需要進一步判斷當前點{if (current_height <= (-SENSOR_HEIGHT+general_height_threshold) && current_height >= (-SENSOR_HEIGHT-general_height_threshold)){current_ground = true;}else{current_ground = false;}}else // 上一點是地面點，當前點在上一點閾值高度范圍內，所以這一點一定是地面點{current_ground = true;}}else{// check if previous point is too far from previous one, if so classify again// 檢查當前點是否離上一個點太遠，如果是，重新分類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;}}// 最終判斷當前點是否為地面點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;}}}????????這里有兩個重要參數，一個是local_max_slope_，是我們設定的同條射線上鄰近兩點的坡度閾值，一個是general_max_slope_，表示整個地面的坡度閾值，這兩個坡度閾值的單位為度（degree），我們通過這兩個坡度閾值以及當前點的半徑（到lidar的水平距離）求得高度閾值，通過判斷當前點的高度（即點的z值）是否在地面加減高度閾值范圍內來判斷當前點是為地面。
????????在地面判斷條件中，current_height <= (-SENSOR_HEIGHT + general_height_threshold) && current_height >= (-SENSOR_HEIGHT - general_height_threshold) 中SENSOR_HEIGHT表示lidar掛載的高度，-SNESOR_HEIGHT即表示水平地面。
1.3.3 分割?
????????我們使用上文中的bag來驗證地面分割節點的工作效果。運行bag并且運行我們的節點，打開Rviz，加載兩個點云display，效果如下所示：
其中，紅色的點為我們分割出來的地面，來自于 /filtered_points_ground 話題，白色的點為非地面，來自于 /filtered_points_no_ground 話題。分割出非地面點云之后，我們就可以讓Lidar Detection的代碼工作在這個點云上了，從而排除了地面對于Lidar聚類以及Detection的影響。
?
2、基于地面平面擬合的激光雷達地面分割方法和ROS實現
? ? ? ? 上面是一種基于射線的地面過濾方法，此方法能夠很好的完成地面分割，但是存在幾點不足：
- 第一，存在少量噪點，不能徹底過濾出地面；
- 第二，非地面的點容易被錯誤分類，造成非地面點缺失；
- 第三，對于目標接近激光雷達盲區的情況，會出現誤分割，即將非地面點云分割為地面。
????????通過本文我們一起學習一種新的地面分割方法，出自論文：Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications，在可靠性、分割精度方面均優于基于射線的地面分割方法。我們將其稱之為地面平面擬合方法。
2.1 地面平面擬合（Ground Plane Fitting）
????????我們采用平面模型（Plane Model）來擬合當前的地面，通常來說，由于現實的地面并不是一個“完美的”平面，而且當距離較大時激光雷達會存在一定的測量噪聲，單一的平面模型并不足以描述我們現實的地面。
????????要很好的完成地面分割，就必須要處理存在一定坡度變化的地面的情況（不能將這種坡度的變化視為非地面，不能因為坡度的存在而引入噪聲），一種簡單的處理方法就是沿著x方向（車頭的方向）將空間分割成若干個子平面，然后對每個子平面使用地面平面擬合算法（GPF）從而得到能夠處理陡坡的地面分割方法。
?
????????那么如何進行平面擬合呢？其偽代碼如下：
?????????我們來詳細的了解這一流程：對于給定的點云 P ,分割的最終結果為兩個點云集合?：地面點云 和 ：非地面點云。該算法有四個重要的參數，分別是：
- ：進行奇異值分解（singular value decomposition，SVD）的次數，也即進行優化擬合的次數；
- ：用于選取LPR的最低高度點的數量；
- ：?用于選取種子點的閾值，當點云內的點的高度小于LPR的高度加上此閾值時，我們將該點加入種子點集；
- ：平面距離閾值，我們會計算點云中每一個點到我們擬合的平面的正交投影的距離，而這個平面距離閾值，就是用來判定點是否屬于地面。
2.2 種子點集選取
????????我們首先選取一個種子點集(seed point set)，這些種子點來源于點云中高度（即z值）較小的點，種子點集被用于建立描述地面的初始平面模型，那么如何選取這個種子點集呢？
????????我們引入最低點代表（Lowest Point Representative, LPR）的概念。LPR就是? 個最低高度點的平均值，LPR 保證了平面擬合階段不受測量噪聲的影響。這個LPR被當作是整個點云 P 的最低點，點云P中高度在閾值? 內的點被當作是種子點，由這些種子點構成一個種子點集合。
????????種子點集的選取代碼如下：
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; // 點云計數// Calculate the mean height value. 因為已經排過序了，所以直接選取前最小的num_lpr個點即可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_. 得到初始地面種子點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 }????????輸入是一個點云，這個點云內的點已經沿著z方向（即高度）做了排序，取?num_lpr_?個最小點，求得高度平均值?lpr_height（即LPR），選取高度小于?lpr_height + th_seeds_?的點作為種子點。
2.3?平面模型
????????接下來我們需要確定一個平面，點云中的點到這個平面的正交投影距離小于閾值? 則認為該點屬于地面，否則屬于非地面，我們采用一個簡單的線性模型用于平面模型估計，如下：
ax+by+cz+d = 0?
也即：
其中，是所有點的均值。這個協方差矩陣 C? 描述了種子點集的散布情況，其三個奇異向量可以通過奇異值分解（Singular Value Decomposition,SVD）求得，這三個奇異向量描述了點集在三個主要方向的散布情況。由于是平面模型，垂直于平面的法向量n表示具有最小方差的方向，可以通過計算具有最小奇異值的奇異向量來求得。
????????那么在求得了 n 以后， d 可以通過代入種子點集的平均值?? (它代表屬于地面的點) 直接求得。整個平面模型計算代碼如下：
// 根據ax+by+cz+d = 0，擬合平面，最重要的是確定點與平面的投影距離閾值Th_dist，并以此判斷點是否在平面上 void PlaneGroundFilter::estimate_plane_(void) {// Create covariance matrix in single pass.// TODO: compare the efficiency.Eigen::Matrix3f cov;Eigen::Vector4f pc_mean;// 計算均值和協方差矩陣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點云的點數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'// 點云中的點到這個平面的正交投影距離小于閾值 Th_dist, 則認為該點屬于地面，否則屬于非地面th_dist_d_ = th_dist_ - d_;// return the equation parameters. }2.4 優化平面主循環?
????????在確定如何選取種子點集以及如何估計平面模型以后，我們來看一下整個算法的主循環，以下是主循環的代碼：
// 4. Extract init ground seeds. 提取初始地面點extract_initial_seeds_(laserCloudIn);g_ground_pc = g_seeds_pc;// 5. Ground plane fitter mainloopfor(int i=0; i<num_iter_; i ++){estimate_plane_(); // 用上面 估計的g_ground_pc點云 來擬合地平面g_ground_pc->clear(); // 準備重新來得到地面點云，用以進行下一次平面擬合//g_not_ground_pc->clear();// pointcloud to matrix 點云數據轉換為矩陣存儲 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 所有點與地平面法線的點乘，得到與地平面的距離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 表示地面，在所有點云中進行標簽標識，加以區分g_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨存放地面點云}else{g_all_pc->points[r].label = 0u; // mean not ground and non clusteredg_not_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨存放非地面點云}}}????????得到這個初始的平面模型以后，我們會計算點云中每一個點到該平面的正交投影的距離，即?points * normal_，并且將這個距離與設定的閾值（即th_dist_d_）比較，當高度差小于此閾值，我們認為該點屬于地面，當高度差大于此閾值，則為非地面點。經過分類以后的所有地面點被當作下一次迭代的種子點集，迭代優化。
2.5 點云過濾
????????下面我們編寫一個簡單的ROS節點實現這一地面分割算法。我們仍然采用第24篇博客（激光雷達的地面-非地面分割和pcl_ros實踐）中的bag進行實驗，這個bag來自Velodyne的VLP32C，在此算法的基礎上我們要進一步濾除雷達過近處和過高處的點，因為雷達安裝位置的原因，近處的車身反射會對Detection造成影響，需要濾除; 過高的目標，如大樹、高樓，對于無人車的雷達感知意義也不大，我們對過近過高的點進行切除，代碼如下：
// 去除過近或者過遠的點 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()函數中：
? ? ? ? 1. 數據預處理，將輸入的Msg消息轉換成pcl::pointcloud，并將點云全部存入g_all_pc中：
// 主要處理函數 void PlaneGroundFilter::point_cb(const sensor_msgs::PointCloud2ConstPtr &in_cloud_ptr) {// 1. Msg to pointcloud 數據類型轉換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;// 將輸入的所有點存放到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. 對點云按照z軸數值大小對輸入點云進行排序：
// 2. Sort on Z-axis value. 按照z軸數值大小對輸入點云進行排序sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_cmp);? ? ? ? 3. 對Z軸數據過小的點直接去除：
// 3. Error point removal 清除一部分錯誤點// 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. 提取初始地面種子點
// 4. Extract init ground seeds. 提取初始地面點extract_initial_seeds_(laserCloudIn);g_ground_pc = g_seeds_pc;? ? ? ? 5. 地面去除主循環
// 5. Ground plane fitter mainloopfor(int i=0; i<num_iter_; i ++){estimate_plane_(); // 用上面 估計的g_ground_pc點云 來擬合地平面g_ground_pc->clear(); // 準備重新來得到地面點云，用以進行下一次平面擬合//g_not_ground_pc->clear();// pointcloud to matrix 點云數據轉換為矩陣存儲 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 所有點與地平面法線的點乘，得到與地平面的距離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 表示地面，在所有點云中進行標簽標識，加以區分g_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨存放地面點云}else{g_all_pc->points[r].label = 0u; // mean not ground and non clusteredg_not_ground_pc->points.push_back(laserCloudIn_org[r]); // 單獨存放非地面點云}}}? ? ? ? 6. 對去除地面點后的點云進行后處理：
// 6. 對去除地面的點云進行后處理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 發布地面點云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; // 幀Idpub_ground_.publish(ground_msg);// publish not ground points 發布非地面點云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; // 時間戳groundless_msg.header.frame_id = in_cloud_ptr->header.frame_id; // 幀Idpub_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(); }總結
以上是生活随笔為你收集整理的两种点云地面去除方法的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 2016 年 VR 行业回顾与展望
- 下一篇： POS机龙头联迪：一款产品的质量事故引发