processPointClouds.h
// PCL lib Functions for processing point clouds
#ifndef PROCESSPOINTCLOUDS_H_
#define PROCESSPOINTCLOUDS_H_
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.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 <iostream>
#include <string>
#include <vector>
#include <ctime>
#include <chrono>
#include "box.h"
#include "ransac3d.cpp"
#include "cluster3d.cpp"
//命名空间的使用:避免了名字相同函数和变量的冲突
namespace lidar_obstacle_detection {
// shorthand for point cloud pointer
template<typename PointT>
using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;
template<typename PointT>
class ProcessPointClouds {
public:
//constructor
ProcessPointClouds();
//deconstructor
~ProcessPointClouds();
void numPoints(PtCdtr<PointT> cloud);
//1、点云滤波 ok
PtCdtr<PointT> FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint);
//——————————————————————————对点云库中的模型分割,将点云保存为两个文件 主程序中依然好像没有用到 ok
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud);
//2、地面与障碍物的分割 两块点云分别保存在in_plane(平面外的点), out_plane(平面内的点)中 ok
std::pair< PtCdtr<PointT>, PtCdtr<PointT> > RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);
// ——————————————————————————此处使用了pcl点云库中的模型分割 程序中貌似没有用到 ok
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);
//3、重写欧式聚类算法聚类障碍物点云 ok
std::vector<PtCdtr<PointT>> EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize);
//——————————————————————————此处使用了点云库中的欧氏距离分割模型 程序中没有用到 ok
std::vector<PtCdtr<PointT>> Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize);
Box BoundingBox(PtCdtr<PointT> cluster);
void savePcd(PtCdtr<PointT> cloud, std::string file);
PtCdtr<PointT> loadPcd(std::string file);
std::vector<boost::filesystem::path> streamPcd(std::string dataPath);
};
}
#endif /* PROCESSPOINTCLOUDS_H_ */
processPointClouds.cpp
// PCL lib Functions for processing point clouds
#include "processPointClouds.h"
using namespace lidar_obstacle_detection;
//constructor: 构造函数
template<typename PointT>
ProcessPointClouds<PointT>::ProcessPointClouds() {}
//de-constructor: 析构函数
template<typename PointT>
ProcessPointClouds<PointT>::~ProcessPointClouds() {}
template<typename PointT>
void ProcessPointClouds<PointT>::numPoints(PtCdtr<PointT> cloud) { std::cout << cloud->points.size() << std::endl; }
//2、随机采样一致性分割 地面与障碍物的分割 两块点云分别保存在in_plane(平面外的点), out_plane(平面内的点)中 ok
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> //pair可以将两种不同类型的值合为一个值
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();
Ransac<PointT> RansacSeg(maxIterations, distanceTol, num_points); //使用Ransac<PointT>类创建了一个对象RansacSeg
// Get inliers from RANSAC implementation
std::unordered_set<int> inliersResult = RansacSeg.Ransac3d(cloud); //使用RansacSeg对象实现函数
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;
//此处表示不清 这里out_plane表示平面中的点 in_plane表示平面外中的点
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);
}
//1、点云滤波 ok
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();
//使用体素滤波下采样
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);
//提取车身周围范围内的所有的点,并将提取到的所有点保存在indices中
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); //false 提取内点也就是提取车身周围的几个点,, 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 cloudRegion;
}
//——————————————————————————对点云库中的模型分割,将点云保存为两个文件 主程序中依然好像没有用到 ok
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud)
{
//创建了两个点云块,一个存放障碍物,一个存放地面
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;
}
//——————————————————————————此处使用了pcl点云库中的模型分割 程序中貌似没有用到 ok
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;
}
//——————————————————————————此处使用了点云库中的欧氏距离分割模型 程序中没有用到 ok
template<typename PointT>
std::vector<PtCdtr<PointT>>
ProcessPointClouds<PointT>::Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize) {
// Time clustering process
auto startTime = std::chrono::steady_clock::now();
std::vector<PtCdtr<PointT>> clusters; //保存分割后的所有类 每一类为一个点云
// 欧式聚类对检测到的障碍物进行分组
typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); //对cloud点云创建kdtree
tree->setInputCloud(cloud);
std::vector<pcl::PointIndices> clusterIndices; // 创建索引类型对象
pcl::EuclideanClusterExtraction<PointT> ec; // 欧式聚类对象
ec.setClusterTolerance(clusterTolerance); //设置近邻搜索半径
ec.setMinClusterSize(minSize); //设置一个类需要的最小的点数
ec.setMaxClusterSize(maxSize); //设置一个类需要的最大的点数
ec.setSearchMethod(tree); //设置搜索方法
ec.setInputCloud(cloud); // feed point cloud
ec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices
// 将得到的所有类的索引分别在点云中找到,即每一个索引形成了一个类
for (pcl::PointIndices getIndices : clusterIndices)
{
PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);
// For each point indice in each cluster
for (int index : getIndices.indices)
{
cloudCluster->points.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;
}
//3、欧式聚类函数: 对分割掉地面点云的障碍物点云进行欧式聚类 传入 segmentCloud.first, "obstCloud" 0.5 10 140
template<typename PointT>
std::vector<PtCdtr<PointT>>
ProcessPointClouds<PointT>::EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize,int maxSize)
{
// Time clustering process
auto startTime = std::chrono::steady_clock::now();
// 创建聚类对象 实例化ClusterPts类 创建对象clusterPoints
ClusterPts<PointT> clusterPoints(cloud->points.size(), clusterTolerance, minSize, maxSize);
//对象调用欧式聚类函数 clusters 保存了类数的所有点云
std::vector<PtCdtr<PointT>> clusters = clusterPoints.EuclidCluster(cloud);
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "KDTree clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size()<< " clusters" << std::endl;
return clusters;
}
//4、框出每个类
template<typename PointT>
Box ProcessPointClouds<PointT>::BoundingBox(PtCdtr<PointT> cluster)
{
// Find bounding box for one of the clusters
PointT minPoint, maxPoint;
pcl::getMinMax3D(*cluster, minPoint, maxPoint); //想得到它x,y,z三个轴上的最大值和最小值
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;
}
template<typename PointT>
void ProcessPointClouds<PointT>::savePcd(PtCdtr<PointT> cloud, std::string file) {
pcl::io::savePCDFileASCII(file, *cloud);
std::cerr << "Saved " << cloud->points.size() << " data points to " + file << std::endl;
}
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;
}
template<typename PointT>
std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath)
{
std::vector<boost::filesystem::path> paths(boost::filesystem::directory_iterator{dataPath},boost::filesystem::directory_iterator{});
// sort files in accending order so playback is chronological
sort(paths.begin(), paths.end());
return paths;
}