Segmentation的任務是將屬于道路的點和屬于場景的點分開，在該分割部分作者工使用了兩種方法：

第一種：

下圖中的12分別是第一種程序保存的點云結果。

下面是保存的點云的可視化結果：

針對本項目, 創建了兩個函數SegmentPlane和SeparateClouds, 分別用來尋找點云中在道路平面的inliers(內聯點)和提取點云中的outliers(物體).

在SegmentPlane函數中我們接受點云、最大迭代次數和距離容忍度作為參數, 使用std::pair對象來儲存物體和道路路面的點云. SeparateClouds 函數提取點云中的非inliers點, 即obstacles點. 粒子分割是一個迭代的過程, ?更多的迭代有機會返回更好的結果, 但同時需要更長的時間. 過大的距離容忍度會導致不是一個物體的點云被當成同一個物體, ?而過小的距離容忍度會導致一個較長的物體無法被當成同一個物體, 比如卡車.

頭文件：

//segmentation.h
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/visualization/cloud_viewer.h>
#include<vector>
#include <iostream>
#include <string>
#include<Eigen/Core>
#include<Eigen/Dense>
#include <pcl/common/common.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <unordered_set>
#include <ctime>
#include <chrono>using namespace std;
using namespace pcl;struct Color  //結構體中使用構造函數初始化列表
{float r, g, b;//Color(float setR, float setG, float setB): r(setR), g(setG), b(setB){}  //構造函數初始化列表Color(float setR, float setG, float setB)    //含有參數的構造函數，以便創建Color變量時不向其傳遞參數時，提供默認值{r = setR;g = setG;b = setB;}
};
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SegmentPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol);
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SeparateClouds(pcl::PointIndices::Ptr inliers, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color = Color(1, 1, 1));
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color = Color(-1, -1, -1));

源文件：

#include "segmentation.h"
//——————————————(2.1)————————————此處使用了pcl點云庫中的模型分割 程序中貌似沒有用到   ok
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SegmentPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceThreshold)
{// Time segmentation processauto startTime = std::chrono::steady_clock::now();//	pcl::PointIndices::Ptr inliers; // Build on the stackpcl::PointIndices::Ptr inliers(new pcl::PointIndices); // Build on the heap
// TODO:: Fill in this function to find inliers for the cloud.pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);pcl::SACSegmentation<pcl::PointXYZ> seg;//創建分割對象
//可選seg.setOptimizeCoefficients(true);//設置對估計的模型參數進行優化處理
//必選seg.setModelType(pcl::SACMODEL_PLANE);設置分割模型類別seg.setMethodType(pcl::SAC_RANSAC);//設置用哪個隨機參數估計方法seg.setMaxIterations(maxIterations);設置最大迭代次數seg.setDistanceThreshold(distanceThreshold);//設置判斷是否為模型內點的距離閾值// Segment the largest planar component from the remaining cloudseg.setInputCloud(cloud);seg.segment(*inliers, *coefficient);if (inliers->indices.size() == 0) 
{std::cerr << "Could not estimate a planar model for the given dataset" << std::endl;}auto endTime = std::chrono::steady_clock::now();auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segResult = SeparateClouds(inliers, cloud);return segResult;
}//———————————————(2.2)———————————對點云庫中的模型分割，將點云保存為兩個文件  主程序中依然好像沒有用到  ok
std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> SeparateClouds(pcl::PointIndices::Ptr inliers, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{//創建了兩個點云塊，一個存放障礙物，一個存放地面 pcl::PointCloud<pcl::PointXYZ>::Ptr obstCloud(new pcl::PointCloud<pcl::PointXYZ>());pcl::PointCloud<pcl::PointXYZ>::Ptr planeCloud(new pcl::PointCloud<pcl::PointXYZ>());for (int index : inliers->indices){planeCloud->points.push_back(cloud->points[index]);}// create extraction objectpcl::ExtractIndices<pcl::PointXYZ> extract;//創建點云提取對象extract.setInputCloud(cloud);//設置輸入點云extract.setIndices(inliers);//設置分割后的內點為需要提取的點集extract.setNegative(true);//true是保留外點extract.filter(*obstCloud);//提取輸出存儲到obstCloudstd::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segResult(obstCloud,planeCloud);pcl::PCDWriter writer;	//只會寫入obstCloudwriter.write("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\obstCloud.pcd", *obstCloud);extract.setNegative(false);//設置提取內點而非外點extract.filter(*planeCloud);//提取輸出存儲到planeCloudwriter.write("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\planeCloud.pcd", *planeCloud);//    std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(cloud, cloud);return segResult;
}void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color)
{viewer->addPointCloud<pcl::PointXYZ>(cloud, name);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color)
{if (color.r == -1){// Select color based off of cloud intensitypcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud, "intensity");viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, name);}else{// Select color based off input valueviewer->addPointCloud<pcl::PointXYZI>(cloud, name);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);}viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
}

主函數：

#include "filter.h"
int main(int argc, char **argv)
{bool render_obst = false;bool render_plane = false;// 1、輸入的是  濾波后存入的點云filter.pcd	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader;reader.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\filter.pcd", *inputCloud);pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("顯示窗口"));  //窗口顯示點云	std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentCloud = SegmentPlane(inputCloud, 100, 0.2);//-------------------------(2.1)if (render_obst) {renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));}if (render_plane) {renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));}viewer->addPointCloud(segmentCloud.first, "*cloud");viewer->resetCamera();		system("pause");	return (0);}

第二種：基于Manual RANSAC Segmentation實現的分割效果展示：

代碼涉及到的知識：

C++ unordered_set定義及初始化詳解：http://c.biancheng.net/view/546.html

容器中的count()函數：https://blog.csdn.net/lyj2014211626/article/details/69615514

count函數的功能是：統計容器中等于value元素的個數。count(first,last,value); first是容器的首迭代器，last是容器的末迭代器，value是詢問的元素。

下圖代表的是輸入，是在上個博客處理過的濾波后的點云：

下圖表示的是分割的效果。

保存的out_plane結果：

保存的in_plane結果：

代碼展示：

目前粒子分割主要使用RANSAC算法. RANSAC全稱Random Sample Consensus, 即隨機樣本一致性, 是一種檢測數據中異常值的方法. ?RANSAC通過多次迭代, 返回最佳的模型. ?每次迭代隨機選取數據的一個子集, 并生成一個模型擬合這個子樣本, 例如一條直線或一個平面. ?然后具有最多inliers(內聯點)或最低噪聲的擬合模型被作為最佳模型. 如其中一種RANSAC 算法使用數據的最小可能子集作為擬合對象. ?對于直線來說是兩點, 對于平面來說是三點. ?然后通過迭代每個剩余點并計算其到模型的距離來計算 inliers 的個數. ?與模型在一定距離內的點被計算為inliers. ?具有最高 inliers 數的迭代模型就是最佳模型. ?這是我們在這個項目中的實現版本. 也就是說RANSAC算法通過不斷迭代, 找到擬合最多inliers的模型, 而outliers被排除在外. RANSAC 的另一種方法對模型點的某個百分比進行采樣, 例如20% 的總點, 然后將其擬合成一條直線. ?然后計算該直線的誤差, 以誤差最小的迭代法為最佳模型. ?這種方法的優點在于不需要考慮每次迭代每一點. 以下是使用RANSAC算法擬合一條直線的示意圖, 真實激光數據下是對一個平面進行擬合, 從而分離物體和路面. 以下將單獨對RANSAC平面算法進行實現.

以下為平面RANSAC的主體代碼。（但在實際中PCL已經內置了RANSAC函數, 而且比寫的這個計算速度更快, 所以直接用內置的就行了.）

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <unordered_set>
#include<cstdlib>using namespace std;int main(){pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader;reader.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\filter.pcd", *cloud);int num_points = cloud->points.size();cout <<"輸入點云的總點數："<< num_points << endl; //2063std::unordered_set<int> inliersResult;//是個容器int maxIterations = 40; //迭代次數while (maxIterations--)  // {std::unordered_set<int> inliers;  //存放平面的內點，平面上的點也屬于平面的內點//因為一開始定義inliers，內點并沒有存在，所以隨機在原始點云中隨機選取了三個點作為點云的求取平面系數的三個點while (inliers.size() < 3)  //當內點小于3 就隨機選取一個點 放入內點中 也就是需要利用到三個內點{inliers.insert(rand() % num_points);   //產生 0~num_points 中的一個隨機數}//是為了隨機的選取三個點// 需要至少三個點 才能找到地面float x1, y1, z1, x2, y2, z2, x3, y3, z3;auto itr = inliers.begin();  //auto 自動類型  //itr是個迭代器x1 = cloud->points[*itr].x;y1 = cloud->points[*itr].y;z1 = cloud->points[*itr].z;itr++;x2 = cloud->points[*itr].x;y2 = cloud->points[*itr].y;z2 = cloud->points[*itr].z;itr++;x3 = cloud->points[*itr].x;y3 = cloud->points[*itr].y;z3 = cloud->points[*itr].z;//計算平面系數float a, b, c, d, sqrt_abc;a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);d = -(a * x1 + b * y1 + c * z1);sqrt_abc = sqrt(a * a + b * b + c * c);//分別計算這些點到平面的距離 for (int i = 0; i < num_points; i++)//遍歷輸入點云內的所有點，判斷該點距離找到平面的距離是否在閾值范圍內{if (inliers.count(i) > 0) //判斷一下有沒有內點{ // that means: if the inlier in already exist, we dont need do anymorecontinue;}pcl::PointXYZ point = cloud->points[i];float x = point.x;float y = point.y;float z = point.z;float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and planefloat distanceTol = 0.3;if (dist < distanceTol)//如果距離平面的距離小于設定的閾值就把他歸為平面上的點{inliers.insert(i); //如果點云中的點 距離平面距離的點比較遠 那么該點則視為內點}//將inliersResult 中的內容不斷更新，因為地面的點一定是最多的，所以迭代40次 找到的inliersResult最大時（即找到最大的那個平面），也就相當于找到了地面//inliersResult 中存儲的也就是地面上的點云if (inliers.size() > inliersResult.size()){inliersResult = inliers;}}//cout <<"每次迭代提取的平面點數："<< inliers.size() << endl;}//迭代結束后，所有屬于平面上的內點都存放在inliersResult中//std::unordered_set<int> inliersResult;cout << "總迭代完成后找到的最大平面點數："<< inliersResult.size() << endl;  //1633//創建兩片點云，一片存放地面，一片存放非地面的其他點-----------111111111111111/*pcl::PointCloud<pcl::PointXYZ>::Ptr out_plane(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr in_plane(new pcl::PointCloud<pcl::PointXYZ>);*/pcl::PointCloud<pcl::PointXYZ> out_plane;//此處是反著定義的，out_plane代表的是平面pcl::PointCloud<pcl::PointXYZ> in_plane;//此處是反著定義的，in_plane代表的是非平面for (int i = 0; i < num_points; i++)//遍歷點云中所有的點{pcl::PointXYZ pt = cloud->points[i];if (inliersResult.count(i))//if非0為真；容器中的count函數是統計容器中等于value元素的個數；count(first,last,value); first是容器的首迭代器，last是容器的末迭代器，value是詢問的元素。{//-----------111111111111111//out_plane->points.push_back(pt);out_plane.push_back(pt);//if為真表明遍歷到的該點是存在于查找到的平面上的點，則將該點保存到創建的平面點云中。}else{//-----------111111111111111//in_plane->points.push_back(pt);in_plane.push_back(pt);}}使用非ptr得點云定義在使用write函數時會很容易保存成功。如果定義ptr形式得out_cloud,in_cloud，在寫入時有時間會不成功但是也不保存只是調試得時候可以看見異常pcl::PCDWriter writer;//保存提取的點云文件writer.write("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\out_plane.pcd", out_plane);writer.write("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\in_plane.pcd", in_plane);使用非指針的點云結構就是不是ptr定義的cloud的話，就不能用viewer,至于為什么個人覺得viewer->addPointCloud()這個函數只接受指針點云傳入//pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("顯示窗口"));  //窗口顯示點云//viewer->addPointCloud(in_plane, "*cloud");//viewer->resetCamera();		//相機點重置//viewer->spin();system("pause");return (0);}

?

總結

以上是生活随笔為你收集整理的读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（3）：Segmentation的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。