论文阅读:IMLS-SLAM

发表于 更新于 分类于
我爱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,θ,φ)Tm = (r, \theta, \varphi)^T

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

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

        • 切平面：nxx+nyy+nzz+d=0n_xx + n_yy + n_zz + d = 0或者pTn+d=0p^Tn+d=0
        • 法向量：n=(nx,ny,nz)T,∣∣n∣∣22=1\textbf{n}=(n_x,n_y,n_z)^T, ||n||_2^2=1

      • Description of SRI(Spherical Range Image)

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

      • Descrition of Fast Approximate Least Squares

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

        \begin{equation} \begin{aligned} p = \left[\begin{array}{c} x \\ y \\ z \end{array}\right] = m = r\left[\begin{array}{c} cos\theta cos\varphi\\ sin\theta cos\varphi\\ sin \varphi \end{array}\right] \end{aligned} \end{equation}

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

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

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

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

          e~=∑(piTn~−1)2\tilde{e}=\sum(p_i^T\tilde{n}-1)^2

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

          e~=∑ri2(viTn~−ri−1)2\tilde{e}=\sum r_i^2(v_i^T\tilde{n}-r_i^{-1})^2

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

          e~=∑(viTn~−ri−1)2\tilde{e}=\sum (v_i^T\tilde{n}-r_i^{-1})^2

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

          M~=∑viviT,b~=∑1riviFinally,n~=M~−1b~\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算子在笛卡尔坐标系下为

          ∇=ex∂∂x+ey∂∂y+ez∂∂z\nabla = e_x \frac{\partial}{\partial{x}} + e_y \frac{\partial}{\partial{y}} + e_z \frac{\partial}{\partial{z}}

          其中ex,ey,eze_x,e_y,e_z分别对应s,y,zs,y,z轴上的单位向量

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

          x=rcosφcosθy=rcosφsinθz=rsinφx = rcos\varphi cos\theta \\ y= rcos \varphi sin\theta \\ z = rsin \varphi

          以及有

          r=x2+y2+z2φ=arctanx2+y2zθ=arctanyxr = \sqrt{x^2+y^2+z^2} \\ \varphi = arctan \frac{\sqrt{x^2+y^2}}{z} \\ \theta = arctan \frac{y}{x}

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

          \begin{equation} \begin{aligned} \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 \varphi} \frac{\partial \varphi}{\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 \varphi} \\ & = \frac{x}{r} \frac{\partial }{\partial r} + \frac{-y}{x^2+y^2}\frac{\partial }{\partial \theta} + \frac{xz^2}{\sqrt{x^2+y^2}({x^2+y^2+z^2})}\frac{\partial }{\partial \varphi} \\ & = cos\varphi cos \theta \frac{\partial }{\partial r} + \frac{-sin\theta}{rcos \varphi}\frac{\partial }{\partial \theta} + \frac{cos\theta sin\varphi}{r}\frac{\partial }{\partial \varphi} \end{aligned} \end{equation}

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

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

          n=∇s(θ,φ)n = \nabla s(\theta, \varphi)

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

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

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

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

      • 实现细节：

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

          ∣∣P−Pk∣∣≤r,Pk∈Vpr||P-P_k|| \leq r,P_k \in V_p^r

          • 如何选择半径rr？

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

            Ef(Vpr)=−a1Dln(a1D)−a2Dln(a2D)−a3Dln(a3D)Ef(V_p^r) = -a_{1D}ln(a_{1D})-a_{2D}ln(a_{2D})-a_{3D}ln(a_{3D})

          • 如何计算a1D，a2D，a3Da_{1D}，a_{2D}，a_{3D}？

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

            M=(x1−x‾,⋯ ,xN−x‾)M = (x_1-\overline{x}, \cdots, x_N - \overline{x})

            计算特征矩阵

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

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

            \begin{equation} \begin{aligned} 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{aligned} \end{equation}

          • 注意：最后一定有a1D+a2D+a3D=1a_{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(tk−1)T(t_{k-1})，和本次扫描结束后的位姿T(tk)T(t_{k})是线性关系，能够在tk−1<t<tkt_{k -1} < t<t_k这段时间中进行线性插值

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

      ∵T(tk)T(tk−1)−1=T(tk−1)T(tk−2)−1=ΔT∴T~(tk)=T(tk−1)T(tk−2)−1T(tk−1)\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.5m0.5m就把他们聚集(cluster)到一起，形成一个Bounding Box
      • 如果这个Bounding Box相对雷达坐标系Xv,Yv,ZvX_v, Y_v, Z_v三个坐标轴上的长度小于14m,14m,5m14m, 14m, 5m，就认为他们是人、车这些有动态可能性的物体，因此要丢弃(discard)他们这部分点云
      • 最后在把地面点云添加进来

Part Four：Scan Sampling Strategy

  • 这里作者提出

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

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

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

  • 整个采样步骤

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

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

      a2D2(xi×ni⃗)⋅Xv−a2D2(xi×ni⃗)⋅Xva2D2(xi×ni⃗)⋅Yv−a2D2(xi×ni⃗)⋅Yva2D2(xi×ni⃗)⋅Zv−a2D2(xi×ni⃗)⋅Zva2D2∣ni⃗⋅Xv∣a2D2∣ni⃗⋅Yv∣a2D2∣ni⃗⋅Zv∣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中的第一个所对应的点PLiP_{Li}，在这个点附近找s个点，遵循下列条件

      ∣∣PLi−Pk∣∣<r,k=1,⋯ ,s||P_{Li}-P_k|| < r,k=1,\cdots ,s

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

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

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

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

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

    I(x)=∑si∈SWi(x)((x−si)⋅ni)∑sj∈SWj(x)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)}

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

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

    1. 设pi∈Pkp_i \in P_k，且PkP_k是在点xx附近以该点xx为球心，半径为3h3h的邻域内的点

    2. 计算隐函数

      IPk(x)=∑pi∈PkWi(x)((x−pi)⋅ni)∑pj∈PkWj(x)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)}

      其中nin_i是pip_i的法向量，求法向量的方法就是前面提到的SRI求导法

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

      argmin ∑xj∈Sk∣IPk(Rxj+t)∣argmin \space \sum_{x_j \in S_k}|I^{P_k}(Rx_j+t)|

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

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

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

        yj=xj−IPk(xj)njy_j = x_j - I^{P_k}(x_j)n_j

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

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

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

        argmin∑xj∈Sk∣nj⋅(Rxj+t−yj)∣2argmin \sum_{x_j \in S_k} |n_j \cdot (Rx_j+t-y_j)|^2