自动驾驶激光雷达物体检测技术
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
作者是william。
文章链接为https://zhuanlan.zhihu.com/p/128511171。
本文转自知乎平台。
已获得授权使用文章内容，请未经授权请勿二次转载。
Lidar Obstacle Detection
**Github:**https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection
Email: williamhyin@outlook.com
Lidar Sensors
该系统通过发射大量激光脉冲来收集数据。这些脉冲被目标物体反射回传感器后，系统能够通过计算信号返回所需时间来确定物体的距离。进一步分析反射光的强度特征，则可了解相关参数信息。所有光线均位于红外光谱范围内，并以多角度发射，通常覆盖360度范围。值得注意的是，尽管这类系统能生成高精度三维模型（3D），但它们的成本也相当高昂。
-
激光雷达以不同的角度发射数千束激光
-
激光被发射出来, 从障碍物上反射出来, 然后用接收器探测到
-
根据激光发射和接收的时间差, 计算出距离
-
接收到的激光强度值可用于评价被激光反射物体的材料性质
这是Velodyne 激光雷达传感器, 由左至右采用 HDL 64, HDL 32, VLP 16. 传感器越大, 分辨率越高. 下面是 HDL 64激光雷达的规格说明. 激光雷达共有64层, 每一层都以与 z 轴不同的角度发射出去, 因此倾斜度也不同. 每一层都覆盖了360度的视角, 角分辨率为0.08度. 激光雷达平均每秒扫描十次. 激光雷达可以探测到远达120米的汽车和树木, 还可以探测到远达50米的人行道.
VLP 64示意图, 显示激光雷达发射器、接收器和外壳
这个传感器有64层, 角分辨率为0.09度, 平均更新频率为10Hz, 每秒收集(64x (360 / 0.08) x10)=288万个数据
PCD data
我们系统地研究了激光雷达数据的存储方式。 激光雷达通过点云数据（缩略为PCD）的形式记录空间信息。 PCD文件由(x,y,z)笛卡尔坐标及其强度值构成，在单次扫描中捕捉环境特征。 VLP 64型激光雷达生成的数据包含约256 000个参数化的空间信息点。
点云数据与汽车采用相同的本地坐标系，在此系统中x轴朝向车辆前方延伸,y轴朝向车辆左侧延伸.此外,z轴则垂直于地面平面.
PCL 库在机器人技术领域具有广泛应用，在三维点云数据处理方面表现突出。网上提供了丰富的教程资源。
PCL 中包含多种辅助完成障碍物检测任务的功能模块。
本项目将利用 PCL 内置的分割、提取与聚类功能模块进行相关处理。
有关PCL 库的详细说明，请访问项目官网获取。
Steps For Obstacle Detection
Stream PCD
首先我们需要流式载入激光点云数据.
template<typenamePointT>std::vector<boost::filesystem::path>ProcessPointClouds<PointT>::streamPcd(std::stringdataPath){
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());
returnpaths;}// ####################################################ProcessPointClouds<pcl::PointXYZI>*pointProcessorI=newProcessPointClouds<pcl::PointXYZI>();std::vector<boost::filesystem::path>stream=pointProcessorI>streamPcd("../src/sensors/data/pcd/data_1");autostreamIterator=stream.begin();pcl::PointCloud<pcl::PointXYZI>::PtrinputCloudI;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;
真实PCD数据
Point Processing
生成处理点云数据所需的核心组件时，首先要创建一个命名为processPointClouds的对象。该对象整合了多种功能模块：包括滤波器、分割器、聚类算法以及用于加载和保存Point Cloud Data（PCD）的功能。为了适应不同类型的Point Cloud Data需求，在设计通用模板时，请考虑使用以下结构：template<typename PointT>。基于PCL库的标准定义，在真实场景中的Point Cloud Data通常采用XYZI格式表示坐标信息。构建该处理器时有两种选择：一种是基于堆栈（Stack）实现；另一种则是基于队列（Queue）进行操作。但推荐采用堆栈方式更为高效。
// 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表示体素网格的空间分辨率，在感兴趣区域内决定了最小距离与最大距离的关键点位置
我们首先利用VoxelGrid结构来减少空间中的点云密度；接着，在选定区域内确定相邻最近与最远的采样点；最后，在剩余的数据集中去除车顶区域的所有采样点。
c++ // To note, "using PtCdtr = typename pcl::PointCloud::Ptr;" template PtCdtr ProcessPointClouds::FilterCloud(PtCdtr cloud, float filterRes, Eigen::Vector4f minPoint,Eigen::Vector4f maxPoint) {
// Time segmentation process
auto startTime = std::chrono::steady_clock::now();// TODO:: 实现该函数用于执行体素网格点缩减和基于区域的过滤
// Construct the filtering mechanism: decimate the dataset processed with a minimum spacing of 200mm.
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();定义为elapsedTime的时间段为将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
template
std::pair<PtCdtr
ProcessPointclouds
// 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 is computed as a duration conversion of the time difference between endTime and startTime into milliseconds.
std::cout << "平面分割消耗的时间为" << elapsedTime.count() << "毫秒" << std::endl;
std::pair<PtCdtr<PointT>, PtCdtr<PointT>> segResult = SeparateClouds(
inliers, cloud);
return segResult;}
template
std::pair<PtCdtr
Process a collection of PointT point clouds to separate them using the SeparableClouds method (PCL::PointIndices indices).
// TODO: Generate two new point clouds, one containing obstacles and another containing a 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
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的平面计算公式：
然后我们需要计算点 `(x,y,z)`到平面的距离
以下为平面RANSAC的主体代码template
std::unordered_set
std::unordered_set
// For max iterations
while (maxIterations--) {
std::unordered_set<int> inliers;
while (inliers.size() < 3) {
inliers.insert(rand()%num_points);
}
// TO define plane, need 3 points
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;
// Calulate plane coefficient
float 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 plane
for (int ind = 0; ind < num_points; ind++) {if (inliers.count(ind) > 0) { //该方法表明：当存在已有的inlier时，则无需进一步操作。
continue;
}
PointT point = cloud->points[ind];
float x = point.x;
float y = point.y;
float z = point.z;计算得到距离值为 (|a⋅x + b⋅y + c⋅z + d|) 除以 √(a²+b²+c²)，此公式用于确定空间中其他点与给定平面之间的距离。
if (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
template
std::vector<PtCtr
Implement ClusteringAlgorithm
// Time clustering process
auto startTime = std::chrono::steady_clock::now();
std::vector<PtCdtr<PointT>> clusters;// TODO:: Implement the function for performing Euclidean clustering to cluster detected obstacles
// Build Kd-Tree Object
typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
// Input obstacle point cloud to create KD-tree
tree->setInputCloud(cloud);std::vectorpcl::PointIndices clusterIndices; // represents a point cloud indices structure
pcl::EuclideanClusterExtraction<PointT> ec; // clustering object
ec.setClusterTolerance(clusterTolerance);
ec.setMinClusterSize(minSize);
ec.setMaxClusterSize(maxSize);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud); // feed point cloud
ec.extract(clusterIndices);// get all clusters Indice
// For each cluster indice
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();decltype(endTime - startTime) elapsedTime = std::chrono::duration_caststd::chrono::milliseconds(两个时间点之间的差值)；
This code block required a duration of elapsedTime.count() milliseconds to perform the clustering and identified clusters.size() clusters in total.
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`的目的是将空间分成不同的区域, 从而减少最紧邻搜索的时间.
它是通过递归的方式使用新插入点更新节点. 其基本思想是遍历树, 直到它到达的节点为 NULL, 在这种情况下, 将创建一个新节点并替换 NULL 节点. 我们可以使用一个双指针来分配一个节点, 也就是说可以从根开始传递一个指向节点的指针, 然后当你想要替换一个节点时, 您可以解引用双指针并将其分配给新创建的节点.
我们可以通过代码了解在`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 insert
void insertHelper(Node **node, uint depth, std::vector<float> point, int id) {
// Tree is empty
if (*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 is expected to create a new node and be properly placed within the root
insertHelper(&root, 0, point, id);
}// #############################################################################################################
// Kd-Tree search
无返回值的搜索辅助函数searchHelper接受目标向量target、节点指针node、整数深度depth以及浮点距离阈值distanceTol，并通过ref ids引用结果向量ids
{
if (node != NULL)
{判断节点是否位于box内部。其中, point[0], 即x轴坐标; point[1], 即y轴坐标.
判断节点是否位于box内部。其中, point[0], 即x轴坐标; point[1], 即y轴坐标.
如果（节点位置x坐标大于等于（目标点x坐标减去公差值）并且小于等于（目标点x坐标加上公差值））并且（节点位置y坐标大于等于（目标点y坐标减去公差值）并且小于等于（目标点y坐标加上公差值））
{该程序中将通过计算两个点之间的距离来确定两点间的欧几里得距离。
if (distance <= distanceTol)
{
ids.push_back(node->id);
}
}
// check across boundary
if ((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 target
std::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 processed
add point to cluster
nearby points = tree(point)
Iterate through each nearby point
If point has not been processed
Proximity(cluster)EuclideanCluster():
list of clusters
Iterate through each point
If point has not been processed
Create cluster
Proximity(point, cluster)
cluster add clusters
return clusters真实代码：void clusterHelper(整数 indice, 常引用浮点矩阵 points, 输出引用整数向量 cluster, 输出引用布尔向量 processed, 指针 Kd树 tree, 浮点距离阈值 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);
}
}}
该函数返回一个int类型的二维向量，并接受三个参数：const float二维向量引用、KdTree指针和float距离阈值。
（注释内容保持不变）
// TODO: Fill out this function to return list of indices for each cluster
std::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
Box ProcessPointClouds
// 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;}
// Calling Bouding box function and render box
Box box = pointProcessor->BoundingBox(cluster);
renderBox(viewer,box,clusterId);
最终我们通过`renderbox`函数显示出一个个的Bounding Boxes.
对于Bounding Boxes的生成可以使用PCA主成分分析法生成更小的方框, 实现更高的预测结果精准性. 具体PCL实现可以查看这个链接：PCL-PCA.
现在我们完成了所有的激光点云处理的步骤, 让我们来看看最终成果吧!
###
推荐阅读：
[吐血整理|3D视觉系统化学习路线](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2e38cc88a72e180f0f6b0f7b906dd616e7d71fffb9205d529f1238e8ef0f0c5554c27dd7&idx=1&mid=2247484684&scene=21&sn=e812540aee03a4fc54e44d5555ccb843#wechat_redirect)
[那些精贵的3D视觉系统学习资源总结（附书籍、网址与视频教程）](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2e63cc88a775d02f284e1c35d71bd9eb33d72d9bfbbf13b14430c8ba9e6a131c616f471c&idx=2&mid=2247484759&scene=21&sn=ac6f2a0c8e94d4d1f204630d9dddaf4c#wechat_redirect)
[超全的3D视觉数据集汇总](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2e47cc88a7517c8c381cd6e55f04f8b5433053e8d896fc18cd09878fb58a85efabb1670b&idx=1&mid=2247484787&scene=21&sn=d781d80abb06a1b79e80a04248892b49#wechat_redirect)
[大盘点|6D姿态估计算法汇总(上)](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2eadcc88a7bb740a356ce1e223fa0c3010d009006ce75586b5cb1bb2275c06534634a330&idx=1&mid=2247484825&scene=21&sn=48fb69123bedbe990f2f4b51fcb650c4#wechat_redirect)
[大盘点|6D姿态估计算法汇总（下）](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2ef3cc88a7e59e50dbb9239bbb4839cb608a2b36fa8fd39c29f38f03c17bdd606e81bd3b&idx=1&mid=2247484871&scene=21&sn=ba4749a133b8a456b5430e5d1ea96ace#wechat_redirect)
[机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff236acc88aa7c9b6e60818f3e56542319810f8a8d617ba30cf9cf7d144bbec522f0a3d407&idx=1&mid=2247485534&scene=21&sn=f32a17a09ee1853e9ce852fea23d951c#wechat_redirect)
##
[汇总|3D点云目标检测算法](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2368cc88aa7eeb66aea64cbaea0229ef7addffda306ec0e814deb7af53a33613feb0f6cc&idx=2&mid=2247485532&scene=21&sn=5b473a2987485e5baf604004a6810380#wechat_redirect)
[汇总|3D人脸重建算法](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2ecbcc88a7dd1f4a0bd6c7dee9039fa12ad10be8661e6a6d1b1a1b7706e1dbcc3e6df351&idx=1&mid=2247484927&scene=21&sn=ab65d3e521562142023534f5bbfac9d5#wechat_redirect)
[那些年，我们一起刷过的计算机视觉比赛](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2d0bcc88a41d2be50e55d86f694b179facc524d9584d474c9131e873285d8d1c56a559b2&idx=1&mid=2247484991&scene=21&sn=e019b0abc6e483521c563c58a6b45f0d#wechat_redirect)
[总结|深度学习实现缺陷检测](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2db0cc88a4a6102c97f341a7e6c824231f3c6bad55b418d059176707eed73ecfd3f8fc7d&idx=1&mid=2247485060&scene=21&sn=a173dd18ef80489a6621ed43163411e3#wechat_redirect)
[深度学习在3-D环境重建中的应用](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2de6cc88a4f067cb3d4f56a60db1c0a1d60ed7bc30c63e9f51d04ba36b42c96c1ed05a88&idx=1&mid=2247485138&scene=21&sn=70226623c856cd95a501b403d2d2e498#wechat_redirect)
[汇总|医学图像分析领域论文](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2d82cc88a4949c07a6522c7bb688857fb008703022628c3829d4fc406a7a7b69be46c273&idx=2&mid=2247485110&scene=21&sn=e019bf86a9472b4687321be222e0fe0d#wechat_redirect)
[大盘点|OCR算法汇总](http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ%3D%3D&chksm=fbff2c1acc88a50c04e4803f2653409a2afd95dd8627abcd36d24fe0fd5c576d364c419d6a29&idx=1&mid=2247485230&scene=21&sn=2dda079dcb3e3886f48c342d1c773561#wechat_redirect)
**重磅！****3DCVer-****学术论文写作投稿****交流群****已成立**
****扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会（ICRA/IROS/ROBIO/CVPR/ICCV/ECCV等）、顶刊（IJCV/TPAMI/TIP等）、SCI、EI等写作与投稿事宜。****
同时也可申请加入我们的细分方向交流群，目前主要有**3D视觉** 、**CV &深度学习**、**SLAM** 、**三维重建** 、**点云后处理** 、**自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、**学术交流、** 求职交流**等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。**原创投稿** 也请联系。
▲长按加微信群或投稿
▲长按关注公众号
###
**3D视觉从入门到精通知识星球** ：针对3D视觉领域的**知识点汇总、入门进阶学习路线、最新paper分享、疑问解答** 四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：
###
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题