ERASOR:基于栅格占用差异的动态物体去除方法

论文提出了一种如何去除激光slam生成点云地图中动态物体的后处理方法。

1问题定义与核心思想

定义经过slam生成的最终点云$\mathcal{M}(map)$定义如下：
$$\mathcal{M}=\bigcup _{t\in [T]}{}_Q^W{\mathbf{T}}_t\ast {\mathcal{P}}_t^Q\text{\hspace{1em}(1)}$$
$W$:world frame ,$Q$:query frame，$t:$query frame time，${\mathbf{T}}_t$:query frame 到 world frame的变换矩阵。

假设${\mathcal{P}}_t^M$为 $\mathcal{M}(map)$ 在${\mathcal{P}}_t^Q$坐标系的点云（即对$\mathcal{M}(map)$乘以${}_Q^W{\mathbf{T}}_t$的逆矩阵）

定义最终的静态点云地图为：
$$\hat{\mathcal{M}}=\mathcal{M}-\bigcup _{t\in [T]}{\hat{\mathcal{M}}}_{dyn,t}\text{\hspace{1em}(2)}$$
${\hat{\mathcal{M}}}_{dyn,t}$指由 ${\mathcal{P}}_t^M$和${\mathcal{P}}_t^Q$之间的差异确定的估计动态点云。

论文的方法将注意力集中在城市环境中大多数动态物体上，例如陆地车辆和行人，它们不可避免地与地面接触。基于这个假设，我们可以构建四种可能的情况：

• 在${\mathcal{P}}_t^M$中地面上有物体，而 ${\mathcal{P}}_t^Q$中相同位置的地面上没有物体

• ${\mathcal{P}}_t^M$中地面没有物体，而 ${\mathcal{P}}_t^Q$中相同位置的地面上有物体

• 物体同时存在${\mathcal{P}}_t^M$和${\mathcal{P}}_t^Q$中的地面上

• 物体同时不存在${\mathcal{P}}_t^M$和${\mathcal{P}}_t^Q$中的地面上

在这些情况中，表示动态对象的点将被分类为第一种情况和第二种情况。然而，我们的目标是细化地图云，因此忽略第二种情况（目的是将map中的动态点云去掉，如果map没有物体就不用考虑了）。为了清楚起见，我们定义两个术语：第一种情况是潜在的动态情况，第三种和第四种情况是绝对静态的情况。

2具体方法

2.1生成感兴趣区域（ Volume of Interest）

由于map范围很大，因此有必要在map中划定一块查询帧附近的小区域，再和查询帧进行比较，因此定义感兴趣区域为：
V t = { p k ∣ p k ∈ P t , ρ k < L max ⁡ , h min ⁡ < z k < h max ⁡ } \mathcal{V}_t=\left\{\mathbf{p}_k \mid \mathbf{p}_k \in \mathcal{P}_t, \rho_k<L_{\max }, h_{\min }<z_k<h_{\max }\right\} Vt​={pk​∣pk​∈Pt​,ρk​<Lmax​,hmin​<zk​<hmax​}
${\rho }_k=\sqrt{x_k^2+y_k^2}$，$L_{max},h_{min},h_{max}$为阈值。

请注意，${\mathcal{V}}_t$是通过kdtree从$\mathcal{M}(map)$ 中提取的，提取之后再变换到查询帧的坐标系，这样可以避免所有点云进行运算，节约时间。

2.2区域伪占用描述符(RPOD: Region-wise Pseudo Occupancy Descriptor)

RPOD 将${\mathcal{V}}_t$划分为方位和径向方向（即扇形和环）的规则间隔(bin)(即沿着径向和垂直于径向方向将Vt划分为二维栅格（bin），如果读过地面分割相关论文这个概念不难理解。)。令 Nr 和 Nθ 为环数和扇区数。则R-POD记为St，可表示为：
$${\mathcal{S}}_t=\bigcup _{i\in \left[N_r\right],j\in \left[N_{\theta }\right]}{\mathcal{S}}_{(i,j),t}\text{\hspace{1em}(1)}$$
${\mathcal{S}}_{(i,j),t}$表示t时刻，第i-j个bin内的所有点云${\mathcal{S}}_{(i,j),t}$满足以下条件：
$$\begin{array}{r}{\mathcal{S}}_{(i,j),t}=\{{\mathbf{p}}_k\mid {\mathbf{p}}_k\in {\mathcal{V}}_t,\frac{(i-1)\cdot L_{\max }}{N_r}\le {\rho }_k<\frac{i\cdot L_{\max }}{N_r}\\ \frac{(j-1)\cdot 2\pi }{N_{\theta }}-\pi \le {\theta }_k<\frac{j\cdot 2\pi }{N_{\theta }}-\pi \}\end{array}\text{\hspace{1em}(5)}$$
Thereafter, the unit space, i.e. each bin, assigns a single real value to describe pseudo occupancy, $\Delta h_{(i,j),t}$. Let $Z_{(i,j),t}=$ $\left\{z_k\in {\mathbf{p}}_k\mid {\mathbf{p}}_k\in {\mathcal{S}}_{(i,j),t}\right\}$. Then, the pseudo occupancy of each bin is encoded as follows:
$$\Delta h_{(i,j),t}=\sup \left\{Z_{(i,j),t}\right\}-\inf \left\{Z_{(i,j),t}\right\}\text{\hspace{1em}(6)}$$
其中sup和inf表示上边界和下边界。

2.3扫描占比测试（Scan Ratio Test）

对比${\mathcal{P}}_t^M$和${\mathcal{P}}_t^Q$同一个bin（栅格）的 $\Delta h_{(i,j),t}^M$和$\Delta h_{(i,j),t}^Q$的比值判断该bin内的点云是否存在动态物体，文中比例设置为0.2.

2.4区域地面平面拟合(Region-wise Ground Plane Fitting)

对2.3判断为存在动态物体的bin的点云，进行地面点和非地面点分割，非地面点即为动态物体点云。

由于每个bin相对于整个地图来说都很小，因此可以安全地假设bin内的地面是平坦的。

对于包含动态物体的bin${\mathcal{S}}_{l,t}^M$而言，地面点位于最下方，因此初始地面点${}^0{\mathcal{I}}_{l,t}$估计为：
$${}^0{\mathcal{I}}_{l,t}=\left\{{\mathbf{p}}_k\mid {\mathbf{p}}_k\in {\mathcal{S}}_{l,t}^M,z\left({\mathbf{p}}_k\right)<\bar{z}+{\tau }_{\text{seed }}\right\}$$
$\bar{z}$为从${\mathcal{S}}_{l,t}^M$选择几个最低点的z值的平均值，${\tau }_{\text{seed }}$为高度阈值。

由于方法是迭代的，因此 假设第 $i$次迭代bin中的地面点（ inliers） 为 ${}^i{\mathcal{I}}_{l,t}$, 然后计算${}^i{\mathcal{I}}_{l,t}$ 的协方差阵 ${}^i{C}_{l,t}$:
$${}^i{C}_{l,t}=\sum _{j=1:|{}^i{\mathcal{I}}_{l,t}|}\left({\mathbf{p}}_j-{}^i{\overline{\mathbf{p}}}_{l,t}\right){\left({\mathbf{p}}_j-{}^i{\overline{\mathbf{p}}}_{l,t}\right)}^T$$
where $|\cdot |$ and ${}^i{\overline{\mathbf{p}}}_{l,t}$ denote the size of a set and mean position of ${}^i{\mathcal{I}}_{l,t}$, respectively.

然后利用PCA分解得到协方差阵的特征值与特征向量, i.e. ${}^i{C}_{l,t}{\vec{v}}_m={\lambda }_m{\vec{v}}_m$ where $m=1,2,3$. 最小特征值对应的特征值向量则为地面点法线. 让特征向量为 ${}^i{\mathbf{n}}_{l,t}={\left[{}^i{a}_{l,t}{}^i{b}_{l,t}{}^i{c}_{l,t}\right]}^T$. 然后地面点的平面方程为：${}^i{d}_{l,t}=-{}^i{\mathbf{n}}_{l,t}^T{}^i{\overline{\mathbf{p}}}_{l,t}$, 展开后： ${}^i{a}_{l,t}x+{}^i{b}_{l,t}y+{}^i{c}_{l,t}z+{}^i{d}_{l,t}=0$. 我们的目标是提取地面点，即位于该平面下方一定距离的点认为是地面点：
$${}^{i+1}{\mathcal{I}}_{l,t}=\left\{{\mathbf{p}}_k\mid {\mathbf{p}}_k\in {\mathcal{S}}_{l,t}^M,{}^i{d}_{l,t}-{}^i{\hat{d}}_k<{\tau }_g\right\}$$
where ${}^i{\hat{d}}_k=-{}^i{\mathbf{n}}_{l,t}^T{\mathbf{p}}_k$ and ${\tau }_g$ denotes the distance margin of the plane.

设置迭代3次就够了，得到每个bin最终的地面点 ${}^3{\mathcal{I}}_{l,t}$ 。 因此非地面点即为动态点云:
$${\hat{\mathcal{M}}}_{dyn,t}=\bigcup _{l\in [L]}\left({\mathcal{S}}_{l,t}^M-{}^3{\mathcal{I}}_{l,t}\right).$$
对每一frame的每个bin进行同样的处理就得到了最终的动态物体点云和静态地图点云。

《ERASOR: Egocentric Ratio of Pseudo Occupancy-based Dynamic Object Removal for Static 3D Point Cloud Map Building》

[1](动态环境SLAM专题(一) | ICRA2021：ERASOR — 解决动态环境下SLAM建图"鬼影"问题 - 知乎 (zhihu.com))