基于“分布 —— 多分布” 的点云配准方法

点击上方“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. 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. 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. 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)

推荐阅读：

• 专辑|相机标定
  

• 专辑|3D点云
  

• 专辑|SLAM
  

• 专辑|深度学习与自动驾驶
  

• 专辑|结构光
  

• 专辑|事件相机
  

• 专辑|OpenCV学习
  

• 专辑|学习资源汇总
  

• 专辑|招聘与项目对接

• 专辑|读书笔记

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题