GP-SLAM论文阅读（有关GP地图内容节选）

GP MAP RECONSTRUCTION

我们使用空间GP从噪声距离观测中重建局部表面。为了获得更可行的数据关联和地图更新，我们从恢复的表面提取离散样本。这一过程被称为区域化GP地图构建。换句话说，它可以看作是某种曲面插值。这一过程包括区域化和重建两个部分。

A. Regionalization

首先，为了在局部建立不同的函数关系，我们在世界坐标系 { W } \{W\} {W}中将整个域划分为几个均匀分布的立方单元。这种分解也加速了重构过程[19]。每个单元格的边长为a。原始点$S_t^W$位于第k个单元格的子集记为 S t , k W = { p k , i , i = 1 , . . . , n k } S^W_{t,k} = \{ p_{k,i},i = 1,...,n_k\} St,kW​={pk,i​,i=1,...,nk​}。

然后我们需要确定坐标之间的函数关系，$x=f(y,z)$，或者$y=f(x,z)$，或者$z=f(x;y)$.考虑到一个函数只能表达一个2.5D的曲面，对于复杂的三维结构，我们一般假设一个cell中存在三个函数。每个函数沿其方向提供相应的约束，这将在第四节中详细介绍。因此，如果一个单元格中的表面垂直于一个坐标平面，由于它只沿其法线提供约束，我们可以省略该单元格中的相应函数。这种情况是根据主成分分析(PCA)来判断的。图2给出了一个例子，当从垂直墙体中提取一组原始数据时，由于墙体不能提供垂直约束，因此忽略方向为z的函数。

B. Reconstruction

区域化后，在每个非空单元中进行GP地图重建。激光雷达测量的是环境的噪声样本。其噪声模型可由制造数据导出，见[11]。在这里，我们简单地假设每个激光雷达点遵循一个各向同性方差${\sigma }^2$的独立正态分布。在这种情况下，使用GP回归，它可以产生最好的线性无偏预测[19]。

基于[21]，GP回归问题具体如下:给定$n_k$个训练点作为$D=\{(f_i,l_i),i=1,...,n_k\}$,训练位置$l_i\in R^2$的观测值和$f_i\in R$之间的关系表示为$f_i=f(l_i)+{\epsilon }_i,i=1,...,n_k$,其中${\epsilon }_i$为噪声项，服从${\epsilon }_i\sim N(0,{\sigma }^2)$的分布。目标是实现$n_{test}$预测$f_{\ast }$在预设测试位置$l_{\ast }=[l_{\ast 1},l_{\ast 2},...,l_{\ast n_{test}}{]}^T$用$f_{\ast j}=f(l_{\ast j})+{\epsilon }_{\ast j},j=1,...,n_{test}$表示的。定义$f=[f_1,f_2,...,f_{n_k}{]}^T,l=[l_1,l_2,...,l_{n_k}{]}^T$给定$f$的预测分布$f_{\ast }$为

其中取平均值$k_{l\ast }^T({\sigma }^{2}I+K_{ll}{)}^{-1}f$作为$f_{\ast }$在测试位置$l_{\ast }$的点预测。其方差估计为$k_{\ast \ast }-k_{l\ast }^T({\sigma }^{2}I+K_{ll}{)}^{-1}{k}_{l\ast }$。这里，$k_{\ast \ast }=k(l_{\ast },l_{\ast })$，$k_{l_{\ast }}=k(l,l_{\ast }{)}^T$，并且$K_{ll}$是一个$n_k\times n_k$矩阵，$K_{ll}(i,j)=k(i,j),k(.,.)$表示核函数。在这项工作中，我们选择常用的指数协方差函数$k(l_i,l_j)=exp(-\kappa |l_i-l_j|)$，具有预设的长度尺度参数κ。在这种情况下，训练点是单元格中的原始点$S_{t,k}^W$。作为观测的坐标称为方向，另外两个坐标作为训练位置。每个$n_{test}$测试点均匀设置，间隔为$r$。回想一下单元格的边长是$a$，我们设$a$为$r$的积分倍，这意味着$n_{test}=(a/r{)}^2$。这些有方差的预测是从隐式曲面中抽取的样本，每一组样本被命名为层。如图2所示，那些离原始数据较远的预测更不可靠。我们使用这些样本作为重建结果。

重建后，一个细胞中有0 ~ 3层。单元格存储在散列表数据结构中。一个样本用$p_i=(f_i,l_i)$，其中$f_i\sim N(u_i,{\sigma }_i^2)$以试验地点为指标。这样可以直接查询样本。

C. Acceleration of the Reconstruction

在区域分解的基础上，通过局部回归的概念进一步加快了GP的训练过程。其核心思想是，预测主要受到那些训练位置更接近该预测的测试位置的观察值的影响。因此，训练过程可以通过有原则地对训练点进行下采样来加速，而不会有很大的精度损失[20]。因此，我们保留原始数据，但每次重建GP地图时只使用每个测试位置的所有最近点。因此，过滤后的训练点数量显著减少。

完成这个过滤过程的一种方法是利用kd树。然而，该数据结构的初始化代价为$O(nlogn)$，输入数为n，平均搜索代价为$O(logn)$[18]。虽然Kd-tree的搜索速度比暴力搜索快，但当点的数量很大时，它仍然很耗时。由于我们的应用中的搜索目标是均匀分布的，我们使用了一个改进的二维体素过滤器来近似这个过程。如图3所示，在一个单元中，训练位置分布在一个2D域上，该域被分成更小的网格，网格的中心是测试位置。原始体素过滤器计算每个较小网格中所有原始数据的平均值。修改后，如果它是同一小网格中所有点中离测试位置最近的一个，我们保留该点。通过这种方法，滤波过程可以用线性复杂度代价完成。

STATE ESTIMATION

在重建之后，当前帧与地图对齐。以地图为参照系，可以抑制姿态漂移。迭代进行GP地图重建和扫描配准过程，直到收敛到提供状态估计。扫描图配准过程包括两个主要步骤，匹配和对齐。匹配步骤建立两帧之间的对应关系，对齐步骤的目标是计算匹配对之间的转换。

A. Matching

当分别来自当前帧$p_t^w$和参考帧$Q_{t-1}^w$的两个样本满足以下条件时，将建立对应关系:1)两个样本位于相同或相邻的单元中;2)两个样本具有相同的预测方向和测试位置;3)两个样本的方差均小于阈值${\sigma }_{thr}^2$。为了简单起见，我们在图4的二维空间中说明了这个过程。Pair-1是合格的对应关系，而Pair-2由于一个样本的方差过大而无效。Pair-3在两个样本之间建立，因为它们共享另一个方向，并且位于相邻的细胞中。

在Pair-1中，有几个样本满足上述条件。在这种情况下，选择最接近的那个。配对样本用 { p i , q i } \{p_i,q_i\} {pi​,qi​}表示，其中$p_i=(f_{pi},l_{pi})$， $f_{pi}\sim N（u_{pi},{\sigma }_{pi}^2）$，$q_i=(f_{qi},l_{qi})$， $f_{qi}\sim N(u_{qi},{\sigma }_{qi}^2)$。

B. Alignment

基于层只在其方向上提供可观测性的思想，我们将误差度量设计为样本预测之间的距离。首先，给定$e_x=[1,0,0]$，三维点的坐标$p_i=[x_i,y_i,z_i{]}^T$可由$x_i=e_x\cdot p_i$计算，其他两个方向相同。为简洁起见，我们定义了一个算子(·)◦来表示以下上下文中获取方向对应坐标的操作。与[8]中表达的MLE方法类似，$n_{cur}$配对样本 { p i , q i } , i = 1 , . . . , n c u r \{p_i,q_i\},i = 1,...,n_{cur} {pi​,qi​},i=1,...,ncur​在所有层中，对于变换T，我们将某个测试位置的观测值的一维分布定义为$di\sim N((T_{pi})\cdot ◦-q_i\cdot ◦,{\sigma }_{pi}^2+{\sigma }_{qi}^2)$。然后计算相对变换

其中方差可以看作权重。该优化问题由非线性求解器Ceres[22]求解。

与我们之前的工作相比，注册表在几个方面进行了重新设计。在之前的工作中，仅使用每个单元中的一层来建立通信。该方法将所有重构样本视为三维点，并以三维欧氏距离作为误差度量。采用奇异值分解(SVD)方法求解该问题。相反，我们使用多层模型来模拟复杂结构，并扩展对应搜索区域以避免边界信息丢失(在之前的工作中，省略了图4中的pair-3)。误差度量也被改变了，这样它就会把表面拖得更近，而不是像以前的工作那样把点拖得更近。这导致更快的收敛，因为它避免了将测试位置引入目标函数。

C. Demonstration of Registration

我们选择了两组典型的范围数据来证明我们的注册在ICP和以前的方法的优势。在第一个测试中，我们使用了来自第七节实验a的旋转二维激光雷达测量的两帧距离数据。如图5(a)所示，这种传感器提供的点云较为稀疏且不均匀。这里选择广义ICP (Generalized ICP, GICP)作为基准。图5(b)和©的结果表明，GICP陷入了错误的局部极小值，而我们的方法很好地对准了结构。

其次，我们检查了我们的修改对配准策略的影响。我们选择实验C中Velodyne VLP-16采集的距离数据帧作为目标帧，在其上设置初始变换误差(平移1m，绕z轴旋转5度)，形成源帧。然后使用原始和当前的配准方法对这两个帧进行对齐。采用两帧最接近点之间距离的均方根误差(RMSE)作为收敛准则。如图6所示，结果表明，我们的方法的配准部分有了明显的改进。