CSF(Cloth Simulation Filter)点云地面点滤波

0.引言

• code

• 基本原理：原始点云被倒置过来，然后⼀块布从上⽅掉落到倒置的表⾯。通过分析布的节点与相应的LIDAR点之间的相互作⽤，可以确定布的最终形状，并将其⽤作将原始点分类为地⾯和⾮地⾯。

1.布料模拟

在计算图形学中，可以将⼀块布认为由具有质量和互连的粒⼦组成的⽹格，称为质量弹簧模型（mass spring model）。下图显⽰了mass spring model结构。⽹格节点上的粒⼦没有⼤⼩，但分配了恒定质量。

粒⼦在三维空间中的位置决定了布的位置和形状。在此模型中，粒⼦之间的互连被建模为“虚拟弹簧”，它连接两个粒⼦并遵守胡克定律。为了充分描述布的特性，已经定义了三种类型的弹簧：牵引弹簧、剪切弹簧、屈曲弹簧。

粒⼦的位置和速度取决于作⽤在粒⼦上的⼒。根据⽜顿的第⼆定律，位置和⼒之间的关系由等式

（1）确定：

$$m\frac{\partial X(t)}{\partial t^2}=F_{\text{ext }}(X,t)+F_{\text{int }}(X,t)$$

其中 x 表⽰粒⼦在时间 t 上的位置；$f_{ext}(x，t)$ 代表外⼒，由重⼒和碰撞⼒组成，$F_{int}(x，t)$代表位置x和时间t处的粒⼦的内⼒（由粒⼦互相连接产⽣）。由于内部和外⼒随时间t的变化⽽变化，因此通常通过数值⽅法（例如，Euler⽅法）求解⽅程.

2.修改布料模拟

为了使布料模拟适应地⾯滤波，需要做以下修改：

• (1)将粒⼦的运动限制为沿垂直⽅向，因此可以通过⽐较粒⼦和地形的⾼度值来实现地⾯点云检测
• (2)当粒⼦到达地⾯时，设置粒⼦为不可移动，如果不可移动的粒⼦与地形的⾼度相当，就认为是地⾯
• (3)将作⽤在粒⼦上的⼒分解为2个⼒。⾸先考虑重⼒对粒⼦的作⽤，当在重⼒作⽤下，粒⼦到达地⾯时，此时将粒⼦变为不可移动状态，此时可以忽略碰撞⼒。其次根据内⼒改变粒⼦位置。

其过程如下图所⽰：

CSF中的主要步骤：

• （a）初始状态。⼀块布倒置放在的激光雷达点云上⽅；
• （b）在重⼒的影响下计算每个粒⼦的位移。因此，某些粒⼦可能会出现在地⾯下。
• （c）交叉检查。对于那些在地下的粒⼦，他们被移动在地⾯上并设置为⽆法移动的（蓝⾊粒⼦）；
• （d）考虑内部⼒量。根据粒⼦之间的内⼒移动可移动的粒⼦（红⾊粒⼦）。

3.布料滤波

根据 2.对布料模拟的修改，布料滤波可以分为两个部分：

• ⾸先考虑外⼒（考虑重⼒忽略碰撞⼒）：在外⼒中，我们仅从重⼒中计算每个粒⼦的位移，即内⼒等于零的求解⽅程:

$$X(t+\Delta t)=2X(t)-X(t-\Delta t)+\frac{G}{m}\Delta t^2$$

• 其次，考虑内⼒部分：仅遍历每个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}$$

$\vec{d}$ 表示粒子位移向量， $b$ :粒子可移动时等于 1 ，否则等于 $0,\overrightarrow{p_0},\overrightarrow{p_i}$ 分别表示粒子与其近邻粒子的当前位置 (坐标) 向量， $\vec{n}=(0,0,1)$ 表示垂直方向的单位向量。

内力作用下的移动过程是一个迭代过程，用rigidness表示迭代次数。每次移动距离为之前距离的二分之一，设两个粒子原始距离为 $\mathrm{VD}$ ，则rigidness=1，两个粒子之间的距离变为 $\frac{1}{2}VD$, rigidness=2，两个粒子之间的距离变为 $\frac{1}{4}VD$, rigidness $=3$ ，两个粒子之间的距离变为 $\frac{1}{8}VD$ ，其过程如下图:

4.CSF总体过程

⾸先，我们将布粒⼦和LIDAR点投射到同⼀⽔平平⾯，然后在此2D平⾯中找到每个布粒⼦的最近的激光点云（称为对应点，CP）。定义了⼀个相交⾼度值（IHV）以记录CP的⾼度差值（投影之前）。IHV表⽰粒⼦能到达的最低位置，即如果粒⼦达到由该值定义的最低位置，则不能再向前移动。

在每次迭代期间，我们将粒⼦的当前⾼度值（CHV）与IHV进⾏⽐较；如果CHV等于或低于IHV，则将粒⼦移回IHV的位置并使粒⼦⽆法移动.

在经过外⼒和内⼒计算之后获得了近似的真实地形，然后通过计算原始LIDAR点云和模拟粒⼦之间的距离。距离⼩于阈值HCC的LIDAR点被归类为地⾯点，⽽其余点为⾮地⾯点。

CSF具体过程

• 1.对点云进行异常点云（噪点）处理。
• 2.倒置原始的LIDAR点云。
• 3.初始化布料⽹格。根据⽤⼾定义的⽹格分辨率（GR）确定粒⼦数。布的初始位置通常设置在最⾼点上⽅。
• 4.将所有LIDAR点和⽹格粒⼦投影到⽔平⾯，并在该平⾯中找到每个⽹格粒⼦的CP，然后记录 IHV。
• 5.对于每个⽹格粒⼦，如果该粒⼦可移动，则计算受重⼒影响的位置，并将该布粒⼦的⾼度与IHV 进⾏⽐较。如果粒⼦的⾼度等于或⼩于IHV，则将该粒⼦放置在IHV的⾼度上，并将其设置为“⽆法移动”
• 6.对于每个⽹格粒⼦，计算受内⼒影响的每个粒⼦的位移
• 7.重复（5）-（6）。当所有粒⼦的最⼤⾼度变化（M_HV）⾜够⼩时，或者超过⽤⼾指定的最⼤迭代次数时，布料模拟过程将终⽌。
• 8.计算云到⽹格粒⼦和激光元点云之间的云距离。
• 9.分离地⾯点。对于每个LIDAR点云，如果与模拟粒⼦的距离⼩于HCC，则将该点归为地⾯点云，否则将其归为⾮地⾯点