1、摘要

提出了一种手持式3D传感器的SLAM算法，该算法使用点和平面作为基本体。我们表明，可以使用三个点平面基本体（3个平面、2个平面和1个点、1个平面和2个点以及3个点）的任意组合在两个不同坐标系中注册3D数据。算法使用RANSAC框架中的最小基元集来稳健地计算对应关系并估计传感器姿态。由于平面数明显小于典型3D数据中的点数，我们的RANSAC算法更喜欢包含更多平面的原始组合，而不是点。与现有的主要使用点进行配准的方法相比， 优点：

1. 由于平面图元的数量较少，因此可以实现更快的对应搜索和配准；
2. 它生成的基于平面的3D模型比基于点的模型更紧凑；
3. 作为一种全局配准算法，我们的方法不存在局部极小或任何初始化问题。

2、贡献

• 我们表明，三个3D点/平面图元的任何组合都允许在两个不同坐标系之间进行配准。
• 提出了一个使用3D点和3D平面的束平差框架。
• 演示了一个实时SLAM系统，该系统使用了手持式Kinect传感器的拟议技术。

3、系统概述

图2显示了我们的SLAM系统的流程图。系统的输入是一对彩色图像和深度图（RGB-D数据）。我们使用测量和地标的标准术语：我们的系统从输入数据中提取测量值，并生成/更新全球地图中的地标。这是通过从输入数据中提取点和平面，并将其与使用先前测量生成的全球地图中的点/平面地标进行注册来实现的。我们在该系统中的主要贡献是：（1）基于RANSAC的注册算法，使用最少的基元数；（2）使用点和平面的地图优化算法，分别在第三节和第四节中详细介绍。在这里，我们简要描述了系统的每个组件。

3.1 点测量:

我们的系统从每个彩色图像中提取2D关键点，并使用获得3D点测量值的相应深度图。每个点的测量值都用$（p_m，D_m）$表示，其中$p_m$表示3D位置，$D_m$表示关键点描述符。我们使用OpenCV中实现的SURF作为描述符。

3.2 平面测量：

为了从深度地图生成的3D点云中提取基本平面，我们使用以下多级RANSAC算法：

1. 在3D点云中随机选择几个（我们的实验中为20个）参考点。
2. 对于每个参考点，使用小局部窗口（101×101像素）内的附近点找到最佳平面。
3. 查找所有内点（阈值为20 mm），这些内点与图像空间上网格图的参考点相连接。
4. 确定最佳平面为具有最大且足够（>10000）个内点的平面。
5. 如果找到了最佳平面，请删除与之对应的内点，然后返回第1阶段。否则，终止算法。

每个平面测量值都用$（{\pi }_m，I_m）$表示，分别表示平面参数和内点数。4D矢量${\pi }_m=（n_m^T，d_m{）}^T$，其中$n_m$是单位法向量，$d_m$是到相机坐标系原点的距离。

3.3 注册：

当前帧的姿势是通过使用我们基于RANSAC的注册算法将测量值注册到地图中的地标来计算的。第三节介绍了使用点到点和平面到平面对应以及有效的RANSAC程序的注册问题的封闭式解决方案。

3.4 地图更新

在全局地图中，我们的系统维护从关键帧中的测量生成的点和平面地标。如果使用基于RANSAC的注册计算的姿势与以前的关键帧的姿势有足够的不同，我们的系统会将当前帧作为关键帧添加到地图中。基于RANSAC的注册结果还用于确定关键帧中的测量是与现有地标关联（是内点的情况）还是生成新地标（是局外点的情况）。每个点地标用$（p_l，D_l）$表示，其中$p_l$表示3D位置，$D_l$表示从相关联点测量获得的关键点描述子信息。每个平面地标用$（{\pi }_l，I_l）$表示，其中${\pi }_l=（n_l^T，d_l{)}^T$表示平面参数，$I_l$表示相关联平面内点的集合。

3.5 平面优化

为了获得全局一致的结果，我们的系统运行束调整，以使用所有测量值优化关键帧和点/平面地标的姿势。第四节解释了这一步骤。如[1]所示，束调整与主摄像机跟踪线程一起异步运行。

4、使用点和平面注册

在本节中，提出一种使用点到点和平面到平面对应的闭合形式配准算法。该算法适用于3个或3个以上的匹配；因此，它可以用于在我们的RANSAC框架中生成假设，以及使用所有内点优化相机姿势。然后，我们介绍了高效的RANSAC程序，该程序将平面图元优先于点图元，因为三维数据中的平面数通常小于点数。我们还描述了几种几何约束，以使用解释树去删除错误的对应。

A. Closed-Form Solution for Mixed Point-to-Point and Plane-to-Plane Correspondences

设$p_i$和$p{\prime }_i$，其中$i=1，\dots ，M$为相对应的3D点，${\pi }_j=（n_j^T，d_j{）}^T$和$\pi {\prime }_j=（n{\prime }_j^T，d{\prime }_j{）}^T$，其中$j=1,....,N$是相对应的3D平面。我们希望找到坐标系之间的刚体变换$[R，t]$。

单个情况解决方法:对于点对点对应情况，已经证明旋转和平移分量可以解耦。三维点集的质心$\overline{p}=\frac{1}{M}{\sum }_i{p}_i$ 和$\overline{p\prime }=\frac{1}{M}{\sum }_{i}p{\prime }_i$。$q_i=p_i-\overline{p}$和$q{\prime }_i=p{\prime }_i-\overline{p\prime }$然后通过最小化得到旋转的最小二乘解

这个问题可以用旋转的四元数表示或奇异值分解（SVD）来解决。使用估计的旋转R，平移计算为旋转质心之间的差值：

对于平面到平面的对应情况，旋转通过最小化获得

这可以类似于点到点对应的情况来解决。为了计算平移，我们可以叠加一个线性约束

对于3个或更多平面，并求解线性系统。

混合情况的解决方法
在我们考虑混合情况，其中我们既有点到点的对应，也有平面到平面的对应。我们利用在个别情况下使用的解耦，首先计算旋转，然后计算平移。
为了计算旋转，我们结合公式。（1） 和（3）得到：

其中，$w_j$是分配给第$j$个平面对应的权重。该等式与等式$(1)(3)$的形式相同。以相同的方式获得最优旋转。具体来说，我们将3×3协方差矩阵K定义为

设$K=UD{V}^T$是K的奇异值分解，其中$D=diag（\sigma 1，\sigma 2，\sigma 3）$，$\sigma 1\ge \sigma 2\ge \sigma 3\ge 0$。那么最佳旋转R:

为了计算平移，我们最小化

这对应于定义线性系统

其中，$I_3$是3×3单位矩阵。如果点对应的数量M为零，$A_1$和$b_1$消失；如果平面对应的数量N为零，$A_2$和$b_2$消失。最小二乘解由下式给出

B. Degeneracy Issues

为了唯一提取R和t，方程（6）中的相关矩阵K和方程（9）中的矩阵A应该满足某些条件。
为了唯一的计算R，K的秩应该大于1，并且必须至少满足以下条件之一：
（1）$det（U{V}^T）=1$；
（2） K的最小奇异值是一个单根$（\sigma 2!=\sigma 3）$
对于唯一确定的平移t，矩阵A应为秩3。注意，如果至少有一个点对应（M>0），则A的秩为3（因为是A1）
如果矩阵K和A的对应至少具有图3所示的以下其中一种，则矩阵K和矩阵A满足上述特性：

1. 3个非共线点------秩（K）= 2
2. 2个点和1个平面，其中连接两点的矢量与平面的法线不平行-----秩（K）=2
3. 1个点和2个非平行平面--------秩（K）=2
4. 法线跨度为$R^3$的三个平面-------秩（A2）=3，-------秩（K）=3。

C. Efficient RANSAC Procedure

与2D图像空间中的对应相反，3D基本体提供了几个不变量，可用于删除错误的匹配。这些不变量是独立于坐标系的几何实体。我们确定了以下三个不变量：

• I1：两点之间的距离。
• I2：点和平面之间的距离。
• I3：两个平面法线之间的角度。

不变量可以用向量$I=（i_1,i_2,i_3)$表示，其中$i_1$、$i_2$和$i_3$分别表示类型$I_1$、$I_2$和$I_3$的不变量数。我们可以观察到，所有涉及点和平面的对应三元组共有三个不变量，如下所示：

• 3 points: I = (3, 0, 0)
• 1 plane and 2 points: I = (1, 2, 0)
• 2 planes and 1 point: I = (0, 2, 1)
• 3 planes: I = (0, 0, 3)
  在我们的系统中，我们使用解释树[11]来使用这些不变量删除错误的对应。

在基于不变量的剪枝之前，我们需要获得一些对应的初始候选。在只有点的情况下，我们通过使用当前帧中的点测量值与地图中的点地标之间的最近邻描述符匹配来获得此类候选点。对于平面，我们使用所有可能的组合作为候选。我们使用3平面对应开始RANSAC程序，因为3D数据中的平面数通常比关键点的平面数小得多。此外，由于平面是由多个点生成的，因此它们受三维数据中的噪声影响较小，从而实现更精确的配准。如果测量包括少于3个平面，或者如果使用3个平面对应计算的内点数量很小，那么我们尝试使用2个平面和1个点对应等，如图3所示。我们通过使用要求取样的最小三元组数量的标准来终止RANSAC程序。

5、BUNDLE ADJUSTMENT USING BOTH POINTS & PLANES

传统的BA方法使用从2D图像或3D传感器获得的测量值，同时优化所有关键帧和/或点标记的姿势。我们通过添加平面地标来扩展3D传感器的方法：我们同时优化点和平面地标以及所有关键帧的姿势。
我们将要优化的变量表示为：

• Point landmarks: $p_l^i=(x^i,y^i,z^i{)}^T$
• Plane landmarks: ${\pi }_l^j=(a_j,b_j,c_j,d_j{)}^T$
• Keyframe poses: $T^k=(t_x^k,t_y^k,t_z^k,{\theta }_x^k,{\theta }_y^k,{\theta }_z^k)T$
  这里，$t^k=（t_x^k,t_y^k,t_z^k{）}^T$是第k个关键帧平移的$（x，y，z）$分量，而${\theta }^k=（{\theta }_x^k，{\theta }_y^k，{\theta }_z^k{）}^T$表示围绕（x，y，z）轴的旋转。第k个关键帧的旋转矩阵$R^k$由$R^k=R_z（{\theta }_z^k）R_y（{\theta }_y^k）R_x（{\theta }_x^k）$表示。我们使用点和平面测量值计算雅可比矩阵，如下所示。

5.1 Point Landmarks:

对于点路标点，我们最小化了路标点$p_l^i$和第$k$个关键帧中相关点测量$p_m^k=（x_m^k，y_m^k，z_m^k{）}^T$之间的距离误差，表示为

使用当前估计的路标点${\hat{p}}_l^i=({\hat{x}}^i,{\hat{y}}^i,{\hat{z}}^i{)}^T$和关键帧位姿$[{\hat{R}}^k,{\hat{t}}^k]$,我们将方程线性化为

其中${\hat{p}}_m^k={\hat{R}}^k{p}_m^k+{\hat{t}}^k$

对于每个（x，y，z）分量，我们分别得到3个方程。x分量的方程式为
对于y分量和z分量的值也可以类似地得到。

5.2 Plane Landmarks:

对于平面路标点，我们不是最小化平面之间的代数误差，而是最小化由平面路标点和关键帧中从相关平面测量中采样的3D点之间的距离之和定义的几何误差。具体来说，我们将均匀采样3D点，$x_m^{k,s}$，这些点来自平面${\pi }_m^k$的内点3D点，并计算每个采样点与相关平面${\pi }_l^j$之间的距离。最小值表示为:

我们使用当前估计的平面路标$\hat{\pi }=({\hat{a}}^j,{\hat{b}}^j,{\hat{c}}^j,{\hat{d}}^j)$和关键帧位姿$[{\hat{R}}^k,{\hat{t}}^k]$将当前方程线性化

其中

简化方程后，我们得到

5.3 求解

通过Eqs。（15） （18）对于所有点/平面路标和关键帧，我们得到一个线性系统$J∆=-{ϵ}_0$，其中$J$代表雅可比矩阵，${ϵ}_0$是给定变量与当前估计值的误差向量。和$∆$ 是更新向量，包括$∆p_j^i$，$∆{\pi }_l^j$，和$∆T^k$待计算。我们的系统目前在每次调用束平差过程时计算整个线性系统，并使用高斯-牛顿迭代法和Eigen中的稀疏线性解算器求解。它可以通过使用分层的[30]或基于树的[31]图结构来更有效地计算线性系统的增量更新和解。