读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（3）：Segmentation

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相关推荐

读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（4）：Clustering（欧式聚类）
在第(3)实现了地面点与障碍物的分离,此部分要实现的是聚类,聚类是指把不同物体的点云分别组合聚集起来, 从而能让你跟踪汽车, 行人等多个目标. 其中一种对点云数据进行分组和聚类的方法称为欧氏聚类. 欧 ...
读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（2）：点云滤波FilterCloud（）函数
FilterCloud()所包括的功能: 1.首先使用体素滤波(相当于做稀释减少点的数量)(体素网格过滤将创建一个立方体网格, 过滤点云的方法是每个体素立方体内只留下一个点, 因此立方体每一边的长度越 ...
读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（1）：Stream PCD流式载入激光点云数据
首先贴一下大佬的github链接:https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection 知乎专栏:https://www.zhihu ...
自动驾驶激光雷达物体检测技术
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:william 链接:https://zhuanlan.zhihu.com/p/12851117 ...
自动驾驶感知——物体检测与跟踪算法|4D毫米波雷达
文章目录 1. 物体检测与跟踪算法 1.1 DBSCAN 1.2 卡尔曼滤波 2. 毫米波雷达公开数据库的未来发展方向 3. 4D毫米波雷达特点及发展趋势 3.1 4D毫米波雷达特点 3.1.1 FM ...
自动驾驶感知——激光雷达物体检测算法
文章目录 1. 基于激光雷达的物体检测 1.1 物体检测的输入与输出 1.2 点云数据库 1.3 激光雷达物体检测算法 1.3.1 点视图 1.3.1.1 PointNet 1.3.1.2 Point ...
基于激光雷达的自动驾驶车辆障碍物检测研究的选题意义和目的
回答:基于激光雷达的自动驾驶车辆障碍物检测研究的选题意义在于提升驾驶安全,降低交通事故的发生率,实现智能驾驶.目的则是探索激光雷达技术在自动驾驶车辆障碍物检测中的应用,开发出高效.准确的检测算法,为智 ...
无人驾驶、自动驾驶MDC、车联网技术报告
20191013文章不断更新中..... 2019中国汽车后市场白皮书欧洲电动汽车2019-2025 2019年中国电动汽车充电桩行业研究报告-前瞻产业研究院宝马汽车安全评估报告国际车载网络安全 ...
特征级融合_自动驾驶多传感器融合技术浅析
文章转自公众号:计算机视觉之路原文链接: 头条 | 自动驾驶多传感器融合技术浅析mp.weixin.qq.com 自动驾驶车上使用了多种多样的传感器,不同类型的传感器间在功用上互相补充,提高自动驾 ...

读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（3）：Segmentation

读自动驾驶激光雷达物体检测技术（Lidar Obstacle Detection）（3）：Segmentation相关推荐

最新文章

热门文章