IMLS-SLAM 论文学习

)

写作背景

应学长要求，在此做论文与代码的阅读整理。
本文章来源论文《IMLS-SLAM: scan-to-model matching based on 3D data》

摘要

论文展示了一个仅依赖点云数据的低漂移（low-drift）SLAM算法。
方法依赖于一个scan-to-model的匹配框架。
无闭环情况下4km，仅有16m的误差，即0.40％。

scan egomotion 和 动态对象删除

一次scan S是雷达传感器一次rotation获得的数据。

在rotation过程中，汽车正在移动，我们需要创建一个考虑这种变化的点云。

egomotion：在一次scan时间内汽车的移动

我们假定在两次连续的scans中，egomotion大致相同。我们用之前的位移估计当前的egomoiton。在一次scan中雷达角度和位置的变化是线性的。

τ(t)：在时刻t，汽车相对起始点的位置。

τ(t_k)：汽车在scan_k结束时的位置；即scan_k结束时刻为t_k。

t时刻（t_k-1<t<t_k）汽车位置可以在τ(t_k-1)和τ(t_k)之间线性插值。为了获得矫正的local点云，我们需要首先获得

估计值τ(t_k)=τ(t_k-1)*τ(t_k-1)*τ(t_k-2)^-1。

在匹配scan和model之前，我们要去除scan中所有移动物体。我们采用去除所有可能会动的小物体的方法：首先，删去地面点云，聚类，并抛弃所有（bounding box的）长小于14m，宽小于14m，高小于4m的物体。剩下的结构足够我们匹配。最后添上去掉的地面点云。

scan特征点选取策略

一旦我们创建了scan的unwarped点云，我们需要选择特征去匹配。传统ICP是随机选择特征点。论文《Geometrically stable sampling for the ICP algorithm》给出了一个采用scan点云的协方差矩阵去寻找几何稳定的特征点。且证明了如果选择合适ICP方法收敛。
该论文方法来源于这个方法，但有所不同。放弃使用点云的主轴（由点云协方差矩阵算得），而是保持汽车框架的坐标轴不变。我们定义

(X_v,Y_v,Z_v)：scan中点云在汽车框架坐标轴下的坐标。

这样大部分平面区域都会和轴对齐,例如地面是Z轴平移的参照,立面是XY轴上变换的参考。
首先我们计算每个点的法向。为了计算速度，使用论文《Fast and accurate computation of surface normals from range images》提出的SRI Derivative approach计算法向，并获得以下九个值：

我们的场景中并非一定要有平面出现，但是平面区域的出现可以提高算法的性能。这也为什么我们使用a_2D参与计算。
前六个来自点x_i值汽车的三个未知的角度相关(roll,pitch,yaw，即姿态的三个自由度)。而后三个与位置上的变换相关。
我们将所有点分别关于该九个值降序排序，获得九个序列。
我们从每个序列的前端取点，然后寻找该特征点x在model中最近的点p_c。只有当
才保留该特征点。其中参数r是待定参数，这一步是为了去除离群点（outliers）。重复该过程直到从每个序列中获得s个特征点，即一共获得9s个特征点。

基于IMLS的scan-to-match匹配

论文《Provably good moving least squares》阐释了IMLS框架。
我们定义点云map P_k 是前n张经过定位矫正后的scans的集合。所以P_k不仅包含了雷达测量误差，也包含scans的定位误差。使用IMLS框架我们隐函数I^Pk(x):

其中p_i是P_k中的点，n_i是p_i处的法向。
权
其中h时模型IMLS的参数，与点云采样密度和噪声有关。
该隐函数表示x距离隐含在点云P_k中的曲面的距离。
观察这个权，我们不难发现Wi(x)在x距离p_i较远时，会迅速减小，且当距离为3h时，Wi(x)<0.0002。所以，应用该式时我们只需要考虑以p_i为中心3h为半径的邻域。
我们想要在map P_k中定位当前的scan S_k，我们需要找到变换R和t使得，误差

最小。
即拟合变换后的scan S_k与map P_k。
因为指数权重的存在，我们不能直接采用ICP方法迭代。
所以定义一个映射：

将map S_k上的点x_i映射为y_i，其中n_j为map上距离x_j最近点的法向。
即y_j是x_j对map中曲面做垂线获得的垂足。
将误差换一种写法：

而该式可以用线性最小二乘获得其解。
所以整个迭代过程是 获得scan中所有点的映射点云Y_k，可以获得一组变换(R,t)，然后将scan点云S_k做(R,t)变换获得S’_k，再取得其映射点云Y’_k，获得新的(R’,t’)。重复直到(R,t)几乎为0。
最后取得的scan点云S_k，即是经过定位矫正后的点云，将其添入map中，继续下一张scan的迭代计算。

附1：论文《Fast and accurate computation of surface normals from range images》简述

以往获得一个点的切平面或者法向，都是对该点的邻域采用最小二乘法。但是该方法计算代价昂贵，论文提出了一种通过对来自spherical range image的表面求导的方法直接获得法向的方法。
首先引入表示方法：
球面坐标系：

m=(r,θ,Φ)_T

其中，r是range，即距离。θ，Φ分别是两个偏向角（azimuth，elevation component）。
笛卡尔坐标系下：

p = (x,y,z)_T

Aphercial Range Image(SRI) 是一个函数 s(θ,Φ)，指在从原点出发，在方向(θ,Φ)上，某点的深度（range，distance）。

几个传统最小二乘法：
1，Traditional Total Least Squares
目标切平面 n_x+n_y+n_z-d = 0 ；
即是目标法向 n=(n_x,n_y,n_z)_T ;
最小化误差

其中p_i是目标点邻域点云中的点。
其解n是对应矩阵M中最小特征值的特征向量。



2，Normalized Least Squares
取

K是一个下三角阵，由下式确定：

然后再从M’获取目标法向。
3，Unconstrained Least Squares
将第一种方法的误差函数两边同时除以d的平方：

其解n~：
其中：

目标法向：

4，Fast Approximate Least Squares
我们即球坐标系和笛卡尔坐标系的转化为：

p  = rv 

p是31的列向量，点的笛卡尔下的坐标。
r是球坐标系下的range，v是用θ和Φ表示的31的列向量。
对误差函数右边提出r_i。

因为点都来自一个邻域r_i认为相等。

由此获得结果：

SRI求导法：
del算子：
笛卡尔坐标系下del算子：

其中x^, y^, z^是坐标系上三个方向上的单位向量。
应用链式法则带入球坐标系中：
即对某关于（r，θ，Φ）的函数求导，可以获得一个用x^ ,y^ ,z^表示的变化量。
而对SRI函数应用del算子，可得：

其中：

附2，论文《dimensionality based scale selection in 3d lidar point clouds》简述

论文提出了一个多尺度方法计算点云图的鲁棒几何特征，以期获得每个点的最优邻域大小。邻域点的分布一般用来估计局部的几何特征，找到对每个点最合适的邻域是很多应用所共有的重要问题。传统上，邻域检索是通过查找目标点最近k个点或者以目标点为中心的几何体包含点来实现的。但他们都有共有的问题：1，k值或者几何体半径都是启发式地选择。2，常量而非数据引导。
论文目的在于提出一个方法寻找最优邻域半径。所谓最优邻域半径是指：与目标点属于同一对象的空间闭合点的最大集合。
论文提出了一个说法，指一个点云局部几何结构更具有1线性性（linear）2平面性（planar）3空间性（volumetric）。据我理解就是一个点云集排列成线，其线性性大于其他两个性质；如果排列成面，则平面性最为突出；同样，如果形成一个闭合球面，则点云具有空间性。
这三个维度性质我们记为a_1D，a_2D，a_3D。
采用球面定义邻域：
邻域点云V^r_P是以目标点P为中心，尺度(scale) r为半径的球面所包围的所有点。

该定义方法具有各向同性和旋转不变性，且r是唯一一个需要优化的参数。
使用PCA获得三个维度性质：



此时C是一个3*3对称正定阵，特征分解，获得三个特征值λ₁，λ₂，λ₃。（λ₁>λ₂>λ₃)
σ_j = 根号λ_j 是特征向量v_j方向下的标准差。
此时，PCA分解结果是一个三维椭球，三个轴的方向分别是3个特征向量,长度为特征值 。该椭球揭示了该邻域集在三个维度上的表现。
我们得到
a_1D = σ₁-σ₂ /μ
a_2D = σ₂-σ₃ / μ
a_3D = σ₃ / μ
我们选择μ=σ₁，使得a_1D+a_2D+a_3D=1

尺度r的选择：
首先确定尺度r的上下界r_min,r_max：
最小边界的选择取决于 1点云噪声2传感器规格3计算约束
1：局部散射：可能来源于传感器噪声,也可能是不可预期的几何结构(激光光线几乎平行表面)可能使得维度标签错误地标为volumetric
解决这个问题的先决条件是获得噪声密度 或者 用section1的方法获得最小尺寸
2：在车载扫描上很容易产生某些角度点云采样稀疏,提高r_min使得邻域至少包含不同扫描线
3：PCA所需点数：相关数据只需要10个点即可运算；如果上面两个不构成约束，则下界直接与点密度关联。
上界的选择相对来说并不重要，可以直接使用场景中最大物体尺寸。在实践中3-4m就可以了。

尺度r我们在[r_min，r_max]之间采样16个值获得，由于通常结果半径都更为接近r_min，所以该十六个值得选取是以平方因子增加。
有两种半径选择方案：熵特征Ef 和 相似度指数Si。
1，先定义：点集V^r_p的熵
Ef(V^r_p) = -a_1Dln(a_1D)-a_2Dln(a_2D)-a_3Dln(a_3D)。
即如果熵越小，特征越是集中于其中一个维度上的特征。
由此获得的最优半径r_Ef定义为：
r_Ef = argmin(Ef(V^r_p)) ；r∈[r_min,r_max]
即在(r_min,r_max)范围内使得特征最为集中的半径。
而集中在的维度即是维度标签 d*(V^r_p,r_Ef)。

2，点集Vrp的相似度指数Si(V^r_p)

即点集V^r_p中点特征在同一个维度上的比例，相当于点集属于同一个结构的比例。

两个半径选择标准Ef和Si都被测试了，并且半径选择结果有明显不同
并且前者优于后者。

附3：论文《Provably Good Moving Least Squares》简述

为了构建曲面，我们利用了moving least squares(MLS)算法，构建了一个隐函数I，其零解即目标曲。并且证明了只要点云满足ε-sampling，隐函数I是一个良好的距离函数（目标点距离目标曲面），同时其零解U几何近似于目标曲面F，且与F同态。
考虑在一个光滑闭合的表面F的点云S，且已知每个点的表面法向n_i。
定义关于点s_i的函数
P_si(x)=(x-s_i)* n_i
即，x距离s_i所在平面的距离（signed distance)
而隐函数I，是关于该函数在点云空间的权平均，定义如下：

其中权值为：

其中 A_i是s_i邻域中点的数量。