Lidar系列文章

传感器融合是将多个传感器采集的数据进行融合处理，以更好感知周围环境；这里首先介绍激光雷达的相关内容，包括激光雷达基本介绍（本节内容），激光点云数据处理方法（点云数据显示，点云分割，点云聚类，障碍物识别实例）等。

系列文章目录

1. 激光雷达基本介绍
2. 激光点云数据显示
3. 基于RANSAC的激光点云分割
4. 点云分割入门级实例学习
5. 激光点云目标物聚类
6. 基于PCL实现欧式聚类提取
7. 激光雷达障碍物识别

文章目录

Lidar系列文章
- RANSAC随机抽样一致简介
- 实例-使用RANSAC拟合直线
- 实例-使用RANSAC拟合平面
- 使用PCL点云库分割平面

RANSAC随机抽样一致简介

RANSAC（RANdom SAmple Consensus）随机抽样一致，最早于1981年由Fischler和Bolles提出，基本思想是根据一组包含噪声、外点的各种缺陷的样本数据集中，估计出数据的数学模型参数，并同时得到有效样本数据。

RANSAC从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。每次迭代时选取一个数据子集（线：2个数据点，面：三个数据点）并对其进行拟合，计算剩余点到拟合后的模型（线/面）的距离，包含最多数量的“局内点”（距离模型一定范围内的点）或最小的噪声作为最佳模型。

类似的RANSAC方法也包括先对数据集进行采样（如20%数据），对其进行拟合并计算偏差，偏差最小的为最佳模型。这种方法的优势是每次迭代中不用考虑所有点。

实例-使用RANSAC拟合直线

对于平面中的点，RANSAC方法拟合直线的主要步骤如下：

每次迭代中从数据集中随机采样2个点；
基于两个采样点拟合一条直线；
计算每个点到拟合直线的距离，容差内的点为内点。
每次迭代过程中跟踪拟合的直线，保留内点最多的拟合直线，即为最佳的模型。

下面将以RANSAC拟合2D点云中的直线进行介绍。下图是对数据集进行采样的子集。

直线公式：
Ax+By+C=0Ax+By+C=0Ax+By+C=0
设其经过点1（x1, y1）和点2（x2, y2），则经过这两点的直线公式为：
(y1−y2)x+(x2−x1)y+(x1∗y2−x2∗y1)=0(y1−y2)x+(x2−x1)y+(x1∗y2−x2∗y1)=0(y1−y2)x+(x2−x1)y+(x1∗y2−x2∗y1)=0
平面内点（x，y）到直线的距离d：
d=∣Ax+By+C∣/sqrt(A2+B2)d=∣Ax+By+C∣/sqrt(A^2 +B^2 )d=∣Ax+By+C∣/sqrt(A2+B2)

std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{std::unordered_set<int> inliersResult;srand(time(NULL)); //srand() generate a random seed associate with current time.// TODO: Fill in this function// For max iterations // Randomly sample subset and fit line// Measure distance between every point and fitted line// If distance is smaller than threshold count it as inlier// Return indicies of inliers from fitted line with most inliers// Time segmentation processauto startTime = std::chrono::steady_clock::now();while(maxIterations--)// For max iterations {//Randomly pick two points;std::unordered_set<int> inliers;while(inliers.size()<2)inliers.insert(rand()%(cloud->points.size()));float x1, y1, x2, y2;auto itr = inliers.begin();x1 = cloud->points[*itr].x;y1 = cloud->points[*itr].y;itr++;x2 = cloud->points[*itr].x;y2 = cloud->points[*itr].y;//Line formula ax + by + c = 0float a = y1 - y2;float b = x2 - x1;float c = x1*y2 - x2*y1;// Measure distance between every point and fitted line// If distance is smaller than threshold count it as inlierfor(int index=0; index < cloud->points.size(); index++){//discard previous two pointsif(inliers.count(index)>0)continue;pcl::PointXYZ point = cloud->points[index];float x3 = point.x;float y3 = point.y;//Calculate distancefloat d = fabs(a*x3+b*y3+c)/sqrt(a*a+b*b);if(d<=distanceTol)inliers.insert(index);}if(inliers.size()>inliersResult.size())inliersResult = inliers;}auto endTime = std::chrono::steady_clock::now();auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);std::cout << "Ransac took " << elapsedTime.count() << " milliseconds" << std::endl;return inliersResult;// Return indicies of inliers from fitted line with most inliers
}

函数调用如下：

std::unordered_set<int> inliers = Ransac(cloud, 10, 1.2);

所得结果如下图所示。

实例-使用RANSAC拟合平面

下面将介绍使用RANSAC拟合平面。
平面公式：
Ax+By+Cz+D=0Ax+By+Cz+D=0Ax+By+Cz+D=0
设平面中三点P1(x1,y1,z1)(x1,y1,z1)(x1,y1,z1)，P2(x2,y2,z2)(x2,y2,z2)(x2,y2,z2)，P3(x3,y3,z3)(x3,y3,z3)(x3,y3,z3)构成平面。

P1到P2的向量 v1=<x2−x1,y2−y1,z2−z1>v1=<x2−x1,y2−y1,z2−z1>v1=<x2−x1,y2−y1,z2−z1>
P1到P3的向量 v2=<x3−x1,y3−y1,z3−z1>v2=<x3−x1,y3−y1,z3−z1>v2=<x3−x1,y3−y1,z3−z1>

v1v1v1和V2V2V2的向量积
v1×v2=<i,j,k>=<(y2−y1)(z3−z1)−(z2−z1)(y3−y1),(z2−z1)(x3−x1)−(x2−x1)(z3−z1),(x2−x1)(y3−y1)−(y2−y1)(x3−x1)>v1 \times v2 = <i, j, k> = <(y2-y1)(z3-z1)-(z2-z1)(y3-y1), (z2-z1)(x3-x1)-(x2-x1)(z3-z1), (x2-x1)(y3-y1)-(y2-y1)(x3-x1)>v1×v2=<i,j,k>=<(y2−y1)(z3−z1)−(z2−z1)(y3−y1),(z2−z1)(x3−x1)−(x2−x1)(z3−z1),(x2−x1)(y3−y1)−(y2−y1)(x3−x1)>

i(x−x1)+j(y−y1)+k(z−z1)=0i(x-x1)+j(y-y1)+k(z-z1) = 0i(x−x1)+j(y−y1)+k(z−z1)=0
ix+jy+kz−(ix1+jy1+kz1)=0ix + jy + kz -( ix1 + jy1 + kz1 ) = 0ix+jy+kz−(ix1+jy1+kz1)=0
A=iA=iA=i
B=jB=jB=j
c=kc=kc=k
D=−(ix1+jy1+kz1)D = -( ix1 + jy1 + kz1 )D=−(ix1+jy1+kz1)
空间中点（x，y，z）到平面的距离d：
d=∣A∗x+B∗y+C∗z+D∣/sqrt(A2+B2+C2)d = |A*x+B*y+C*z+D|/sqrt(A^2+B^2+C^2)d=∣A∗x+B∗y+C∗z+D∣/sqrt(A2+B2+C2)

std::unordered_set<int> Ransac3D(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{std::unordered_set<int> inliersResult;srand(time(NULL)); //srand() generate a random seed associate with current time.// TODO: Fill in this function// For max iterations, Randomly sample subset and fit plane, // Measure distance between every point and fitted plane,// If distance is smaller than threshold count it as inlier,// Return indicies of inliers from fitted plane with most inliers// Time segmentation processauto startTime = std::chrono::steady_clock::now();while(maxIterations--)// For max iterations {//Randomly pick two points;std::unordered_set<int> inliers;while(inliers.size()<3)inliers.insert(rand()%(cloud->points.size()));float x1, y1, z1, x2, y2, z2, x3, y3, z3;auto itr = inliers.begin();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;//plane formula ax + by + cz +d = 0float a = (y2-y1)*(z3-z1)-(z2-z1)*(y3-y1); //a=i=(y2−y1)(z3−z1)−(z2−z1)(y3−y1)float b = (z2-z1)*(x3-x1)-(x2-x1)*(z3-z1); //b=j=(z2-z1)(x3-x1)-(x2-x1)(z3-z1)float c = (x2-x1)*(y3-y1)-(y2-y1)*(x3-x1); //c=k=(x2−x1)(y3−y1)−(y2−y1)(x3−x1)float d = -(a*x1+b*y1+c*z1);// Measure distance between every point and fitted line// If distance is smaller than threshold count it as inlierfor(int index=0; index < cloud->points.size(); index++){if(inliers.count(index)>0)continue;pcl::PointXYZ point = cloud->points[index];float x4 = point.x;float y4 = point.y;float z4 = point.z;float d = fabs(a*x4+b*y4+c*z4+d)/sqrt(a*a+b*b+c*c);if(d<=distanceTol)inliers.insert(index);}if(inliers.size()>inliersResult.size())inliersResult = inliers;    }auto endTime = std::chrono::steady_clock::now();auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);std::cout << "Ransac took " << elapsedTime.count() << " milliseconds" << std::endl;return inliersResult;// Return indicies of inliers from fitted line with most inliers
}

函数调用如下：

std::unordered_set<int> inliers = Ransac3D(cloud, 20, 0.3);

所得结果如下图所示。

使用PCL点云库分割平面

下面我们将使用PCL点云库中的点云分割函数对激光点云进行分割。该分割算法拟合一个平面，并确定点云是否属于平面，设置的容差（distanceThreshold）越大，属于平面的点就越多。
首先基于PCL点云库的segmentation进行点云分割，获取属于拟合平面的内点（inliers）。然后分离出地面点云（拟合平面点云）和障碍物点云（非拟合平面点云）。

点云分割函数SegmentPlane如下：

template<typename PointT>
std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SegmentPlane(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceThreshold)
{// Time segmentation processauto startTime = std::chrono::steady_clock::now();//pcl::PointIndices::Ptr inliers; //Initiate on the stack, not use;// TODO:: Fill in this function to find inliers for the cloud.//segment cloud into two parts, the driveable plane and obstacles.// Create the segmentation objectpcl::SACSegmentation<PointT> seg;pcl::ModelCoefficients::Ptr coefficients {new pcl::ModelCoefficients};pcl::PointIndices::Ptr inliers {new pcl::PointIndices};//pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());//pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());// Optionalseg.setOptimizeCoefficients (true);// Mandatoryseg.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, *coefficients);if(inliers->indices.size() == 0){std::cout<<"Could not estimate a planner model for the given dateset."<<std::endl;}//std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);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<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);return segResult;
}

分离出地面点云（拟合平面点云）和障碍物点云（非拟合平面点云）的SeparateClouds函数定义如下。

template<typename PointT>
std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud)
{// TODO: Create two new point clouds, one cloud with obstacles and other with segmented planetypename pcl::PointCloud<PointT>::Ptr obstcCoud (new pcl::PointCloud<PointT>());typename pcl::PointCloud<PointT>::Ptr planeCloud (new pcl::PointCloud<PointT>());for(int index: inliers->indices)planeCloud->points.push_back(cloud->points[index]);// Extract the inlierspcl::ExtractIndices<PointT> extract;extract.setInputCloud (cloud);extract.setIndices (inliers);extract.setNegative (true);extract.filter (*obstcCoud);std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(obstcCoud, planeCloud);//std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(cloud, cloud);return segResult;
}

函数调用如下，即可实现分割及分离地面点云（绿色）和障碍物点云（红色）；

std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentCloud = pointProcessor.SegmentPlane(inputCloud, 100, 0.2);
renderPointCloud(viewer,segmentCloud.first,"obstCloud",Color(1,0,0));
renderPointCloud(viewer,segmentCloud.second,"planeCloud",Color(0,1,0));

Segment and separating point clouds: road points in green, and other obstacle points in red

资源链接：RANSAC激光点云分割

基于RANSAC的激光点云分割相关推荐

基于八叉树的区域增长点云分割算法
基于八叉树的区域增长点云分割算法 1. 提出问题激光雷达探测到城市环境的物体表面构成三维几何点,相应的点云分割技术常用于建筑物重建.由于建筑物的复杂性,数据分割计算量很大,传统的点云分割方法需人工干 ...
基于深度学习的点云分割网络及点云分割数据集
作者丨泡椒味的泡泡糖来源丨深蓝AI 引言点云分割是根据空间.几何和纹理等特征对点云进行划分,使得同一划分内的点云拥有相似的特征.点云的有效分割是许多应用的前提,例如在三维重建领域,需要对场景内的物 ...
前沿丨基于深度学习的点云分割网络及点云分割数据集
众所周知,点云的有效分割是许多应用的前提,例如在三维重建领域,需要对场景内的物体首先进行分类处理,然后才能进行后期的识别和重建.传统的点云分割主要依赖聚类算法和基于随机采样一致性的分割算法,在很多技术 ...
大盘点 | 基于深度学习的点云分割网络及点云分割数据集
编辑 | 深蓝前沿点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取后台回复[数据集下载]获取计算机视觉近30种数据集! 引言点云分割是根据空间.几何和纹理 ...
【点云处理】基于欧式聚类的点云分割
对于范围较广的点云来说,一开始先使用基于模型的点云分割方法将类似于平面这样的点云块提出来,然后在对留下的小部分点云进行像欧式聚类这样的后续分割处理. 原始点云: 代码,对代码的理解都注释上了,以便于以 ...
CloudComparePCL 基于超体素的点云分割
文章目录一.原理概述二.实现过程三.实现效果参考资料一.原理概述一般而言,孤立的点并没有什么意义,只有许多点组合在一起形成一种形状,这样才能对我们有所意义.二维图像处理领域中,很早就出现了 ...
基于 RANSAC 及其改进的面片分割
源代码展示: //经典ransac void main01() {IO IOexample;RANSAC RANSACExample;EuclideanCluster RGExample;c ...
基于图像和激光的多模态点云融合与视觉定位【100010392】
基于图像和激光的多模态点云融合与视觉定位第 1 章引言 1.1 研究背景及意义人工智能(ArtificialIntelligence)在过去十几年来的蓬勃发展让现实生活中的许多领域变得日趋无人化 ...
PCL 点云分割与分类 Segmentation RANSAC随机采样一致性平面模型分割欧氏距离分割区域聚类分割算法最小分割算法超体聚类渐进式形态学滤波器
点云分割博文末尾支持二维码赞赏哦 _ 点云分割是根据空间,几何和纹理等特征对点云进行划分, 使得同一划分内的点云拥有相似的特征,点云的有效分割往往是许多应用的前提, 例如逆向工作,CAD领域对零件的 ...

基于RANSAC的激光点云分割