论文阅读:IMLS-SLAM

Daily Paper Reading: IMLS-SLAM

Part One: Abstract

• 原文叙述

  

• 我的理解

  • 本文贡献点：提出了一种低漂移的SLAM算法，主要框架是scan to model and matching
  • 主要思路：
    1. 基于雷达扫描数据进行一次特殊设计的采样策略
    2. 将之前局部雷达sweep结果作为模型，使用IMLS(Implicit Moving Least Squares)曲面重建

Part Two：Propaedeutics

• 预备知识一：引自文章《Fast and accurate computation of surface normals from range images》

  • 论文简述：论文提出一种通过对来自Spherical Range Image的表面求导，直接获得法向量的方法。

  • 研究背景：在之前的研究中，要获得一个点的切平面或者法向量，都是对该点的邻域采用最小二乘法，但是该方法计算代价昂贵。

  • 注意！！！：在研究一个点的切平面或者法向量时，该点一定不是一个孤独的点，附近一定有邻域点！

  • 具体推理

    • 定义球面坐标系中一点$m = (r, \theta, \varphi)^T$

    • 定义其对应着笛卡尔坐标系一点$p = (x,y,z)^T$

    • 某一点的切平面与法向量

      • 切平面：$n_xx + n_yy + n_zz + d = 0$或者$p^Tn+d=0$
      • 法向量：$\textbf{n}=(n_x,n_y,n_z)^T, ||n||_2^2=1$

    • Description of SRI(Spherical Range Image)

      SRI是指一个函数$s(\theta, \varphi)$，是指从坐标系原点出发，沿着$(\theta, \varphi)$方向，其深度为$r$的某一点

    • Descrition of Fast Approximate Least Squares

      首先要确定球面坐标系与笛卡尔坐标系之间的转化关系：

      

      $$\begin{array}{}\text{(1)}& \begin{array}{r}p=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=m=r\left[\begin{array}{c}cos\theta cos\phi \\ sin\theta cos\phi \\ sin\phi \end{array}\right]\end{array}\end{array}$$

      设$v = [cos\theta, sin\theta, sin\varphi]^T$，则上面的式子又可以表示为$p = rv$，截止至此，准备工作就做好了。

      • 目标优化的损失函数：尽量使得点都满足平面约束方程！

        $argmin\space \{n,d\}^* st. \space e=\sum(p_i^Tn-d)^2$

      • 来自Unconstrained Least Squares的思路，对目标函数两侧除以d的平方

        $\tilde{e}=\sum(p_i^T\tilde{n}-1)^2$

      • 用$p=rv$​替换上式，把$r_i$提到括号外，可以得到

        $\tilde{e}=\sum r_i^2(v_i^T\tilde{n}-r_i^{-1})^2$

      • 默认同一个邻域内的点深度$r_i$​都近似相等，因此可以进一步简化式子

        $\tilde{e}=\sum (v_i^T\tilde{n}-r_i^{-1})^2$

      • 应用最小二程思想求解即可，或者直接使用下面的公式

        $\tilde{M} = \sum v_iv_i^T,\tilde{b}=\sum \frac{1}{r_i}v_i\\Finally ,\tilde{n}=\tilde{M}^{-1} \tilde{b}$

    • 该论文方法：SRI求导法

      • 设DEL算子在笛卡尔坐标系下为

        $\nabla = e_x \frac{\partial}{\partial{x}} + e_y \frac{\partial}{\partial{y}} + e_z \frac{\partial}{\partial{z}}$

        其中$e_x,e_y,e_z$分别对应$s,y,z$轴上的单位向量

      • 则对应到球面坐标系中有

        $x = rcos\varphi cos\theta \\y= rcos \varphi sin\theta \\z = rsin \varphi$

        以及有

        $r = \sqrt{x^2+y^2+z^2}\\\varphi = arctan \frac{\sqrt{x^2+y^2}}{z}\\\theta = arctan \frac{y}{x}$

        因为$x=g(r,\theta, \varphi)$，则

        $$\begin{array}{}\text{(2)}& \begin{array}{rl}\frac{\partial }{\partial x}& =\frac{\partial }{\partial r}\frac{\partial r}{\partial x}+\frac{\partial }{\partial \theta }\frac{\partial \theta }{\partial x}+\frac{\partial }{\partial \phi }\frac{\partial \phi }{\partial x}\\ & =\frac{2x}{2\sqrt{x^2+y^2+z^2}}\frac{\partial }{\partial r}+\frac{-\frac{y}{x^2}}{1+(\frac{y}{x}{)}^2}\frac{\partial }{\partial \theta }+\frac{2x}{2\sqrt{x^2+y^2}(1+(\frac{\sqrt{x^2+y^2}}{z}{)}^2)}\frac{\partial }{\partial \phi }\\ & =\frac{x}{r}\frac{\partial }{\partial r}+\frac{-y}{x^2+y^2}\frac{\partial }{\partial \theta }+\frac{x{z}^2}{\sqrt{x^2+y^2}(x^2+y^2+z^2)}\frac{\partial }{\partial \phi }\\ & =cos\phi cos\theta \frac{\partial }{\partial r}+\frac{-sin\theta }{rcos\phi }\frac{\partial }{\partial \theta }+\frac{cos\theta sin\phi }{r}\frac{\partial }{\partial \phi }\end{array}\end{array}$$

        那同理可以得到$\frac{\partial}{\partial y}、\frac{\partial}{\partial z}$，这个式子计算起来很麻烦，我就不写了

      • 因此，有了算子$\nabla$​，就可以对SRI函数进行求导，我们都知道一个点的导数就是它所对应的求俄平面法向量，所以有

        $n = \nabla s(\theta, \varphi)$

  • 预备知识二：引自文章《Dimensionality based Scale Selection in 3D Lidar Point Clouds》

    • 论文简述：提出了一个说法，指一个点云局部几何结构更具有以下三种性质

      我的理解：和LOAM的部分一样，就是对点云集的协方差矩阵进行PCA主成分析，得到3个特征值$a_{1D}，a_{2D}，a_{3D}$分别对应下列三种性质

      1. 线性性(linear)：其线性性大于其他两个性质
      2. 平面性(planar)：平面性最为突出
      3. 空间性(volumetric)：如果形成一个闭合球面，则点云具有空间性

    • 实现细节：

      1. 获取邻域点云$V_p^r$，这里涉及一个邻域半径$r$​的选择

         $||P-P_k|| \leq r,P_k \in V_p^r$

         • 如何选择半径$r$？

           给定一个$r$的上下边界，采样边界中的16个值，对比发现使得$V_p^r$的熵最小化的$r$就是最优半径，这里给出熵的计算公式

           $Ef(V_p^r) = -a_{1D}ln(a_{1D})-a_{2D}ln(a_{2D})-a_{3D}ln(a_{3D})$

         • 如何计算$a_{1D}，a_{2D}，a_{3D}$？

           假设现在获取了一个堆邻域点云，首先要对他们去质心，得到

           $M = (x_1-\overline{x}, \cdots, x_N - \overline{x})$

           计算特征矩阵

           $C = \frac{1}{N}M^TM$

           对该矩阵进行特征分解可以得到特征值$\lambda_1、\lambda_2、\lambda_3$​(从小到大排列)，因此可以得到

           $$\begin{array}{}\text{(3)}& \begin{array}{rl}{a}_{1D}& =\frac{{\sigma }_1-{\sigma }_2}{{\sigma }_1}\\ a_{2D}& =\frac{{\sigma }_2-{\sigma }_3}{{\sigma }_1}\\ a_{3D}& =\frac{{\sigma }_3}{{\sigma }_1}\end{array}\end{array}$$

         • 注意：最后一定有$a_{1D}+a_{2D}+a_{3D}=1$

Part Three：Scan Egomotion and Dynamic Object Removal

• 论文中提到了Egomotion，并给出了释义

  • Egomotion：the movement of the vehicle during the acquisition time of a scan

    就是指在获取激光雷达数据的过程中，vehicle也在运动，这个过程中也有一个movement，需要补偿

• 在Scan Egomotion部分中

  • 由于在数据获取中ehicle也在移动，因此需要在创建点云时把这个问题也考虑进来

  • 作者认为在上一次扫描的结尾对应的位姿：$T(t_{k-1})$，和本次扫描结束后的位姿$T(t_{k})$是线性关系，能够在$t_{k -1} < t<t_k$这段时间中进行线性插值

  • 为获取Local Deskewed PointCloud(局部无偏点云)，需要估计当前时间$t_k$的位姿$\widetilde T(t_k)$​，考虑到线性关系

    $\because T(t_{k}) T(t_{k-1}) ^{-1} = T(t_{k-1}) T(t_{k-2})^{-1} = \Delta_{T} \\\\\therefore \widetilde T(t_k) = T(t_{k-1}) T(t_{k-2})^{-1} T(t_{k-1})$

• 在Dynamic Object Removal部分中

  • 本篇论文就是暴力去除存在动态可能性的物体，就是认为一个大小为$14m\times 14m\times 5m$的物体一定是动态的，那就一定要去除！！！反正多线雷达数据有的是
  • 具体去除方法：
    • 首先，用一个容器存储好地面点，并在原点云中去除这部分点云
    • 之后，使用一个聚类的思想，只要相邻数据点之间的距离小于$0.5m$就把他们聚集(cluster)到一起，形成一个Bounding Box
    • 如果这个Bounding Box相对雷达坐标系$X_v, Y_v, Z_v$三个坐标轴上的长度小于$14m, 14m, 5m$，就认为他们是人、车这些有动态可能性的物体，因此要丢弃(discard)他们这部分点云
    • 最后在把地面点云添加进来

Part Four：Scan Sampling Strategy

• 这里作者提出

  • 目前在使用ICP之前，都会提前计算一下点云的主轴方向，并且把点云分配到各个方向上

  • 他不要，我直接把激光雷达的三个主轴作为点云的三个主轴

  • 这样的话，大部分平面会直接被分配到每个轴上，比如地面就相当于沿着Z轴观测，而一些物体立着的面就相当于沿着X、Y轴发生移动

• 整个采样步骤

  1. 使用前面提到的SRI求导法计算每一个点的法线，以及使用PCA分析点云的三组特征值$a_{1D}、a_{2D}、a_{3D}$

  2. 作者提出了9个神奇的指标，至于为啥我想了很久也没明白

     $a^2_{2D}(x_i\times\vec{n_i} )\cdot X_v\\-a^2_{2D}(x_i\times\vec{n_i} )\cdot X_v\\a^2_{2D}(x_i\times\vec{n_i} )\cdot Y_v\\-a^2_{2D}(x_i\times\vec{n_i} )\cdot Y_v\\a^2_{2D}(x_i\times\vec{n_i} )\cdot Z_v\\-a^2_{2D}(x_i\times\vec{n_i} )\cdot Z_v\\a^2_{2D}|\vec{n_i}\cdot X_v|\\a^2_{2D}|\vec{n_i}\cdot Y_v|\\a^2_{2D}|\vec{n_i}\cdot Z_v|\\$

     前6个代表Rotation，后面3个代表translation

  3. 之后对所有点都计算一遍这9个指标，形成9个list，按照从大到小的顺序给这9个list依次排序

  4. 取每一个list中的第一个所对应的点$P_{Li}$，在这个点附近找s个点，遵循下列条件

     $||P_{Li}-P_k|| < r,k=1,\cdots ,s$

  5. 这样一共能找到$9s$个点，这个就是我们要的点

至此，采样过程结束，那9个指标我真的想了好久。。。但确实不懂了。可能后面作ppt后结合图片就明白了。

Part Five：Scan-to-Model Matching with IMLS Surface Representation

• 在进行了数据采样子后，就该轮到优化求解$R、t$的部分了，这里他用到了IMLS的方法

• IMLS起源于MLS，说的就是仿照MLS，构建了一个隐函数

  $I(x) = \frac{\sum_{s_i \in S}W_i(x)((x-s_i)\cdot n_i)}{\sum_{s_j \in S}W_j(x)}$

  • 其中$W_i(x)=e^\frac{-||x-s_i||^2}{h^2}$代表权重，只要点云服从$h-sampling$的采样条件，这个隐函数$I(x)$就能够很好地描述目标点到目标曲面的距离
  • $I(x)=0$的解近似于要估计的目标曲面！

• 从本文角度出发，假设现在有一个点$x$，我们要估计它相邻两个时刻的$R$和$t$，他们利用这个IMLS就能够很到地解决这个问题

  1. 设$p_i \in P_k$，且$P_k$是在点$x$附近以该点$x$为球心，半径为$3h$的邻域内的点

  2. 计算隐函数

     $I^{P_k}(x)=\frac{\sum_{p_i\in P_k}W_i(x)((x-p_i)\cdot n_i)}{\sum_{p_j \in P_k }W_j(x)}$

     其中$n_i$是$p_i$的法向量，求法向量的方法就是前面提到的SRI求导法

  3. 之后我们的任务就很简单了，就是求得每一个变换后的点到曲面距离，并且令这个距离最小

     $argmin \space \sum_{x_j \in S_k}|I^{P_k}(Rx_j+t)|$

     但是问题来了，这个$I(x)$隐函数里面有e指数呀，这个非线性项是不允许我们去做线性最小二乘的，因此也就不能直接使用ICP点到面的对齐方式优化$R,t$

  4. 论文提出了另一个方法：

     • 首先把点投影到IMLS曲面上，得到投影结果$y_j$

       $y_j = x_j - I^{P_k}(x_j)n_j$

       其中，$n_j$是$x_j$最近邻点$p_c$的法线，这里用到了一个知识，就是点到平面投影的向量表示

       我加个链接，之后作ppt的时候可以参考这个https://zhuanlan.zhihu.com/p/148499539

     • 而经过变换后的点，应该落在曲面上，所以有

       $argmin \sum_{x_j \in S_k} |n_j \cdot (Rx_j+t-y_j)|^2$