点云地面滤波--布料模拟滤波（CSF）

xinxiangwangzhi_

已于 2022-09-12 21:29:48 修改

阅读量7.3k

点赞数 6

分类专栏：地面滤波点云文章标签：自动驾驶

于 2022-09-12 21:12:09 首次发布

本文链接：https://blog.csdn.net/xinxiangwangzhi_/article/details/126822913

版权

点云同时被 2 个专栏收录

65 篇文章 31 订阅

订阅专栏

地面滤波

9 篇文章 6 订阅

订阅专栏

文章目录

1基本原理
1.1什么是布料模拟
1.2修改布料模拟
1.3布料滤波
- 1.4后处理（可选）
2使用

1基本原理

总体思路：
如下图所示，原始点云被倒置过来，然后一块布从上方掉落到倒置的表面。通过分析布的节点与相应的LIDAR点之间的相互作用，可以确定布的最终形状，并将其用作将原始点分类为地面和非地面。
在这里插入图片描述

1.1什么是布料模拟

布料模拟是3D计算机图形学的术语。它也称为布料建模，用于在计算机程序中模拟衣服布料。在计算图形学中，可以将一块布认为由具有质量和互连的粒子组成的网格，称为质量弹簧模型（mass spring model）。下图显示了mass spring model结构。网格节点上的粒子没有大小，但分配了恒定质量。
粒子在三维空间中的位置决定了布的位置和形状。在此模型中，粒子之间的互连被建模为“虚拟弹簧”，它连接两个粒子并遵守胡克定律。为了充分描述布的特性，已经定义了三种类型的弹簧：剪切弹簧，牵引弹簧和屈曲弹簧。
在这里插入图片描述

粒子的位置和速度取决于作用在粒子上的力。根据牛顿的第二定律，位置和力之间的关系由等式（1）确定：
$\frac{\partial X(t)}{\partial t^2}=F_{e x t}(X, t)+F_{i n t}(X, t)\tag{1}$
其中x表示粒子在时间t上的位置；fext（x，t）代表外力，由重力和碰撞力组成，fint（x，t）代表位置x和时间t处的粒子的内力（由粒子互相连接产生）。由于内部和外力随时间t的变化而变化，因此通常通过数值方法（例如，Euler方法）求解方程（1）。

1.2修改布料模拟

为了使布料模拟适应地面滤波，需要做以下修改：
(1)将粒子的运动限制为沿垂直方向，因此可以通过比较粒子和地形的高度值来实现地面点云检测
(2)当粒子到达地面时，设置粒子为不可移动
(3)将作用在粒子上的力分解为2个力。首先考虑重力对粒子的作用，当在重力作用下，粒子到达地面时，此时将粒子变为不可移动状态，此时可以忽略碰撞力。其次根据内力改变粒子位置。
其过程如下图所示：

在这里插入图片描述

CSF中的主要步骤：（a）初始状态。一块布倒置放在的激光雷达点云上方；（b）在重力的影响下计算每个粒子的位移。因此，某些粒子可能会出现在地面下。（c）交叉检查。对于那些在地下的粒子，他们被移动在地面上并设置为无法移动的（蓝色粒子）；（d）考虑内部力量。根据粒子之间的内力移动可移动的粒子（红色粒子）。

1.3布料滤波

根据1.2对布料模拟的修改，布料滤波可以分为两个部分：
首先考虑外力：
在外力中，我们仅从重力中计算每个粒子的位移（忽略碰撞力），即，内力等于零的求解方程（1）。
$X(t+\Delta t)=2 X(t)-X(t-\Delta t)+\frac{G}{m} \Delta t^2\tag{2}$
其中m是粒子的质量（通常将m设置为1），而∆T是时间步长。这个方程式非常简单。给定时间步长和初始位置，可以直接计算当前位置，因为G是常数。
其次，考虑内力部分：
仅遍历每个spring（spring由每两个粒子组成），而非遍历每个粒子。对于每个spring，我们比较了构成当前spring的两个粒子之间的高度差。因此，二维（2-D）问题被抽象为一维（1-D）问题，如下图所示：
在这里插入图片描述

当使用内力移动粒子的运动方向时，两个具有不同高度值的粒子将尝试移动到同一水平面。
如果两个连接的粒子都是可移动的，我们将它们以相反的方向移动。如果其中一个是不可移动的，那么另一个将被移动。否则，如果这两个粒子具有相同的高度值，则它们都不会移动。因此，每个粒子的位移（向量）可以通过以下公式计算：
$\vec{d}=\frac{1}{2} b\left(\overrightarrow{p_i}-\overrightarrow{p_0}\right) \cdot \vec{n}\tag{3}$
$\vec{d}$ 表示粒子位移向量， $b$ :粒子可移动时等于1，否则等于0, $\vec{p_0},\vec{p_i}$ 分别表示粒子与其近邻粒子的当前位置（坐标）向量， $\vec{n}=(0,0,1)$ 表示垂直方向的单位向量。
内力作用下的移动过程是一个迭代过程，用rigidness表示迭代次数。每次移动距离为之前距离的二分之一，设两个粒子原始距离为VD，则rigidness=1，两个粒子之间的距离变为 $\frac{1}{2}VD$ ,rigidness=2，两个粒子之间的距离变为 $\frac{1}{4}VD$ ,rigidness=3，两个粒子之间的距离变为 $\frac{1}{8}VD$ ，其过程如下图：
在这里插入图片描述

CSF总体过程：
首先，我们将布粒子和LIDAR点投射到同一水平平面，然后在此2D平面中找到每个布粒子的最近的激光点云（称为对应点，CP）。定义了一个相交高度值（IHV）以记录CP的高度差值（投影之前）。IHV表示粒子能到达的最低位置，即如果粒子达到由该值定义的最低位置，则不能再向前移动。在每次迭代期间，我们将粒子的当前高度值（CHV）与IHV进行比较；如果CHV等于或低于IHV，则将粒子移回IHV的位置并使粒子无法移动。
在经过外力和内力计算之后获得了近似的真实地形，然后通过使用点云到点云的距离计算原始LIDAR点云和模拟粒子之间的距离。距离小于阈值HCC的LIDAR点被归类为地面点，而其余点为非地面点。
CSF具体过程

1使用某些第三方软件（例如CloudCompare）进行自动或手动异常点云（噪点）处理。
2.倒置原始的LIDAR点云。
3.初始化布料网格。根据用户定义的网格分辨率（GR）确定粒子数。布的初始位置通常设置在最高点上方。
4将所有LIDAR点和网格粒子投影到水平面，并在该平面中找到每个网格粒子的CP，然后记录IHV。
5对于每个网格粒子，如果该粒子可移动，则计算受重力影响的位置，并将该布粒子的高度与IHV进行比较。如果粒子的高度等于或小于IHV，则将该粒子放置在IHV的高度上，并将其设置为“无法移动”
6对于每个网格粒子，计算受内力影响的每个粒子的位移
7重复（5）-（6）。当所有粒子的最大高度变化（M_HV）足够小时，或者超过用户指定的最大迭代次数时，布料模拟过程将终止。
8计算云到网格粒子和激光元点云之间的云距离。
9分离地面点。对于每个LIDAR点云，如果与模拟粒子的距离小于HCC，则将该点归为地面点云，否则将其归为非地面点。

1.4后处理（可选）

对于陡峭的斜坡，该算法可能会产生相对较大的误差，因为模拟布料会出现在陡峭的斜坡上方，并且由于粒子之间的内部约束，不能使得粒子非常紧密的贴合陡峭地形，因此布料模拟不能适合陡峭地形，下图中说明了这种现象。
在这里插入图片描述

可以通过后处理方法来解决陡峭地形问题，该方法通过平滑陡峭地形的坡度实现。这种后处理方法在每个可移动粒子的四个相邻邻域中找到一个不可移动的粒子，并比较了CP的高度值。如果高度差位于阈值（HCP）之内，则可移动粒子将其移至地面并设置为无法移动。例如，对于图6中的点d，我们发现该点A是来自D的四个相邻邻居的不可动态粒子。然后，我们比较C和B之间的高度值（分别为D和A的CPs）。如果高度差异小于HCP，则该候选点D将移至C，并将其设置为不可移动的。我们重复此过程，直到正确处理所有可移动粒子（将其设置为无法移动或保持可移动）。
为了实施后处理，应遍历所有可移动粒子，如果我们行扫描布网，结果可能会受到此特定扫描方向的影响。因此，我们首先构建了一组强连接的组件（SCC），每个SCC包含一组连接的可移动粒子。在SCC中，它通常包含两种粒子，那些至少有一个不可移动的邻居标记为M1型的粒子，而其他粒子则标记为M2（见图7）。使用M1作为初始种子，我们为SCC执行优先遍历，如下图所示，从1到18的一一处理可移动粒子。
在这里插入图片描述