【SFND_Lidar_Obstacle_Detection】代码笔记

源代码链接：
https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection
激光雷达数据：
x，y，z，indensity（可用于评价物体的材料性质）
数据格式
PCD：点云数据(x, y, z, i)，点云数据的坐标系与汽车的本地坐标系相同，在这个坐标系中, x 轴指向汽车的前部, y 轴指向汽车的左侧. 此外, z轴指向车的上方.
PCL库
广泛应用于机器人技术领域, 用于处理点云数据, 网上有许多教程可供使用. PCL 中有许多内置的功能可以帮助检测障碍物. 本项目后面会使用 PCL内置的分割、提取和聚类函数. 你在这里可以找到PCL库的文档.
Steps For Obstacle Detection
Stream PCD
首先需要流式载入激光点云数据
（question1：如何在线使用？直接读取雷达数据，可以借鉴loam的写法？）

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 chronologicalsort(paths.begin(), paths.end());return paths;
}// ####################################################ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI >streamPcd("../src/sensors/data/pcd/data_1");
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;

Point Processing
处理点云数据的第一步就是要创建一个processPointClouds的对象, 这个对象中包含所有处理激光点云数据的模块, 如过滤, 分割, 聚类, 载入、存储PCD数据. 我们需要为不同的点云数据创建一个通用模板: template. 在真实点云数据中, 点云的类型是pcl::PointXYZI. 创建pointProcessor可以建立在Stack上也可以建立在Heap上, 但是建议在Heap上, 毕竟使用指针更加轻便.

// Build PointProcessor on the heap
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
// Build PointProcessor on the stack
ProcessPointClouds<pcl::PointXYZI> pointProcessorI;

Filtering

点云数据一般有很高的分辨率和相当远的可视距离. 我们希望代码处理管道能够尽可能快地处理点云, 因此需要对点云进行过滤. 这里有两种方法可以用来做到这一点.
1.Voxel Grid
体素网格过滤将创建一个立方体网格, 过滤点云的方法是每个体素立方体内只留下一个点, 因此立方体每一边的长度越大, 点云的分辨率就越低. 但是如果体素网格太大, 就会损失掉物体原本的特征. 具体实现可以查看PCL-voxel grid filtering的文档 .
2.Region of Interest
定义感兴趣区域, 并删除感兴趣区域外的任何点：感兴趣区域的选择两侧需要尽量覆盖车道的宽度, 而前后的区域要保证你可以及时检测到前后车辆的移动. 具体实现可以查看PCL-region of interest的文档. 在最终结果中, 我们使用pcl CropBox 查找自身车辆车顶的点云数据索引, 然后将这些索引提供给 pcl ExtractIndices 对象删除, 因为这些对于我们分析点云数据没有用处.
以下是Filtering的代码实现：
filterRes是体素网格的大小, minPoint/maxPoint为感兴趣区域的最近点和最远点.
我们首先执行VoxelGrid减少点云数量, 然后设置最近和最远点之间的感兴趣区域, 最后再从中删除车顶的点云.

// To note, "using PtCdtr = typename pcl::PointCloud::Ptr;"
template PtCdtr ProcessPointClouds::FilterCloud(PtCdtr cloud, float filterRes, Eigen::Vector4f minPoint,Eigen::Vector4f maxPoint) {// Time segmentation processauto 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 .2mpcl::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;    }

Segmentation
Segmentation的任务是将属于道路的点和属于场景的点分开. 点云分割的具体细节推荐查看PCL的官网文档: Segmentation和Extracting indices .
PCL RANSAC Segmentaion
针对本项目, 我创建了两个函数SegmentPlane和SeparateClouds, 分别用来寻找点云中在道路平面的inliers(内联点)和提取点云中的outliers(物体).
以下是主体代码:

// To note, "using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;"
template<typename PointT>
std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
ProcessPointClouds<PointT>::SegmentPlane(PtCdtr<PointT> 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<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 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<PtCdtr<PointT>, PtCdtr<PointT>> segResult = SeparateClouds(inliers, cloud);return segResult;
}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 planePtCdtr<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 objectpcl::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;
}

在SegmentPlane函数中我们接受点云、最大迭代次数和距离容忍度作为参数, 使用std::pair对象来储存物体和道路路面的点云. SeparateClouds 函数提取点云中的非inliers点, 即obstacles点. 粒子分割是一个迭代的过程, 更多的迭代有机会返回更好的结果, 但同时需要更长的时间. 过大的距离容忍度会导致不是一个物体的点云被当成同一个物体, 而过小的距离容忍度会导致一个较长的物体无法被当成同一个物体, 比如卡车.
Manual RANSAC Segmentation
目前粒子分割主要使用RANSAC算法. RANSAC全称Random Sample Consensus, 即随机样本一致性, 是一种检测数据中异常值的方法. RANSAC通过多次迭代, 返回最佳的模型. 每次迭代随机选取数据的一个子集, 并生成一个模型拟合这个子样本, 例如一条直线或一个平面. 然后具有最多inliers(内联点)或最低噪声的拟合模型被作为最佳模型. 如其中一种RANSAC 算法使用数据的最小可能子集作为拟合对象. 对于直线来说是两点, 对于平面来说是三点. 然后通过迭代每个剩余点并计算其到模型的距离来计算 inliers 的个数. 与模型在一定距离内的点被计算为inliers. 具有最高 inliers 数的迭代模型就是最佳模型. 这是我们在这个项目中的实现版本. 也就是说RANSAC算法通过不断迭代, 找到拟合最多inliers的模型, 而outliers被排除在外. RANSAC 的另一种方法对模型点的某个百分比进行采样, 例如20% 的总点, 然后将其拟合成一条直线. 然后计算该直线的误差, 以误差最小的迭代法为最佳模型. 这种方法的优点在于不需要考虑每次迭代每一点. 以下是使用RANSAC算法拟合一条直线的示意图, 真实激光数据下是对一个平面进行拟合, 从而分离物体和路面. 以下将单独对RANSAC平面算法进行实现.

RANSAC的平面计算公式：
以下为平面RANSAC的主体代码

template<typename PointT>
std::unordered_set<int> Ransac<PointT>::Ransac3d(PtCdtr<PointT> cloud) {std::unordered_set<int> inliersResult; // unordered_set element has been unique// For max iterationswhile (maxIterations--) {std::unordered_set<int> inliers;while (inliers.size() < 3) {inliers.insert(rand()%num_points);}// TO define plane, need 3 pointsfloat 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;// Calulate plane coefficientfloat 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);// Check distance from point to planefor (int ind = 0; ind < num_points; ind++) {if (inliers.count(ind) > 0) { // that means: if the inlier in already exist, we dont need do anymorecontinue;}PointT point = cloud->points[ind];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 planeif (dist < distanceTol) {inliers.insert(ind);}if (inliers.size() > inliersResult.size()) {inliersResult = inliers;}}}return inliersResult;
}

在实际中PCL已经内置了RANSAC函数, 而且比我写的计算速度更快, 所以直接用内置的就行了
Clustering
聚类是指把不同物体的点云分别组合聚集起来, 从而能让你跟踪汽车, 行人等多个目标. 其中一种对点云数据进行分组和聚类的方法称为欧氏聚类.
欧式聚类是指将距离紧密度高的点云聚合起来. 为了有效地进行最近邻搜索, 可以使用 KD-Tree 数据结构, 这种结构平均可以加快从 o (n)到 o (log (n))的查找时间. 这是因为Kd-Tree允许你更好地分割你的搜索空间. 通过将点分组到 KD-Tree 中的区域中, 您可以避免计算可能有数千个点的距离, 因为你知道它们不会被考虑在一个紧邻的区域中.
PCL Euclidean clustering
首先我们使用PCL内置的欧式聚类函数. 点云聚类的具体细节推荐查看PCL的官网文档Euclidean Cluster.

欧氏聚类对象 ec 具有距离容忍度. 在这个距离之内的任何点都将被组合在一起. 它还有用于表示集群的点数的 min 和 max 参数. 如果一个集群真的很小, 它可能只是噪音, min参数会限制使用这个集群. 而max参数允许我们更好地分解非常大的集群, 如果一个集群非常大, 可能只是许多其他集群重叠, 最大容差可以帮助我们更好地解决对象检测. 欧式聚类的最后一个参数是 Kd-Tree. Kd-Tree是使用输入点云创建和构建的, 这些输入云点是初始点云过滤分割后得到障碍物点云.

以下是PCL内置欧式聚类函数的代码:

// To note, "using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;"
template<typename PointT>
std::vector<PtCtr<PointT>>
ProcessPointClouds<PointT>::Clustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize) {// Time clustering processauto startTime = std::chrono::steady_clock::now();std::vector<PtCdtr<PointT>> clusters;// TODO:: Fill in the function to perform euclidean clustering to group detected obstacles// Build Kd-Tree Objecttypename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);// Input obstacle point cloud to create KD-treetree->setInputCloud(cloud);std::vector<pcl::PointIndices> clusterIndices; // this is point cloud indice typepcl::EuclideanClusterExtraction<PointT> ec; // clustering objectec.setClusterTolerance(clusterTolerance);//设置聚类点与点之间的距离阈值。ec.setMinClusterSize(minSize); //设置聚类点最少数目，排除噪音点的影响。ec.setMaxClusterSize(maxSize); //设置聚类点最大数目。只有最小与最大数目之间的聚类才能够返回。ec.setSearchMethod(tree);//通过kd-tree的方式搜索。ec.setInputCloud(cloud); // feed point cloudec.extract(clusterIndices);// get all clusters Indice // For each cluster indicefor (pcl::PointIndices getIndices: clusterIndices) {PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);// For each point indice in each clusterfor (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;
}

Manual Euclidean clustering
除此之外我们也可以直接使用KD-Tree进行欧氏聚类.
在此我们首先对KD-Tree的原理进行介绍. KD-Tree是一个数据结构, 由二进制树表示, 在不同维度之间对插入的点的数值进行交替比较, 通过分割区域来分割空间, 如在3D数据中, 需要交替在X,Y,Z不同轴上比较. 这样会使最近邻搜索可以快得多.
首先我们在试着二维空间上建立KD-Tree, 并讲述欧氏聚类的整个在二维空间上的实现过程, 最终我们将扩展到三维空间.
在KD-Tree中插入点(这是将点云输入到树中创建和构建KD-Tree的步骤)
假设我们需要在KD-Tree中插入４个点(-6.3, 8.4), (-6.2, 7), (-5.2, 7.1), (-5.7, 6.3)
首先我们得确定一个根点(-6.2, 7), 第0层为x轴, 需要插入的点为(-6.3, 8.4), (-5.2, 7.1), 因为-6.3<-6.2,(-6.3, 8.4)划分为左子节点, 而-5.2>-6.2, (-5.2, 7.1)划分为右子节点. (-5.7, 6.3)中x轴-5.7>-6.2,所以放在(-5.2, 7.1)节点下, 接下来第1层使用y轴, 6.3<7.1, 因此放在(-5.2, 7.1)的左子节点. 同理, 如果我们想插入第五个点(7.2, 6.1), 你可以交替对比x,y轴数值, (7.2>-6.2)->(6.1<7.1)->(7.2>-5.7), 第五点应插入到(-5.7, 6.3)的右子节点C.
下图是KD-Tree的结构.

KD-Tree的目的是将空间分成不同的区域, 从而减少最紧邻搜索的时间.Kd-Tree是从BST(Binary search tree)发展而来，是一种高维索引树形数据结构，常用于大规模高维数据密集的查找比对的使用场景中，主要是最近邻查找(Nearest Neighbor)以及近似最近邻查找(Approximate Nearest Neighbor)。在计算机视觉(CV)中主要是图像检索和识别中的高维特征向量的查找和比对。
在介绍Kd-Tree之前，首先介绍下它的父系结构——BST。二叉查找树，是一种具有如下性质的二叉树：
1.若它的左子树不为空，则它的左子树节点上的值皆小于它的根节点。
2.若它的右子树不为空，则它的右子树节点上的值皆大于它的根节点。
3.它的左右子树也分别是二叉查找树。

它是通过递归的方式使用新插入点更新节点. 其基本思想是遍历树, 直到它到达的节点为 NULL, 在这种情况下, 将创建一个新节点并替换 NULL 节点. 我们可以使用一个双指针来分配一个节点, 也就是说可以从根开始传递一个指向节点的指针, 然后当你想要替换一个节点时, 您可以解引用双指针并将其分配给新创建的节点.
1.在现有的数据中选定一个数据作为根节点的存储数值。（要求尽可能保证左右子树的集合的数量相等，优化查找速度）
2.将其它数据按照左小右大的规则往深层递归，直到叶节点，然后开辟新的叶节点，并存储当前值。
3.新的数据按照上一条进行存储。
以下就是一种二叉查找树结构示例：

上面聚类算法中，用到了数据结构kd-tree，采用kd-tree主要是为了加快数据的搜索查找速度。关于kd-tree的基础原理讲解见这里。这里主要通过c++代码来直观的理解kd-tree如何实现二维数据的插入、搜索。
如果是一维数据，我们可以用二叉查找树来进行存储，但是如果是多维的数据，用传统的二叉查找树就不能够满足我们的要求了，因此后来才发展出了满足多维数据的Kd-Tree数据结构。
Kd-tree的构造是在BST的基础上升级：
1.选定数据X1的Y1维数值a1做为根节点比对值，对所有的数值在Y1维进行一层BST排列。相当于根据Y1维数值a1对数据集进行分割。
2.选定数据X2的Y2维数值a2做为根节点比对值，对所有的数值在Y2维进行一层BST排列。也即将数据集在Y2维上又做了一层BST。
下图是一个简单的示例：
那么问题是：

1.如何决定每次根据哪个维度对子空间进行划分呢？
直观的来看，我们一般会选择轮流来。先根据第一维，然后是第二维，然后第三……，那么到底轮流来行不行呢，这就要回到最开始我们为什么要研究选择哪一维进行划分的问题。我们研究Kd-Tree是为了优化在一堆数据中高频查找的速度，用树的形式，也是为了尽快的缩小检索范围，所以这个“比对维”就很关键，通常来说，更为分散的维度，我们就更容易的将其分开，是以这里我们通过求方差，用方差最大的维度来进行划分——这也就是最大方差法（max invarince）。
2.如何选定根节点的比对数值呢？
选择何值未比对值，目的也是为了要加快检索速度。一般来说我们在构造一个二叉树的时候，当然是希望它是一棵尽量平衡的树，即左右子树中的结点个数相差不大。所以这里用当前维度的中值是比较合理的。

Kd-Tree和BST的区别：
BST的每个节点存储的是值，而Kd-Tree的根节点和中间节点存储的是对某个维度的划分信息，只有叶节点里才是存储的值。（此处存疑）

我们可以通过代码了解在KD-Tree中插入点的思路:

struct Node {std::vector<float> point;int id;Node *left;Node *right;Node(std::vector<float> arr, int setId): point(arr), id(setId), left(NULL), right(NULL) {}
};struct KdTree {Node *root;KdTree(): root(NULL) {}
// Kd-Tree insertvoid insertHelper(Node **node, uint depth, std::vector<float> point, int id) {// Tree is emptyif (*node == NULL) {*node = new Node(point, id);} else {// calculate current dim (1 means x axes, 2means y axes)uint cd = depth % 2;if (point[cd] < ((*node)->point[cd])) {insertHelper(&((*node)->left), depth + 1, point, id);} else {insertHelper(&((*node)->right), depth + 1, point, id);}}}void insert(std::vector<float> point, int id) {// TODO: Fill in this function to insert a new point into the tree// the function should create a new node and place correctly with in the rootinsertHelper(&root, 0, point, id);}
// #############################################################################################################// Kd-Tree searchvoid searchHelper(std::vector<float> target, Node *node, int depth, float distanceTol, std::vector<int> &ids){if (node != NULL){// Check whether the node inside box  or not, point[0] means x axes,　point[1]means y axesif ((node->point[0] >= (target[0] - distanceTol) && node->point[0] <= (target[0] + distanceTol)) &&(node->point[1] >= (target[1] - distanceTol) && node->point[1] <= (target[1] + distanceTol))){float distance = sqrt((node->point[0] - target[0]) * (node->point[0] - target[0]) +(node->point[1] - target[1]) * (node->point[1] - target[1]));if (distance <= distanceTol){ids.push_back(node->id);}}// check across boundaryif ((target[depth % 2] - distanceTol) < node->point[depth % 2]){searchHelper(target, node->left, depth + 1, distanceTol, ids);}if ((target[depth % 2] + distanceTol) > node->point[depth % 2]){searchHelper(target, node->right, depth + 1, distanceTol, ids);}}}// return a list of point ids in the tree that are within distance of targetstd::vector<int> search(std::vector<float> target, float distanceTol){std::vector<int> ids;searchHelper(target, root, 0, distanceTol, ids);return ids;}};

使用KD-Tree分割好的空间进行搜索

Kd-Tree分割区域并允许某些区域被完全排除, 从而加快了寻找近临点的进程
在上图中我们有8个点, 常规的方法是遍历计算每一个点到根点的距离, 在距离容忍度内的点为近邻点. 现在我们已经在Kd-Tree中插入了所有点, 我们建立一个根点周围２ x distanceTol长度的方框, 如果当前节点位于此框中, 则可以直接计算距离, 并查看是否应该将点 id 添加到紧邻点id 列表中, 然后通过查看方框是否跨越节点分割区域, 确定是否需要比较下一个节点. 递归地执行此操作, 其优点是如果框区域不在某个分割区域内, 则完全跳过该区域. 如上如图所示, 左上, 左下和右边分割区域均不在方框区域内, 直接跳过这些区域, 只需要计算方框内的绿点到根点的距离.
上面的代码块中第二部分为基于Kd-Tree的搜索代码.
一旦实现了用于搜索邻近点的Kd-Tree 方法, 就不难实现基于邻近度对单个聚类指标进行分组的欧氏聚类方法.
执行欧氏聚类需要迭代遍历云中的每个点, 并跟踪已经处理过的点. 对于每个点, 将其添加到一个集群(cluster)的点列表中, 然后使用前面的搜索函数获得该点附近所有点的列表. 对于距离很近但尚未处理的每个点, 将其添加到集群中, 并重复调用proximity的过程. 对于第一个集群, 递归停止后, 创建一个新的集群并移动点列表, 对于新的集群重复上面的过程. 一旦处理完所有的点, 就会找到一定数量的集群, 返回一个集群列表.

以下是欧氏聚类的伪代码：

Proximity(point,cluster):mark point as processedadd point to clusternearby points = tree(point)Iterate through each nearby pointIf point has not been processedProximity(cluster)EuclideanCluster():list of clusters Iterate through each pointIf point has not been processedCreate clusterProximity(point, cluster)cluster add clustersreturn clusters

真实代码：

void clusterHelper(int indice, const std::vector<std::vector<float>> &points, std::vector<int> &cluster,std::vector<bool> &processed, KdTree *tree, float distanceTol) {processed[indice] = true;cluster.push_back(indice);std::vector<int> nearest = tree->search(points[indice], distanceTol);for (int id:nearest) {if (!processed[id]) {clusterHelper(id, points, cluster, processed, tree, distanceTol);}}
}
//参数里面的 tree为kd-tree,里面存储了点云points.
std::vector<std::vector<int>>euclideanCluster(const std::vector<std::vector<float>> &points, KdTree *tree, float distanceTol) {// TODO: Fill out this function to return list of indices for each clusterstd::vector<std::vector<int>> clusters;std::vector<bool> processed(points.size(), false);int i = 0;while (i < points.size()) {if (processed[i]) {i++;continue;}std::vector<int> cluster;clusterHelper(i, points, cluster, processed, tree, distanceTol);clusters.push_back(cluster);i++;}return clusters;
}

以上是在二维空间下欧式聚类的实现, 在真实激光点云数据中我们需要将欧式聚类扩展到三维空间. 具体代码实现可以参考我的GITHUB中的cluster3d/kdtree3d文件. 自己手写欧氏聚类能够增强对概念的理解, 但其实真正项目上直接用PCL内置欧氏聚类函数就行.
Bounding Boxes
在完成点云聚类之后, 我们最后一步需要为点云集添加边界框. 其他物体如车辆, 行人的边界框的体积空间内是禁止进入的, 以免产生碰撞.

以下是生成边界框的代码实现:

template<typename PointT>
Box ProcessPointClouds<PointT>::BoundingBox(PtCdtr<PointT> cluster) {// Find bounding box for one of the clustersPointT 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;
}// Calling Bouding box function and render box
//输出BoundingBox，在environment.cpp/ simpleHighway下做如下设置。
Box box = pointProcessor->BoundingBox(cluster);
renderBox(viewer,box,clusterId);

对于Bounding Boxes的生成可以使用PCA主成分分析法生成更小的方框, 实现更高的预测结果精准性. 具体PCL实现可以查看这个链接：PCL-PCA.
SFND_Lidar_Obstacle_Detection代码笔记
render.h 文件中实现和构建了用于渲染环境使用的函数和结构体

// Functions and structs used to render the enviroment 用于环境渲染的函数和结构体
// such as cars and the highway#ifndef RENDER_H
#define RENDER_H
//#endif
//以上三行的定义 是为了避免头文件的重复使用#include <pcl/visualization/pcl_visualizer.h>
#include "box.h"
#include <iostream>
#include <vector>
#include <string>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;}
};struct Vect3
{double x, y, z;Vect3(double setX, double setY, double setZ): x(setX), y(setY), z(setZ){}//构造函数初始化列表,给x,y,z赋值Vect3 operator+(const Vect3& vec)  //函数重载,将结构体传递给函数{Vect3 result(x + vec.x, y + vec.y, z + vec.z);return result;}
};enum CameraAngle //枚举类型
{XY, TopDown, Side, FPS
};struct Car
{// 变量 position （位置）和 dimensions （尺寸大小）两个变量中的xyz的单位为米Vect3 position, dimensions;std::string name;Color color;//构造函数初始化列表Car(Vect3 setPosition, Vect3 setDimensions, Color setColor, std::string setName): position(setPosition), dimensions(setDimensions), color(setColor), name(setName){}void render(pcl::visualization::PCLVisualizer::Ptr& viewer){// render bottom of car  车辆底部的渲染//viewer->addCube 向视图中添加一个立方体模型 /*bool pcl::visualization::PCLVisualizer::addCube  ( float  x_min,  float  x_max,  float  y_min,  float  y_max,  float  z_min,  float  z_max,  double  r = 1.0,  double  g = 1.0,  double  b = 1.0,  const std::string &  id = "cube",  int  viewport = 0  ) *///render bottom of car渲染汽车底部viewer->addCube(position.x - dimensions.x / 2, position.x + dimensions.x / 2, position.y - dimensions.y / 2, position.y + dimensions.y / 2, position.z, position.z + dimensions.z * 2 / 3, color.r, color.g, color.b, name);// setShapeRenderingProperties 设置格子的属性viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name);// render top of car  车辆顶部的渲染viewer->addCube(position.x - dimensions.x / 4, position.x + dimensions.x / 4, position.y - dimensions.y / 2, position.y + dimensions.y / 2, position.z + dimensions.z * 2 / 3, position.z + dimensions.z, color.r, color.g, color.b, name + "Top");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name + "Top");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name + "Top");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name + "Top");}// collision helper function  初步猜想：检测是否车辆与周围点碰撞bool inbetween(double point, double center, double range){return (center - range <= point) && (center + range >= point);}bool checkCollision(Vect3 point){return (inbetween(point.x, position.x, dimensions.x / 2) && inbetween(point.y, position.y, dimensions.y / 2) && inbetween(point.z, position.z + dimensions.z / 3, dimensions.z / 3)) ||(inbetween(point.x, position.x, dimensions.x / 4) && inbetween(point.y, position.y, dimensions.y / 2) && inbetween(point.z, position.z + dimensions.z * 5 / 6, dimensions.z / 6));}
};
//函数实现功能待定
void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer);
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer);
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));
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color = Color(1, 0, 0), float opacity = 1);
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, BoxQ box, int id, Color color = Color(1, 0, 0), float opacity = 1);#endif

render.cpp

/* \author Aaron Brown */
// Functions and structs used to render the enviroment
// such as cars and the highway#include "render.h"void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
{// units in metersdouble roadLength = 50.0;double roadWidth = 12.0;double roadHeight = 0.2;viewer->addCube(-roadLength / 2, roadLength / 2, -roadWidth / 2, roadWidth / 2, -roadHeight, 0, .2, .2, .2, "highwayPavement");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, "highwayPavement");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, .2, .2, .2, "highwayPavement");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, "highwayPavement");viewer->addLine(pcl::PointXYZ(-roadLength / 2, -roadWidth / 6, 0.01), pcl::PointXYZ(roadLength / 2, -roadWidth / 6, 0.01), 1, 1, 0, "line1");viewer->addLine(pcl::PointXYZ(-roadLength / 2, roadWidth / 6, 0.01), pcl::PointXYZ(roadLength / 2, roadWidth / 6, 0.01), 1, 1, 0, "line2");
}int countRays = 0;
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{for (pcl::PointXYZ point : cloud->points){viewer->addLine(pcl::PointXYZ(origin.x, origin.y, origin.z), point, 1, 0, 0, "ray" + std::to_string(countRays));countRays++;}
}void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer)
{while (countRays){countRays--;viewer->removeShape("ray" + std::to_string(countRays));}
}//选取需要渲染的点云的种类 障碍物 地面 全部点云
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);
}// Draw wire frame box with filled transparent color
// renderBox(viewer, box, clusterId);  对每个点云画框
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color, float opacity)
{if (opacity > 1.0)opacity = 1.0;if (opacity < 0.0)opacity = 0.0;std::string cube = "box" + std::to_string(id);//viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cube);viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, color.r, color.g, color.b, cube);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cube);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cube);std::string cubeFill = "boxFill" + std::to_string(id);//viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cubeFill);viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, color.r, color.g, color.b, cubeFill);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cubeFill);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity*0.3, cubeFill);
}

box.h

#ifndef BOX_H
#define BOX_H#include <Eigen/Geometry>
struct BoxQ
{Eigen::Vector3f bboxTransform;  // Vector3f 单精度的xyz坐标 与之对应的Vector3D双精度，更加精确，但运行速度也会慢//浮点型的四元数  Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)/*四元数都是由实部w 加上三个虚部 x、y、z 组成四元数一般可表示为a + bx+ cy + dz，其中a、b、c 、d是实数*/Eigen::Quaternionf bboxQuaternion;float cube_length;float cube_width;float cube_height;
};
struct Box
{float x_min;float y_min;float z_min;float x_max;float y_max;float z_max;
};#endif

kdtree3d.h

#ifndef PLAYBACK_KDTREE3D_H
#define PLAYBACK_KDTREE3D_H#include <pcl/impl/point_types.hpp>
#include <vector>struct Node {pcl::PointXYZI point;int id;Node *left;Node *right;Node(pcl::PointXYZI arr, int setId) : point(arr), id(setId), left(NULL), right(NULL) {}
};struct KdTree {Node *root;KdTree() : root(NULL) {}void insertHelper(Node **node, int depth, pcl::PointXYZI point, int id) {// Tree is emptyif (*node == NULL) {*node = new Node{ point, id };}else {// calculate current dinint cd = depth % 3;if (cd == 0) {if (point.x < (*node)->point.x) {insertHelper(&((*node)->left), depth + 1, point, id);}else {insertHelper(&((*node)->right), depth + 1, point, id);}}else if (cd == 1) {if (point.y < (*node)->point.y) {insertHelper(&((*node)->left), depth + 1, point, id);}else {insertHelper(&((*node)->right), depth + 1, point, id);}}else {if (point.z < (*node)->point.z) {insertHelper(&((*node)->left), depth + 1, point, id);}else {insertHelper(&((*node)->right), depth + 1, point, id);}}}}void insert(pcl::PointXYZI point, int id) {// the function should create a new node and place correctly with in the rootinsertHelper(&root, 0, point, id);}void searchHelper(pcl::PointXYZI target, Node *node, int depth, float distanceTol, std::vector<int> &ids) {if (node != NULL) {float delta_x = node->point.x - target.x;float delta_y = node->point.y - target.y;float delta_z = node->point.z - target.z;if ((delta_x >= -distanceTol && delta_x <= distanceTol) &&(delta_y >= -distanceTol && delta_y <= distanceTol) &&(delta_z >= -distanceTol && delta_z <= distanceTol)) {float distance = sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);if (distance <= distanceTol) {ids.push_back(node->id);}}// check across boundaryif (depth % 3 == 0) {if (delta_x > -distanceTol) {searchHelper(target, node->left, depth + 1, distanceTol, ids);}if (delta_x < distanceTol) {searchHelper(target, node->right, depth + 1, distanceTol, ids);}}else if (depth % 3 == 1) {if (delta_y > -distanceTol) {searchHelper(target, node->left, depth + 1, distanceTol, ids);}if (delta_y < distanceTol) {searchHelper(target, node->right, depth + 1, distanceTol, ids);}}else {if (delta_z > -distanceTol) {searchHelper(target, node->left, depth + 1, distanceTol, ids);}if (delta_z < distanceTol) {searchHelper(target, node->right, depth + 1, distanceTol, ids);}}}}// return a list of point ids in the tree that are within distance of targetstd::vector<int> search(pcl::PointXYZI target, float distanceTol){std::vector<int> ids;searchHelper(target, root, 0, distanceTol, ids);return ids;}};#endif //PLAYBACK_KDTREE3D_H

environment.cpp

#include "lidar.h"
#include "render.h"
#include "processPointClouds.h"
// using templates for processPointClouds so also include .cpp to help linker
#include "processPointClouds.cpp"
using namespace lidar_obstacle_detection;std::vector<Car> initHighway(bool renderScene, pcl::visualization::PCLVisualizer::Ptr &viewer) {Car egoCar(Vect3(0, 0, 0), Vect3(4, 2, 2), Color(0, 1, 0), "egoCar");Car car1(Vect3(15, 0, 0), Vect3(4, 2, 2), Color(0, 0, 1), "car1");Car car2(Vect3(8, -4, 0), Vect3(4, 2, 2), Color(0, 0, 1), "car2");Car car3(Vect3(-12, 4, 0), Vect3(4, 2, 2), Color(0, 0, 1), "car3");std::vector<Car> cars;cars.push_back(egoCar);cars.push_back(car1);cars.push_back(car2);cars.push_back(car3);if (renderScene) {renderHighway(viewer);egoCar.render(viewer);car1.render(viewer);car2.render(viewer);car3.render(viewer);}return cars;
}// Test load pcd
//void cityBlock(pcl::visualization::PCLVisualizer::Ptr& viewer){//    ProcessPointClouds<pcl::PointXYZI>pointProcessor;
//    pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloud = pointProcessor.loadPcd("../src/sensors/data/pcd/data_1/0000000000.pcd");
//    renderPointCloud(viewer,inputCloud,"cloud");
//}// Initialize the simple Highway// Test read Lidar data
void cityBlock(pcl::visualization::PCLVisualizer::Ptr &viewer, ProcessPointClouds<pcl::PointXYZI> *pointProcessorI,const pcl::PointCloud<pcl::PointXYZI>::Ptr &inputCloud)
{// 1、滤波  滤波后点云存入filteredCloud      okfloat filterRes = 0.4;Eigen::Vector4f minpoint(-10, -6.5, -2, 1);Eigen::Vector4f maxpoint(30, 6.5, 1, 1);pcl::PointCloud<pcl::PointXYZI>::Ptr filteredCloud = pointProcessorI->FilterCloud(inputCloud, filterRes, minpoint,maxpoint);// 2、将滤波后的点云分割成地面和障碍物 结果存入segmentCloud中       okint maxIterations = 40;float distanceThreshold = 0.3;//2.1 返回地面点云 和 障碍物点云 //segmentCloud.first, "obstCloud" //segmentCloud.second, "planeCloud"std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI->RansacSegmentPlane(filteredCloud, maxIterations, distanceThreshold);//2.2 选取待渲染的点云的种类分别为 障碍物、地面、全部点云//renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));//renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));renderPointCloud(viewer,inputCloud,"inputCloud");// 3、对去除地面后的障碍物点云进行聚类  segmentCloud.first, "obstCloud" float clusterTolerance = 0.5;int minsize = 10;int maxsize = 140;//std::vector<PtCdtr<PointT>> cloudClusters 返回了11类 每类中又包含了属于该类的点云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);  // cloud->points.size()renderPointCloud(viewer, cluster, "obstCLoud" + std::to_string(clusterId),colors[clusterId % colors.size()]);// Fourth: Find bounding boxes for each obstacle clusterBox box = pointProcessorI->BoundingBox(cluster);  //找到每块点云的xyz轴上的最值renderBox(viewer, box, clusterId); //根据最值画框++clusterId;}
}void simpleHighway(pcl::visualization::PCLVisualizer::Ptr &viewer) {// ----------------------------------------------------// -----Open 3D viewer and display simple highway -----// ----------------------------------------------------// RENDER OPTIONSbool renderScene = false;bool render_obst = false;bool render_plane = false;bool render_cluster = true;bool render_box = true;std::vector<Car> cars = initHighway(renderScene, viewer);// TODO:: Create lidar sensorLidar *lidar = new Lidar(cars, 0);pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud = lidar->scan();//    renderRays(viewer,lidar->position,inputCloud);renderPointCloud(viewer, inputCloud, "inputCloud");// TODO:: Create point processorProcessPointClouds<pcl::PointXYZ> pointProcessor;std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentCloud = pointProcessor.SegmentPlane(inputCloud, 100, 0.2);if (render_obst) {renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0, 0));}if (render_plane) {renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1, 0));}std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudClusters = pointProcessor.Clustering(segmentCloud.first, 1.0,3, 30);int clusterId = 0;std::vector<Color> colors = { Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1) };for (pcl::PointCloud<pcl::PointXYZ>::Ptr cluster : cloudClusters){if (render_cluster) {std::cout << "cluster size:  ";pointProcessor.numPoints(cluster);renderPointCloud(viewer, cluster, "obstCLoud" + std::to_string(clusterId),colors[clusterId % colors.size()]);++clusterId;}if (render_box) {Box box = pointProcessor.BoundingBox(cluster);renderBox(viewer, box, clusterId);}++clusterId;}renderPointCloud(viewer, segmentCloud.second, "planeCloud");
}//setAngle: SWITCH CAMERA ANGLE {XY, TopDown, Side, FPS}
void initCamera(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr &viewer) {viewer->setBackgroundColor(0, 0, 0);// set camera position and angleviewer->initCameraParameters();// distance away in metersint distance = 16;switch (setAngle) {case XY:viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0);break;case TopDown:viewer->setCameraPosition(0, 0, distance, 1, 0, 1);break;case Side:viewer->setCameraPosition(0, -distance, 0, 0, 0, 1);break;case FPS:viewer->setCameraPosition(-10, 0, 0, 0, 0, 1);}if (setAngle != FPS)viewer->addCoordinateSystem(1.0);
}// char* argv[] means array of char pointers, whereas char** argv means pointer to a char pointer.
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 类ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();std::vector<boost::filesystem::path> stream = pointProcessorI->streamPcd("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\data_2");auto streamIterator = stream.begin();//从文件中的第一个点云文件开始 pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI; //创建点云对象while (!viewer->wasStopped()) {// Clear viewerviewer->removeAllPointClouds();viewer->removeAllShapes();//inputCloudI 为每一帧点云数据inputCloudI = pointProcessorI->loadPcd((*streamIterator).string()); //对每一个点云进行处理cityBlock(viewer, pointProcessorI, inputCloudI);//主要处理程序 参数为视窗、点云处理类、点云streamIterator++;if (streamIterator == stream.end()) {streamIterator = stream.begin();}viewer->spinOnce();}
}

重点部分
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 pointertemplate<typename PointT>using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;template<typename PointT>class ProcessPointClouds {public://constructorProcessPointClouds();//deconstructor~ProcessPointClouds();void numPoints(PtCdtr<PointT> cloud);//1、点云滤波       okPtCdtr<PointT> FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint);//——————————————————————————对点云库中的模型分割，将点云保存为两个文件  主程序中依然好像没有用到      okstd::pair<PtCdtr<PointT>, PtCdtr<PointT>> SeparateClouds(pcl::PointIndices::Ptr inliers, PtCdtr<PointT> cloud);//2、地面与障碍物的分割 两块点云分别保存在in_plane（平面外的点）, out_plane（平面内的点）中  okstd::pair< PtCdtr<PointT>, PtCdtr<PointT> > RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);// ——————————————————————————此处使用了pcl点云库中的模型分割 程序中貌似没有用到           okstd::pair<PtCdtr<PointT>, PtCdtr<PointT>> SegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol);//3、重写欧式聚类算法聚类障碍物点云        okstd::vector<PtCdtr<PointT>> EuclideanClustering(PtCdtr<PointT> cloud, float clusterTolerance, int minSize, int maxSize);//——————————————————————————此处使用了点云库中的欧氏距离分割模型  程序中没有用到         okstd::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 timeauto 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 implementationstd::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 processauto 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 objectpcl::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 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<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 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<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 processauto startTime = std::chrono::steady_clock::now();std::vector<PtCdtr<PointT>> clusters;  //保存分割后的所有类 每一类为一个点云// 欧式聚类对检测到的障碍物进行分组typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>); //对cloud点云创建kdtreetree->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 cloudec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices  // 将得到的所有类的索引分别在点云中找到，即每一个索引形成了一个类for (pcl::PointIndices getIndices : clusterIndices) {PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);// For each point indice in each clusterfor (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 processauto startTime = std::chrono::steady_clock::now();// 创建聚类对象  实例化ClusterPts类 创建对象clusterPointsClusterPts<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 clustersPointT 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 filePCL_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 chronologicalsort(paths.begin(), paths.end());return paths;
}

cluster3d.h

#ifndef PLAYBACK_CLUSTER3D_H
#define PLAYBACK_CLUSTER3D_H
#include <pcl/common/common.h>
#include <chrono>
#include <string>
#include "kdtree3d.h"namespace lidar_obstacle_detection {// shorthand for point cloud pointertemplate<typename PointT>using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;template<typename PointT>class ClusterPts {private:int num_points;float distanceTol;int minClusterSize;int maxClusterSize;std::vector<bool> processed;std::vector<PtCdtr<PointT>> clusters;public://构造函数ClusterPts(int nPts, float cTol, int minSize, int maxSize) : num_points(nPts), distanceTol(cTol),minClusterSize(minSize), maxClusterSize(maxSize) {processed.assign(num_points, false);}~ClusterPts();void clusterHelper(int ind, PtCdtr<PointT> cloud, std::vector<int> &cluster, KdTree *tree);std::vector<PtCdtr<PointT>> EuclidCluster(PtCdtr<PointT> cloud);};
}
#endif //PLAYBACK_CLUSTER3D_H

cluster3d.cpp

#include "cluster3d.h"
using namespace lidar_obstacle_detection;
template<typename PointT>
ClusterPts<PointT>::~ClusterPts() {}
//根据最近邻搜索的阈值，找到了类数，每一类包含了属于该类的点           ok
template<typename PointT>
void ClusterPts<PointT>::clusterHelper(int ind, PtCdtr<PointT> cloud, std::vector<int> &cluster, KdTree *tree)
{//std::vector<bool> processed;processed[ind] = true;cluster.push_back(ind);//ec.setClusterTolerance(clusterTolerance);  设置近邻搜索半径std::vector<int> nearest_point = tree->search(cloud->points[ind], distanceTol); for (int nearest_id : nearest_point) {if (!processed[nearest_id]) {clusterHelper(nearest_id, cloud, cluster, tree);}}
}//重写欧式聚类算法     ok
template<typename PointT>
std::vector<PtCdtr<PointT>> ClusterPts<PointT>::EuclidCluster(PtCdtr<PointT> cloud)
{KdTree *tree = new KdTree;  //创建重写kdtree的对象//对cloud创建kdtreefor (int ind = 0; ind < num_points; ind++) {tree->insert(cloud->points[ind], ind);}//std::vector<bool> processed;for (int ind = 0; ind < num_points; ind++) {if (processed[ind]) {ind++;continue;}std::vector<int> cluster_ind; //每一类包含点的索引PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);clusterHelper(ind, cloud, cluster_ind, tree);int cluster_size = cluster_ind.size();  //总类数if (cluster_size >= minClusterSize && cluster_size <= maxClusterSize) {for (int i = 0; i < cluster_size; i++) {cloudCluster->points.push_back(cloud->points[cluster_ind[i]]);}cloudCluster->width = cloudCluster->points.size();cloudCluster->height = 1;clusters.push_back(cloudCluster);}}return clusters;
}

https://zhuanlan.zhihu.com/p/128511171
https://blog.csdn.net/qinlele1994

【SFND_Lidar_Obstacle_Detection】代码笔记相关推荐

CSDN技术主题月----“深度学习”代码笔记专栏
from: CSDN技术主题月----"深度学习"代码笔记专栏 2016-09-13 nigelyq 技术专题 Hi,各位用户 CSDN技术主题月代码笔记专栏会每月在CODE博客为 ...
看完师兄的代码笔记，我失眠了
祝大家中秋节快乐! 最近很多公司的秋季招聘都已经启动了. 想必大家(尤其是经历过求职面试的)都知道,数据结构和算法在求职笔试/面试中的重要性. 纵观如今的互联网公司面试,清一色地都在重点考查这块,不开 ...
LSTM TF核心实现代码笔记
LSTM TF核心实现代码笔记 1. LSTM TF里的核心代码实现 2. 代码详细讲解 1. LSTM TF里的核心代码实现 LSTM网络的核心实现是在这个包里tensorflow/python.k ...
2018年最新Spring Boot视频教程附代码笔记资料（50G）
1. Spring Boot 项目实战 ----- 技术栈博客企业前后端链接:https://pan.baidu.com/s/1hueViq4 密码:4ma8 2.Spring Boot 项目实 ...
Python Text Processing with NLTK 2.0 Cookbook代码笔记
如下是<Python Text Processing with NLTK 2.0 Cookbook>一书部分章节的代码笔记. Tokenizing text into sentences ...
Transformer课程第8课NER案例代码笔记-IOB标记
Transformer课程第8课NER案例代码笔记-IOB标记 NER Tags and IOB Format 训练集和测试集都是包含餐厅相关文本(主要是评论和查询)的单个文件,其中每个单词都有一个 ...
2018尚硅谷SpringBoot视频教程附代码+笔记+课件(内含Docker）
尚硅谷SpringBoot视频教程(内含Docker)附代码+笔记+课件下载地址:百度网盘
吴恩达机器学习MATLAB代码笔记（1）梯度下降
吴恩达机器学习MATLAB代码笔记(1)梯度下降单变量线性回归 1.标记数据点(Plotting the Date) fprintf('Plotting Data') data = load('D: ...
Transformer课程第8课NER案例代码笔记-部署简介
Transformer课程第8课NER案例代码笔记 BERT微调器 NER是信息提取的子任务,旨在将非结构化文本中提到的命名实体定位并分类为预定义类别,如人名.组织.位置.医疗代码.时间表达式.数量 ...

【SFND_Lidar_Obstacle_Detection】代码笔记

Filtering

【SFND_Lidar_Obstacle_Detection】代码笔记相关推荐

最新文章

热门文章