Lidar Obstacle Detection
一、最终效果
代码地址：Github: https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection
激光雷达数据以一种称为点云数据(简称 PCD)的格式存储. PCD 文件是(x, y, z)笛卡尔坐标和强度值的列表, 它是在一次扫描之后环境的一个快照. 使用 VLP 64激光雷达, PCD 文件将有大约256,000个(x, y, z, i)点云值。
PCL库 广泛应用于机器人技术领域, 用于处理点云数据, 网上有许多教程可供使用. PCL 中有许多内置的功能可以帮助检测障碍物. 本项目后面会使用 PCL内置的分割、提取和聚类函数.
二、代码讲解以及实物图
1.导入激光点云数据
首先我们需要流式载入激光点云数据
（1）在processPointClouds.cpp中载入pcd点云
template<typename PointT>
PtCdtr<PointT> ProcessPointClouds<PointT>::loadPcd(std::string file) {
PtCdtr<PointT> cloud(new pcl::PointCloud<PointT>);
if (pcl::io::loadPCDFile<PointT>(file, *cloud) == -1) { //* load the file
PCL_ERROR ("Couldn't read file n");
}
std::cerr << "Loaded " << cloud->points.size() << " data points from " + file << std::endl;
return cloud;
}
（2）在environment.cpp中
void cityBlock(pcl::visualization::PCLVisualizer::Ptr &viewer, ProcessPointClouds<pcl::PointXYZI> *pointProcessorI,
const pcl::PointCloud<pcl::PointXYZI>::Ptr &inputCloud) {
// ----------------------------------------------------
// -----Open 3D viewer and display City Block -----
// ----------------------------------------------------
// Setting hyper parameters
// FilterCloud
float filterRes = 0.4;
Eigen::Vector4f minpoint(-10, -6.5, -2, 1);
Eigen::Vector4f maxpoint(30, 6.5, 1, 1);
// SegmentPlane
int maxIterations = 40;
float distanceThreshold = 0.3;
// Clustering
float clusterTolerance = 0.5;
int minsize = 10;
int maxsize = 140;
// First:Filter cloud to reduce amount of points
pcl::PointCloud<pcl::PointXYZI>::Ptr filteredCloud = pointProcessorI->FilterCloud(inputCloud, filterRes, minpoint,maxpoint);
// Second: Segment the filtered cloud into obstacles and road
std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI->RansacSegmentPlane(
filteredCloud, maxIterations, distanceThreshold);
//renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
//renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));
renderPointCloud(viewer,inputCloud,"inputCloud");
// Third: Cluster different obstacle cloud
/*std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI->EuclideanClustering(segmentCloud.first,
clusterTolerance,
minsize, maxsize);*/
/*int clusterId = 0;
std::vector<Color> colors = {Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1)};
for (pcl::PointCloud<pcl::PointXYZI>::Ptr cluster : cloudClusters) {
std::cout << "cluster size";
pointProcessorI->numPoints(cluster);
renderPointCloud(viewer, cluster, "obstCLoud" + std::to_string(clusterId),
colors[clusterId % colors.size()]);*/
// Fourth: Find bounding boxes for each obstacle cluster
/* Box box = pointProcessorI->BoundingBox(cluster);
renderBox(viewer, box, clusterId);
++clusterId;*/
//}
}
int main(int argc, char **argv) {
std::cout << "starting enviroment" << std::endl;
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
CameraAngle setAngle = XY;
initCamera(setAngle, viewer);
// For simpleHighway function
// simpleHighway(viewer);
// cityBlock(viewer);
// while (!viewer->wasStopped ())
// {
// viewer->spinOnce ();
// }
//
// Stream cityBlock function
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI->streamPcd("../src/sensors/data/pcd/data_2");
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;
while (!viewer->wasStopped()) {
// Clear viewer
viewer->removeAllPointClouds();
viewer->removeAllShapes();
// Load pcd and run obstacle detection process
inputCloudI = pointProcessorI->loadPcd((*streamIterator).string());
cityBlock(viewer, pointProcessorI, inputCloudI);
streamIterator++;
if (streamIterator == stream.end()) {
streamIterator = stream.begin();
}
viewer->spinOnce();
}
}
处理点云数据的第一步就是创建一个processPointClouds对象，这个对象包含所有处理激光点云数据的模块，如过滤，分割，聚类，载入，存续从PCD数据，我们需要为不同点云数据创建一个通用模板template，在真实点云数据中，点云的类型是pcl::PointXYZI。创建pointProcessor可以建立在stack上也可以建立在heap上，建立在heap上使用指针更加轻便。
（2）最终效果
创建一个立方体网格, 过滤点云的方法是每个体素立方体内只留下一个点。
作用：保持点云形状特征的情况下减少点云的数量；在提高配准、曲面重建、形状识别等算法的速度。
原理：PCL实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格。在每个体素内，用体素中所有点的中心来近似显示体素中其他点。
（1）在processPointClouds.cpp
template<typename PointT>
PtCdtr<PointT> ProcessPointClouds<PointT>::FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint,
Eigen::Vector4f maxPoint) {
// Time segmentation process
auto startTime = std::chrono::steady_clock::now();
// TODO:: Fill in the function to do voxel grid point reduction and region based filtering
// Create the filtering object: downsample the dataset using a leaf size of .2m
pcl::VoxelGrid<PointT> vg;
PtCdtr<PointT> cloudFiltered(new pcl::PointCloud<PointT>);
vg.setInputCloud(cloud);
vg.setLeafSize(filterRes, filterRes, filterRes);
vg.filter(*cloudFiltered);
PtCdtr<PointT> cloudRegion(new pcl::PointCloud<PointT>);
pcl::CropBox<PointT> region(true);
region.setMin(minPoint);
region.setMax(maxPoint);
region.setInputCloud(cloudFiltered);
region.filter(*cloudRegion);
std::vector<int> indices;
pcl::CropBox<PointT> roof(true);
roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
roof.setInputCloud(cloudRegion);
roof.filter(indices);
pcl::PointIndices::Ptr inliers{new pcl::PointIndices};
for (int point : indices) {
inliers->indices.push_back(point);
}
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloudRegion);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloudRegion);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "filtering took " << elapsedTime.count() << " milliseconds" << std::endl;
// return cloud;
return cloudRegion;
}
在environment.cpp中
renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
int main(int argc, char **argv) {
std::cout << "starting enviroment" << std::endl;
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
CameraAngle setAngle = XY;
initCamera(setAngle, viewer);
// For simpleHighway function
// simpleHighway(viewer);
// cityBlock(viewer);
// while (!viewer->wasStopped ())
// {
// viewer->spinOnce ();
// }
//
// Stream cityBlock function
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI->streamPcd("../src/sensors/data/pcd/data_2");
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;
while (!viewer->wasStopped()) {
// Clear viewer
viewer->removeAllPointClouds();
viewer->removeAllShapes();
// Load pcd and run obstacle detection process
inputCloudI = pointProcessorI->loadPcd((*streamIterator).string());
cityBlock(viewer, pointProcessorI, inputCloudI);
streamIterator++;
if (streamIterator == stream.end()) {
streamIterator = stream.begin();
}
viewer->spinOnce();
}
}
（2）最终效果
3.分割地面点云和物体点云（Segmentation）
Segmentation的任务是将属于道路的点和属于场景的点分开.
PCL RANSAC Segmentaion
针对本项目, 我创建了两个函数SegmentPlane和SeparateClouds, 分别用来寻找点云中在道路平面的inliers(内联点)和提取点云中的outliers(物体).
（1）RANSAC（随机样本一致性） 介绍
目前粒子分割主要使用RANSAC算法. RANSAC全称Random Sample Consensus, 即随机样本一致性, 是一种检测数据中异常值的方法. RANSAC通过多次迭代, 返回最佳的模型. 每次迭代随机选取数据的一个子集, 并生成一个模型拟合这个子样本, 例如一条直线或一个平面. 然后具有最多inliers(内联点)或最低噪声的拟合模型被作为最佳模型. 如其中一种RANSAC 算法使用数据的最小可能子集作为拟合对象. 对于直线来说是两点, 对于平面来说是三点. 然后通过迭代每个剩余点并计算其到模型的距离来计算 inliers 的个数. 与模型在一定距离内的点被计算为inliers. 具有最高 inliers 数的迭代模型就是最佳模型. 这是我们在这个项目中的实现版本. 也就是说RANSAC算法通过不断迭代, 找到拟合最多inliers的模型, 而outliers被排除在外. RANSAC 的另一种方法对模型点的某个百分比进行采样, 例如20% 的总点, 然后将其拟合成一条直线. 然后计算该直线的误差, 以误差最小的迭代法为最佳模型. 这种方法的优点在于不需要考虑每次迭代每一点. 以下是使用RANSAC算法拟合一条直线的示意图, 真实激光数据下是对一个平面进行拟合, 从而分离物体和路面. 以下将单独对RANSAC平面算法进行实现.
理解:可以把三维空间看成一个平面，空间中所有点都投影到平面上，在平面上随机选取两点确定一条直线，计算其余点到这直线的距离，如果距离小于阈值，说明符合条件，以此循环往复，最后找到付着点最多的直线就行。
（2）代码介绍
在
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud) {
// TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
PtCdtr<PointT> obstCloud(new pcl::PointCloud<PointT>());
PtCdtr<PointT> planeCloud(new pcl::PointCloud<PointT>());
for (int index : inliers->indices) {
planeCloud->points.push_back(cloud->points[index]);
}
// create extraction object
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*obstCloud);
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(obstCloud,
planeCloud);
// std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult(cloud, cloud);
return segResult;
}
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceThreshold) {
// Time segmentation process
auto startTime = std::chrono::steady_clock::now();
// pcl::PointIndices::Ptr inliers; // Build on the stack
pcl::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<PointT> 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 cloud
seg.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<PtCdtr<PointT>, PtCdtr<PointT>> segResult = SeparateClouds(
inliers, cloud);
return segResult;
}
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol) {
// Count time
auto startTime = std::chrono::steady_clock::now();
srand(time(NULL));
int num_points = cloud->points.size();
auto cloud_points = cloud->points;
Ransac<PointT> RansacSeg(maxIterations, distanceTol, num_points);
// Get inliers from RANSAC implementation
std::unordered_set<int> inliersResult = RansacSeg.Ransac3d(cloud);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "plane ransac-segment took " << elapsedTime.count() << " milliseconds" << std::endl;
PtCdtr<PointT> out_plane(new pcl::PointCloud<PointT>());
PtCdtr<PointT> in_plane(new pcl::PointCloud<PointT>());
for (int i = 0; i < num_points; i++) {
PointT pt = cloud_points[i];
if (inliersResult.count(i)) {
out_plane->points.push_back(pt);
} else {
in_plane->points.push_back(pt);
}
}
return std::pair<PtCdtr<PointT>, PtCdtr<PointT>>(in_plane, out_plane);
}
在environment.cpp中
renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));
renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));
int main(int argc, char **argv) {
std::cout << "starting enviroment" << std::endl;
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
CameraAngle setAngle = XY;
initCamera(setAngle, viewer);
// For simpleHighway function
// simpleHighway(viewer);
// cityBlock(viewer);
// while (!viewer->wasStopped ())
// {
// viewer->spinOnce ();
// }
//
// Stream cityBlock function
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI->streamPcd("../src/sensors/data/pcd/data_2");
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;
while (!viewer->wasStopped()) {
// Clear viewer
viewer->removeAllPointClouds();
viewer->removeAllShapes();
// Load pcd and run obstacle detection process
inputCloudI = pointProcessorI->loadPcd((*streamIterator).string());
cityBlock(viewer, pointProcessorI, inputCloudI);
streamIterator++;
if (streamIterator == stream.end()) {
streamIterator = stream.begin();
}
viewer->spinOnce();
}
}
（3）最终效果图：
4.聚类（Clustering）
欧式聚类：
理解：对于空间中随便一点P，通过KD-Tree近邻搜索算法找到k个离p点最近的点，这些点中距离小于设定阈值的便聚类到集合Q中，如果Q中元素数目不在增加，整个聚类过程结束，得到一个物体的聚类点云，循环以上过程，找出所有物体的聚类点云。
KD-Tree划分为左子树和右子树，左子树上存放符合距离阈值的点，右子树上存放不符合距离阈值的点。
文章出处登录后可见！
已经登录？立即刷新
