Udacity传感器融合笔记(二)lidar 点云处理
本篇博客主要记录lidar点云的分割以及聚类。
1.Segmentation 点云分割
对于lidar反馈回来的数据,我们总是希望从中准确的找到那些是障碍物,那些是道路点。为此,udacity课程中使用了RANSAC算法来进行点云的分割,将lidar点云分为障碍物点(外点)和道路点(内点)两类。
1.1 RANSAC
对于RANSAC算法(随即抽样一致算法),初次接触是在高博的视觉SLAM14讲中,该算法可以从一组包含“局外点”的数据中通过迭代的方式估计数学模型的参数,从而找到符合模型的“局内点”。这里不多做介绍,详细原理可以推荐一篇博客
RANSAC深度解析
在udacity的课程中,也对RANSAC的原理进行了简要介绍和仿真。RANSAC是一种检测数据中异常值的方法。 RANSAC运行最大迭代次数,并返回最合适的模型。 每次迭代都会随机选择数据的子样本,并通过它拟合模型,例如直线或平面。 然后,将具有最大数量的内部值或最低噪声的迭代用作最佳模型。一种类型的RANSAC选择适合的最小点子集。 对于一条线,这将是两个点,对于平面,则将是三个点。 然后,通过迭代遍历每个剩余点并计算其与模型的距离,来计算内部数。 距模型一定距离内的点计为内部值。 那么,具有最大内部数的迭代就是最佳模型。 这也是udacity课程中实现的版本。RANSAC的其他方法可以对模型点的某些百分比进行采样,例如占总点数的20%,然后对其拟合一条线。 然后计算该行的误差,误差最小的迭代就是最佳模型。 由于不需要考虑每次迭代的每个点,因此该方法可能具有一些优点。
1.2 RANSAC仿真
接下来是ransac的仿真部分代码,这里只记录平面点云的RANSAC分割
main函数
int main ()
{
// Create viewer 创建可视化窗口
pcl::visualization::PCLVisualizer::Ptr viewer = initScene();
// Create data 创建点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = CreateData3D();
// TODO: Change the max iteration and distance tolerance arguments for Ransac function 设定最大迭代次数和距离阈值
std::unordered_set<int> inliers = Ransac(cloud, 100, 0.5);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudInliers(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOutliers(new pcl::PointCloud<pcl::PointXYZ>());
for(int index = 0; index < cloud->points.size(); index++)
{
//对点云进行分割
pcl::PointXYZ point = cloud->points[index];
if(inliers.count(index))
cloudInliers->points.push_back(point);
else
cloudOutliers->points.push_back(point);
}
// Render 2D point cloud with inliers and outliers,对分割后的点云进行渲染 内点绿色,外点红色
if(inliers.size())
{
renderPointCloud(viewer,cloudInliers,"inliers",Color(0,1,0));
renderPointCloud(viewer,cloudOutliers,"outliers",Color(1,0,0));
}
else
{
//其他背景黑色
renderPointCloud(viewer,cloud,"data");
}
while (!viewer->wasStopped ())
{
viewer->spinOnce ();
}
}
加载pcd函数
//加载pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr CreateData3D()
{
ProcessPointClouds<pcl::PointXYZ> pointProcessor; //创建pointProcessor
return pointProcessor.loadPcd("../../../sensors/data/pcd/simpleHighway.pcd"); //加载data中的pcd文件
}
创建可视化窗口并初始化
//创建窗口并初始化函数
pcl::visualization::PCLVisualizer::Ptr initScene()
{
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer ("2D Viewer"));//创建可视化窗口
viewer->setBackgroundColor (0, 0, 0); //背景颜色黑色
viewer->initCameraParameters(); //初始化窗口参数
viewer->setCameraPosition(0, 0, 15, 0, 1, 0);//窗口位置及视角
viewer->addCoordinateSystem (1.0);//创建坐标系
return viewer;
}
RANSAC算法
std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
std::unordered_set<int> inliersResult; //创建关联容器 用于储存内点 std::unordered_set是SLT中的一种容器,基于RB-Tree结构,有机会后面介绍
srand(time(NULL));
auto startTime = std::chrono::steady_clock::now();
// 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
while(maxIterations--)
{
std::unordered_set<int> inliers; //内点容器
while(inliers.size()<3)
inliers.insert(rand()%(cloud->points.size())); //随机选3个点组成平面插入进inlier中
float x1,x2,x3,y1,y2,y3,z1,z2,z3;
auto itr = inliers.begin();//将inlier的开始地址赋给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;
//三点平面公式计算 a b c d
float a = (y2-y1)*(z3-z1)-(z2-z1)*(y3-y1);
float b = (z2-z1)*(x3-x1)-(x2-x1)*(z3-z1);
float c = (x2- x1)*(y3-y1)-(y2-y1)*(x3-x1);
float d = -(a*x1+b*y1+c*z1);
for(int index = 0;index < cloud->points.size(); index++)
{
if(inliers.count(index) > 0)
continue;
pcl::PointXYZ point = cloud->points[index];
float x3 = point.x;
float y3 = point.y;
float z3 = point.z;
//计算点到平面距离公式
float d = fabs(a*x3+b*y3+c*z3+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;
}
结果如图 红色为障碍物,绿色为道路点
在课程的工程中,直接调用了PCL库中的RANSAC功能进行点云的分割。其原理与仿真一致。
在lidar障碍物检测工程中的对应部分
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 process
auto startTime = std::chrono::steady_clock::now();
// TODO:: Fill in this function to find inliers for the cloud.
//过滤之后的点云数据
//创建分割对象 -- 检测平面参数
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers {new pcl::PointIndices}; //存储内点的点索引集合对象inliers
pcl::ModelCoefficients::Ptr coefficients{new pcl::ModelCoefficients};
seg.setOptimizeCoefficients(true); //设置对估计的模型系数需要进行优化
seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型,检测平面
seg.setMethodType(pcl::SAC_RANSAC); //设置方法(随机样本一致性)
seg.setMaxIterations(maxIterations); //最大迭代次数
seg.setDistanceThreshold(distanceThreshold); //距离阈值
seg.setInputCloud(cloud);
seg.segment(*inliers,*coefficients); //分割操作
if(inliers->indices.size() == 0)
{
std::cout<<"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<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);
return segResult;
}
这个函数在environment.cpp文件中被调用
std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr,pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI.SegmentPlane(inputCloud,25,0.3)
inputCloud为输入点云,是经过滤波处理后的,20为最大迭代次数,0.3为判定阈值,小于阈值的点判定为内点。在SegmentPlane函数最后有这样一条语句
std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);
这条语句中调用了SeparateClouds函数,这个函数也在processPointCloud.cpp文件中,函数功能是将提取点云并存为平面点云和目标物点云
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 plane
typename pcl::PointCloud<PointT>::Ptr obstCloud (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]);
pcl::ExtractIndices<PointT> extract; //ExtractIndices通过分割算法提取部分点云数据子集的下标索引
extract.setInputCloud (cloud); //设置输入点云
extract.setIndices(inliers); //提取内点
extract.setNegative(true); //提取外点,继续处理
extract.filter(*obstCloud);//将外点(即目标点)提取至obstCloud
std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(obstCloud, planeCloud);//储存两种点云
return segResult;
}
2.聚类Clustering
在对点云进行分割后,工程已经可以将lidar的返回点云数据分割成目标物(障碍物)点云和平面(车道平面)点云,接下来要完成的任务就是将障碍物的点云进行聚类,将属于一个障碍物的点云归为一类。课程中使用的聚类方法是欧氏距离聚类,这个算法是根据点之间的紧密程度来关联点组。 为了有效地进行最近邻居搜索,可以使用KD-Tree数据结构,该结构平均可以将查找时间从O(n)加快到O(log(n))。相关资料可以参考以下文章:
KD-Tree与欧式距离聚类
udacity的工程中使用的是PCL库的欧式聚类算法,但同样对采用KD-Tree结构进行欧式聚类的方法进行的简要介绍和仿真。这里不过多记录,直接看工程中的实现部分
在environment.cpp函数中下面语句调用了Clustering函数
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI.Clustering(segmentCloud.first,0.53,10,500); //障碍物点云,采用KdTree将其进行欧式聚类
Clustering函数有三个参数
参数1为输入点云,这里segmentCloud.first表示是障碍物点云,我们对障碍物点云进行聚类
参数2为设置欧式聚类的近邻搜索的搜索半径,半径越大,搜索范围越大
参数3、4为一个类中点的上限和下限
Clustering函数定义如下
std::vector<typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{
// Time clustering process
auto startTime = std::chrono::steady_clock::now();
std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;
// TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); //kd树,是一种分割k维数据空间的数据结构。
tree->setInputCloud(cloud); //输入点云
std::vector<pcl::PointIndices> clusterIndices; //找到簇的索引
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance(clusterTolerance); //设置近邻搜索的搜索半径
ec.setMinClusterSize(minSize); //设置一个类所需要的最少点
ec.setMaxClusterSize(maxSize); //设置一个类点云的上限
ec.setSearchMethod(tree); //设置点云的搜索机制,此处为KdTree
ec.setInputCloud(cloud);
ec.extract(clusterIndices); //从点云中提取聚类,并保存在点云索引clusterIndices中
for(pcl::PointIndices getIndices : clusterIndices)
{
typename pcl::PointCloud<PointT>::Ptr cloudCluster (new pcl::PointCloud<PointT>);
for(int index : getIndices.indices)
cloudCluster->push_back(cloud->points[index]);
cloudCluster->width = cloudCluster->points.size();
cloudCluster->height = 1;
cloudCluster->is_dense = true;
clusters.push_back(cloudCluster);
}
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size() << " clusters" << std::endl;
return clusters; //返回聚类
}
拿聚类仿真的图片来说明以下效果吧,同样颜色的点被分为一类。这也是KD-Tree的数据结构
至此,点云的分割和聚类操作已经完成,为方便观看,接下来要对一类的点云进行修饰,用一个立方体将一类的点云数据框起来, 这部分代码相对简单,直接上代码
Box ProcessPointClouds<PointT>::BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster)
{
// Find bounding box for one of the clusters
PointT minPoint, maxPoint;
pcl::getMinMax3D(*cluster, minPoint, maxPoint
);
Box box;
box.x_min = minPoint.x;
box.y_min = minPoint.y;
box.z_min = minPoint.z;
box.x_max = maxPoint.x;
box.y_max = maxPoint.y;
box.z_max = maxPoint.z;
return box;
}
这部分在传感器融合的第一篇文章中也记录过,不再赘述。
最后的结果就是下面这样
绿色点为车道平面点云 障碍物的点云分别渲染成了不同颜色,并对一类的点云,用立方体进行包围。每一个立方体就代表一个障碍物。
上一篇: 14.用定位的方式仿小米导航
下一篇: 常用坐标系转换实现