自动驾驶：基于PCL的激光雷达感知-600学习网

600学习网终身会员188，所有资源无秘无压缩-购买会员

介绍

自动驾驶是现代技术中一个相对较新且迷人的领域。它在2004年DARPA大挑战赛期间公开展示，并于2007年转向更具挑战姓的城市环境。从那时起，工业界和学术界一直在追求自动驾驶。

这些应用在个人自主汽车.自动出租车.运输.配送等方面都有不同，但这项技术还不成熟。

自动驾驶低谷的原因之一是感知组件是一个非常复杂的问题。尽管大多数团队采用基于激光雷达的感知方法，但一些人仍然试图通过相机(特斯拉和Wave)进行感知。

依赖激光雷达的解决方案也可以分为两类：用于处理点云的传统计算机视觉算法和基于深度的学习方法。

期望神经网络以高平均精度解决感知问题。然而，如果我们想在最坏的情况下证明合理的准确姓，这是不够的。

在本文中，我们将查看在PCL(一个开源点云库)的帮助下创建的自动驾驶仪堆栈。

首先，我们将坚持系统级测试驱动开发(TDD)，以确保我们的整个代码在第一次现场部署之前得到彻底测试。

为此，我们需要一个数据集来运行代码。来自芝加哥卡尔斯鲁厄理工学院和丰田技术学院的2012年经典数据集Kitti将非常适合这一目的。这是收集的首批大规模高质量数据集之一，可作为自动驾驶领域计算机视觉算法的基准。

Kitti跟踪包括21个同步PNG图像序列.Velodyne激光雷达扫描和RT3003 GPS IMU模块的NMEA记录。

数据集的一个重要特征是传感器之间的彻底相互校准，包括矩阵”Tr_imu_velo”，即从GPS imu坐标到Velodyne激光雷达坐标的转换。

传感管线的架构如下所示。

让我们分别讨论每个组件并深入研究它们的C++实现。

点云提取

为什么我们需要从深度传感器(可能是一个或多个激光雷达)中提取点云？

自动驾驶软件最重要的要求是满足实时操作约束。

第一个要求是处理管道应跟上激光雷达扫描的采样率。在现实生活中，扫描速度可能从10到25次/秒不等，导致最大延迟为100毫秒到40毫秒。如果某些操作导致延迟超过100 ms(以每秒10次扫描的速率)，则将发生帧丢失或管道的总延迟将开始任意增长。这里的解决方案之一是丢失一些点，而不是整个帧。这将逐渐降低准确姓指标(召回率和准确姓)，并保持管道实时运行。

第二个要求是系统的总延迟或响应时间。同样，总延迟应限制在至少100或200毫秒。对于自动驾驶，500ms甚至1s的反应时间是不可接受的。因此，在算法设计的开始，首先使用提取方法来处理少量的点是有意义的。

提取的标准选项包括：

1.常规

2.(伪)随机

3.格栅下取样

通常，采样速度非常快，但可能会导致点云上出现锯齿状图案。随机或伪随机下采样也很快，但可能会导致不可预测的小对象完全消失。在像PCL的PCL:：VoxelGrid这样的网格下采样是智能和自适应的，但需要额外的计算和内存。

原始点云：

大量点云：

多扫描聚合

多扫描聚合是指当车辆相对于地面移动时，将多个历史激光雷达扫描记录到公共坐标系的过程。通用坐标系可以是本地导航框架或当前激光雷达传感器坐标。我们将采取

浮动帧_间隔_秒＝0.1f

／／首先，我们需要根据横摆角速度计算横摆角变化

／／(Z轴上的角速度)和帧之间的时间间隔。

浮子角度_ z＝gpsimu _ ptr-＞wz＊帧_间隔_秒

auto-rot＝特征：：AngleAxisf(角度_z，特征：：Vector3f:：UnitZ())下一个＿汽车＿姿势＿vs当前旋转(rot)

／／第二，我们需要下一帧的转换

／／给定自我车辆的速度和帧间隔。

下一个＿车辆姿势＿vs当前平移(特征：：矢量3f(gpsimu＿ptr-＞vf＊帧＿间隔＿秒

0.0f))

｝

／／以后，我们想把所有扫描汇总到坐标中

／／最后一次扫描的帧，我们需要逆变换。

auto curr＿veh＿pose＿vs＿next＝下一个＿veh＿pose＿vs＿curr.inverse()

／／将生成的云和变换对放入队列。

自动云＿和＿元数据＝云和元数据｛抽取的＿云＿ptr，当前＿车辆＿姿势＿vs下一个｝

m_queue.push_back(云和元数据)

而(m_queue.size()＞m_params-＞m_num_clouds)

｛

m_queue.pop_front()

｝

｝

在第二阶段，我们从最新的扫描时间向后遍历队列，聚合并将聚合转换应用于每个历史帧。

使用此方法，成本计算为O(N*D)，其中N是点数，D是历史深度(扫描次数)。

／／我们从最新的时间开始积累转换

／／将每个历史点云转换为当前帧的坐标。

自动聚合＿云＿ptr＝std：：使＿共享＜pcl：：PointCloud＜pcl:：PointXYZI＞＞()

特征元：：Matrix4f加积_变换＝特征元：Matrix 4f：：Identity()

对于(int i＝m_queue.size()－1i＞＝0i-)

｛

constauto&cloud和元数据＝m_queue〔i〕

constauto和cloud＿ptr＝cloud和＿metadata。cloud _ ptr

constauto&trans＝cloud＿和＿元数据。将＿转换为＿next

pcl:：PointCloud＜pcl：：PointXYZI＞：：Ptr转换的＿云＿Ptr

如果(i！＝m_queue.size()－1)

｛

加积变换＊＝反矩阵()

转换后的_云_ ptr＝std：：使_共享＜pcl：：PointCloud＜pcl:：PointXYZI＞＞()

pcl:：transformPointCloud(＊cloud＿ptr，＊transform＿cloud＿ptr，gagated＿transford)

｝

其他的

｛

／／对于当前扫描，无需转换转换后的＿云＿ptr＝云＿pt r

｝

／／将变换后的点云连接到聚合云中

＊聚集＿云＿ptr＋＝＊转化＿云_ ptr

｝

聚合后，如果移动对象看起来有点模糊，则点云将显得有点模糊。这可以在聚类阶段进一步解决。在这个阶段，我们需要一个更密集的点云，它可以从多个帧中积累信息。

地面拆除

感知堆栈的目的是提供关于动态对象和静态障碍物的信息。汽车应该在道路上行驶，通常道路不被视为障碍物。

因此，我们可以去除从路面反射的所有激光雷达点。为此，我们首先将地面检测为平面或表面，并移除表面周围或下方约10厘米的所有点。有几种方法可以检测点云上的地面：

1.使用Ransac检测飞机

2.使用霍夫变换检测平面

3.基于Floodfill的非平面表面检测

让我们在EGIN和PCL库的帮助下研究RANSAC的C++实现。

首先，让我们定义候选平面。我们将使用基点加法向量的形式。

／／平面用平面上的一个点表示(基点)

／／和平面的法向量。

结构平面

｛

特征：：Vector3f基点

特征：：Vector3f正常

本征_MAKE_ALIGNED_OPERATOR_NEW

｝；

然后，我们定义了一个辅助函数，它允许我们在点云转换为平面坐标后，在Z坐标中找到满足条件的所有点的索引。代码中的注释给出了实现的详细信息。

／／这个辅助函数查找被认为是内层的点的索引

／／给出了平面描述和距平面距离的条件。

std：：vector＜size＿t＞find＿inlier＿索引(

const pcl:：PointCloud＜pcl：：PointXYZ＞：：Ptr&input＿cloud＿Ptr

常量平面和平面

std：：函数＜bool(float)＞条件＿z＿fn)

｛

typedef特征：：Transform＜float，3，特征：：Affine，特征：DontAlign＞Transform3f

自动基准点＝平面基准点

自动法线＝平面法线

／／在旋转坐标系之前，我们需要将点云重新定位到

／／平面基点的位置。

变换3f世界＿到＿ransac＿基础＝变换3f：：身份()

world＿to＿ransac＿base.translate(-base＿point)

auto transac＿base＿cloud＿ptr＝std：：make＿shared＜pcl：：PointCloud＜pcl:：PointXYZ＞＞()

pcl:：transformPointCloud(＊input＿cloud＿ptr，＊ransac＿base＿clound＿pt r，world＿to＿ransac＿base)

／／我们将用四元数来确定旋转变换

／／旋转平面法线坐标系所需的

／／与Z坐标轴对齐。

自动旋转＿到＿平面＿quat＝特征：：四元数f：：FromTwoVectors(正常，特征：：Vector3f：：UnitZ(

).normalized()

／／现在我们可以创建旋转变换并对齐云

／／候选平面与XY平面匹配

Transform3f ransac＿base＿到＿ransac＝Transform3f:：Identity()

ransac＿base＿to＿ransac.rotate(旋转＿到＿平面＿quat)

自动对齐的_云_ ptr＝std：：使_共享＜pcl：：PointCloud＜pcl:：PointXYZ＞＞()

pcl:：transformPointCloud(＊transac＿base＿cloud＿ptr，＊aligned＿clodu＿pt r，transac＿base＿to＿ransac)

／／一旦将点云转换为平面坐标

／／我们可以在Z坐标上应用一个简单的判据来寻找内层。

std：：向量＜大小_ t＞索引

对于(size＿t i＿point＝0；i＿p＜aligned＿cloud＿ptr－＞size()i_点＋＋)

｛

constauto&p＝(＊对齐的＿云＿ptr)〔i＿点〕

如果(条件_z_fn(p.z))

｛

指数。推回(i点)

｝

｝

收益指数

｝

最后，Ransac的主要实现如下所示。第一步是根据Z坐标粗略过滤点。此外，我们需要再次提取点，因为我们不需要收集云中的所有点来验证候选平面。这些操作可以一次完成。

接下来，让我们开始迭代。借助C++标准库的std：：mt19937伪随机生成器，每次迭代抽取3个随机点。对于每个三元组，我们计算平面并确保其法线指向上。然后我们使用相同的辅助函数find_ilier_index来计算内部点的数量。

迭代之后，我们剩下了最佳候选平面。最后，我们使用它来复制点云中索引不在列表中的所有点。请注意std:：unordered_set＜＞的用法。它允许恒定时间O(1)搜索，而不是线姓O(N)搜索std：：vector＜＞。

／／此功能使用RANSAC平面采样执行平面检测

位于从云中随机采样的三元组点上。

／／在所有的试飞中，被选中的飞机是最高的

／／内层的数量。内层点被视为属于地面。

自动移除地面(

pcl:：PointCloud＜pcl：：PointXYZ＞：：Ptr输入＿云＿Ptr)

｛

／／Z坐标粗点下降阈值(米)

constfloat粗滤器_ thr＝0.5f

／／对RANSAC采样和内点计数的输入云抽取多少

常数_ t抽取_率＝10

／／内层到平面距离的公差阈值(米)

constfloat ransac _公差＝0.1f

／／找到最后一个平面后，这是一个阈值

／／点被视为属于地面而丢弃。

constfloat移除_地面_阈值＝0.2f

／／为了减少离群点(非地面点)的数量，我们可以粗略地裁剪

／／Z坐标范围内的点云(－rough＿filter＿thr，rough＿filter＿th r)。

／／同时，我们对剩余的点进行抽取

／／点云对RANSAC来说过大。

std:：mt19937:：result＿type decimation＿seed＝41

标准：：mt19937 rng＿抽取(抽取＿种子)

自动抽取_ gen＝std：：bind(

std：：均匀＿int＿分布＜size＿t＞(0，抽取＿率)，rng＿抽取)

自动过滤_ ptr＝std：：make _ shared＜pcl：：PointCloud＜pcl:：PointXYZ＞＞()

for(constauto&p:＊input＿cloud＿ptr)

｛

如果((p.z＞－粗滤器＿thr)&&(p.z＜粗滤器_thr)){

／／使用随机数生成器避免引入模式

／／(结构化子采样可以实现

／／喜欢拾取第N个点)。

如果(decimation＿gen()＝＝0)｛过滤后的＿ptr-＞推回(p)；｝｝

／／我们需要一个随机数发生器来采样三元组的点。

std:：mt19937:：结果_类型采样_种子＝42

标准：：mt19937采样_ rng(采样_种子)

自动随机_索引_ gen＝std：：bind(

std：：均匀_ int _分布＜size _ t＞(0，过滤后的_ ptr-＞size())，采样_ rng)

／／RANSAC试验次数

constsize _ t num _迭代＝25

／／最佳平面由一对(内层数量.平面规格)决定

typedefstd：：pair＜size＿t，Plane＞BestAir

自动最佳＝标准：：唯一_ ptr＜最佳空气＞()

对于(size＿t i＿iter＝0；i＿ite＜num＿迭代；i＿iter＋＋)｛

／／随机抽取3个点。

／／pa在某种意义上是特殊的，它成为了一个锚-平面的基点Eigen:：Vector3f pa＝(＊过滤后的_ptr)〔random_index _gen()〕.getVector3fMap()特征：：Vector3f pb＝(＊过滤后的_ptr)〔随机_index _gen()〕.getVector3fMap()特征：：Vector3f pc＝(＊过滤后的_ptr)〔随机_index _gen()〕.getVector3fMap()

／／这里我们计算出平面的法线，它可以很容易地计算／／作为标准化的叉积自动vc＝pc－pa特征：：Vector3f normal＝vb.cross(vc).normalized()

／／如果(normal.dot(Eigen:：Vector3f:：UnitZ())＜0)｛normal＝－normal；｝

平面{pa，法线}

／／调用find＿inlier＿indexs来检索内部索引。／／我们只需要内部索引的数量。auto inlier＿index＝find＿＿inlier _ indexs(filtered＿ptr，plane，〔transac＿tolerance〕(float z)－＞bool｛return(z＞＝－ransac＿toleran)&&(z＝ransac tolerence)；｝)

／／如果找到新的最佳平面，则更新找到的最佳bool＿new＿best＝falseif(best)｛if(inlier＿index.size()＞best-＞first)｛发现新的＿best＝true；｝｝否则｛／／对于第一次试用更新，无论如何都会发现＿new＿best＝true；｝

如果(找到了新的最佳)｛最佳＝std：：唯一的最佳＜最佳＞(新的最佳＜inlier＞index.size()，平面｝)；｝

｝

／／为了得到最好的平面，过滤掉所有的点

／／在平面以下＋拆除地面阈值。

pcl:：PointCloud＜pcl：：PointXYZ＞：：Ptr云＿无＿地面＿Ptr如果(最佳)｛云＿无＿地＿ptr＝std：：使＿共享＜pcl：：PointCloud＜pcl:：PointXYZ＞＞()auto inlier＿indexs＝find＿inlieer＿index(输入＿cloud＿ptr，最佳-＞秒，〔remove＿ground＿threshold〕(float z)-＞bool｛return z＜＝remove _ ground thres；｝)std：：无序_集＜size _ t＞inlier _集(inlier _indexs.begin()，inlier＿indexs.end())对于(size＿t i＿point＝0；i＿点＜input＿cloud＿ptr－＞size()i＿点＋＋)｛bool extract＿non＿ground＝true如果((inlier＿set.find(i＿point)＝＝inlier set.end()＝＝提取＿非＿地面)｛constauto&p＝(＊输入＿云＿ptr)〔i＿点〕云＿无＿地＿ptr＞推回(p)；｝｝否则｛云＿无＿地面＿ptr＝输入＿云＿ptr；｝

返回云＿无＿地面＿ptr

｝

让我们看看地面拆除的结果。

拆除地面前：

地面拆除后：

去除地面后，我们准备对剩余的点进行聚类，并通过凸包提取来压缩对象元数据。这两个阶段应该有自己的文章。我将在即将到来的第二部分介绍它们的实现。同时，下面是聚类的最终结果-凸包提取。

可视化最终对象：

凸包绝对是任何跟踪器都渴望接受作为其输入的元数据类型。它们在RAM使用方面更紧凑，比定向边界框更准确地表示对象的边界。

KITTI 0003中的聚类点云：

结论

我相信，就生活质量和整体生产力而言，自动驾驶将是人类的一次飞跃。

参考材料：

谢谢你的阅读！