点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
作者：iceytan | 来源：知乎
https://zhuanlan.zhihu.com/p/135454523
本文仅做学术分享，如有侵权，请联系删除。
1. 简要
《Voxelized GICP for Fast and Accurate 3D Point Cloud Registration》是National Institute of Advanced Industrial Science and Technology, Tsukuba, Japan团队提出的基于“分布 —— 多分布” 的点云配准方法，目前代码已经开源。
代码下载地址：https://github.com/SMRT-AIST/fast_gicp/blob/non_ros/include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp
开源的代码中包含了：
FastGICP: multi-threaded GICP algorithm (~40FPS)
FastGICPSingleThread: GICP algorithm optimized for single-threading (~15FPS)
FastVGICP: multi-threaded and voxelized GICP algorithm (~70FPS)
FastVGICPCuda:CUDA-optimized voxelized GICP algorithm (~120FPS)
该团队不仅实现了多线程和基于CUDA的VGICP，还额外实现了单线程/线程版本的GICP，两个多线程的版本基于OpenMP完成。从论文的实验结果来看，
VGICP方法在速度和精度上均优于PCL中的GICP方法，但是需要注意的是，这一完全基于“分布”的VGICP方法要求具有较为靠近真值的初值，在实际中还是需要根据应用选择合适的配准方法。
2. 方法说明
我们遵循论文的思路进行说明。首先引入配准的定义，接着介绍GICP和NDT，最后给出VGICP的基本思路，在这一过程中，可以看到VGICP显示了其与GICP和NDT联系和拓展性。
2.1 点到点配准
在许多应用中，我们需要：基于SVD求解 3D-3D 点对匹配
专栏地址：https://zhuanlan.zhihu.com/p/111322916
通过寻找两个点集合之间的刚体变换关系，可以完成模型的合成或者传感器运动的估计。如，在不同方向采集同一个现实模型的点云数据，基于匹配对其拼接，得到一致的完成的模型；在相近的不同时刻采集的点云，求解其刚体变换可以得到激光雷达的运动姿态变换，用于机器人/自动驾驶车辆的运动估计。我们假设点都处于三维的空间中，两个点集合之间的姿态关系可以通过六个自由度的量进行表示，即XYZ上的位移变换关系和XYZ三个轴上的旋转变换关系。一般在实际的计算中，使用齐次坐标系表示的变换矩阵（transformation matrix）描述“位移”和“旋转”：
下文我们忽略齐次坐标系与常规的坐标系之间的变换，所以 得以直接完成。
2.2 GICP
假定我们找到了一对匹配点 和
的最优匹配为
，那么
定义它们之间的距离变量为 ，假定
和
独立，
GICP通过最大似然估计，找到置信最高的变换矩阵
这就构成了GICP中最核心的优化函数。从另外的角度来理解，该方程中间的部分 构成了新的“协方差”，
和
之间的误差将受到其作用，重新影响整体的匹配。
（1）当 其为单位矩阵
时，对于所有的配对都有
，可理解在最速下降时，不在任何解的方向增大或者减弱其权值，求解空间是比较“自由”的，整个方程就变成了最普通的“点到点配准”：
（2）为了要进行“点到面配准”，即我们希望对于所有的点 ，都距离目标点
对应的表面最近，下图[1]展示了红色的原点集合，蓝色的目标点集合，以及三对匹配点。
点到面配准示意图
以上图举例来说，对于“点到面配准”的ICP，在设计误差函数时，希望 、
和
的平方和距离尽可能地小。在实际采集两组点集合进行配准时，绝不可能两两配对点都是处于相同的空间位置的，但是可以认为，变换之后的点
应当处于目标点
周围形成的局部表面上的可能性大一些，这也更合理：
点到点与点到面的一个对比
那么计算时，首先使用SVD和特征值分解，计算目标点对应的法线，形成正交矩阵
，使用这一矩阵将变换之后的点
与目标点
之间的距离投影到法线方向上，最终，最小化投影距离的累加平方和，得到最优变换估计，使用公式表示有：
由于 正交，所有有
，并且
由此得到：
此时对于所有的配对都有 ，可见同样符合GICP的形式。
（3）在 时，限制最小化方向为配对目标点上的法线方向，而在
时则只取点
与
距离方向为最小化方向。目前为止，都只使用到了
而并没有涉及到
，可以猜测，当
也取点
处的法线相关量时，GICP的最小化公式表达出的就是“面到面”匹配。
面到面匹配
在这里不作详细的理论说明，只结合图示给出感性认识。GICP主要的思路是分别分析每一对点——原始点与目标点，各处的局部性质，来调整产生的距离残差的权重，这一权重是关于距离的二次形中的系数 . 上图中，红色与绿色线分别表示了环境平面，线上的黑点表示了采集到的点，红色线上的点与绿色线上的点需要进行配准，完成红色与绿色平面之间的姿态关系估计。每一点周围绘制的椭圆，是与局部几何性质相关的协方差矩阵。如最右侧的点，其椭圆在横向上较宽在纵向上较窄；而在绿色线最左侧的点，其椭圆在横向上较窄在纵向上较宽。在配对的时候，红绿线上右侧的点暂居较大的“权重”，是优化的主要项；而左侧的点，虽然距离较近，但是局部表面方向并不相配（甚至正交了），两处的协方差矩阵相乘将会导致较小的值（甚至为零），这些点对产生的“误差”，在优化中不占据重要作用。在具有较多“面特征”的数据中，GICP具有较大的优势：
2.3 NDT
在介绍GICP的时候，我们提及，不同位置对于现实世界的观测，并不都是一致的，因此使用点到面的配准，或者面到面的配准，在拥有较多面特征的数据中（环境中）会有较高的鲁棒性和收敛性。为了解决不一致的问题，还有另外的思路——体素网格化（voxel-based）
在[2] 中提出的NDT方法，现今被多个知名项目（如hdl_graph_slam）使用。NDT采用基于体素的关联方法，而不是精确的最近邻搜索。该算法首先将目标点云分割为一组体素，可以理解为使用相同大小的多个“立方体”圈定测量数据，接着基于每个体素网格中的点进行正态分布拟合，在三维点云的case中得到的是协方差矩阵，用分布来表示每一个“立方体”中的点的几何特征，如上图(b)与下图(c)中所示。
接着，将原始点云经过变换 投射到NDT表示的网格中，计算响应的概率密度。为了找到最优的变换，求解最大可能性的变换
即可。NDT最大的优点是避免了代价高昂的最近邻关联计算，因此它天生比ICP变量算法快得多。而 D2D-NDT（distribution-distribution ndt) [3]是NDT的一个扩展，它对源点云和目标点云进行体素化，并计算源点云和目标点云分布之间的距离，其用于累计距离误差的方案与GICP相似，即通过协方差进行权重的控制。
2.4 VGICP
VGICP同时计算源点 到多个目标点
的一定半径
范围内的邻点之间的距离：
作者认为，通过同时计算局部的多个点，能达到平滑局部特征的作用。将与GICP的类似，假定 和
独立，那么
通过最大似然估计，找到置信最高的变换矩阵
其中 为邻点的数量。
我们使用渐变的椭圆表示可能的分布，用实心的点表示确切的位置。不难看出，GICP实际上是一种 nearest distribution-to-distribution的方法，每次找到最近的点对，使用各自的局部几何性质加权距离损失，用以优化；而NDT是 voxel-based point-to-distribution 的方法，求解最优的变换，使得点投射到网格化的分布中时，达到的概率函数响应最大；VGICP则希望建模为voxel-based distribution-to-multidistribution方法，同时求解最有可能的分布到分布之间的配对关系。相比较之下，NDT使用点对体素分布对应的模型，但至少需要4个点(实际多于10个)来计算一个立方体中的协方差矩阵，如果体素中的点数很低，协方差矩阵就不能完全可用，而VGICP根据点及其邻点的分布计算体素分布，所以即使体素仅包含一个点，它也可以生成适当的协方差矩阵。
3. 总结
分析下来，VGICP实际上使用了多个邻点平滑配对点的距离和局部协方差，达到更鲁棒的计算，在实现中，使用KD-tree寻找 邻近的点，使用牛顿高斯优化器（Gauss–Newton optimizer）寻找最优的变换，开源的代码风格较好，并且有多线程与cuda的实现，可以从中学到不少技巧。但是细想下来，VGICP所谓的voxel-based，可能只是指使用
的“周围一团”用来做平滑，并没有像NDT那样进行确切的网格划分（公式中体现，可能圆满），再用平滑后的值用于计算。准确来说，在实现时，每添加一个邻点，便做了一次高斯分布的融合，修改均值与协方差，关于这一点的代码。
代码下载地址：https://github.com/SMRT-AIST/fast_gicp/blob/master/include/fast_gicp/gicp/fast_vgicp_voxel.hpp
参考
1、^Low, Kok-Lim. "Linear least-squares optimization for point-to-plane icp surface registration." Chapel Hill, University of North Carolina 4.10 (2004): 1-3.
2、^Biber, P., Strasser, W.: The normal distributions transform: a new approach to laser scan matching. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE (Oct 2003)
3、^Stoyanov, T., Magnusson, M., Andreasson, H., Lilienthal, A.J.: Fast and accurate scan registration through minimization of the distance between compact 3d NDT representations. International Journal of Robotics Research 31(12), 1377–1393 (Sep 2012)