论文阅读参考：https://zhuanlan.zhihu.com/p/135454523
fast-vgicp代码中以pcl::Registration为基类，写了自己GICP和VGICP方法。自己只对少数关键函数进行了重构。首先简述gicp和vgicp代码的逻辑，然后按照继承顺序逐层梳理代码原理。

GICP代码解析

重载了pcl::Registration类中的虚函数computeTransformation。
使用lm算法迭代优化位姿变换，在标准lm流程中重载了协方差的计算，以及匹配误差的计算。代码的核心函数分布在pcl::Registration->fast_gicp::LsqRegistration->fast_gicp::FastGICP三层继承关系中，分别是：

• pcl::Registration::align
• fast_gicp::LsqRegistration::computeTransformation
• fast_gicp::LsqRegistration::step_lm
• fast_gicp::LsqRegistration::is_converged
• fast_gicp::FastGICP::calculate_covariances
• fast_gicp::FastGICP::linearize
• fast_gicp::FastGICP::update_correspondences
• fast_gicp::FastGICP::compute_error

从上往下依次介绍函数的功能和调用关系。

pcl::Registration::align和fast_gicp::LsqRegistration::computeTransformation

pcl::Registration::align对点云进行一系列的初始化操作，同时调用computeTransformation函数估计位姿。
fast_gicp::LsqRegistration::computeTransformation迭代调用LM优化器 fast_gicp::LsqRegistration::step_lm直至收敛或者达到迭代次数上限。

fast_gicp::LsqRegistration::step_lm和fast_gicp::LsqRegistration::is_converged

LM流程如下图（来自METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS）所示，区别在于$A$，$g$和$F(X)$的计算。

收敛条件分为平移和旋转两个部分，平移向量绝对值的最大元素小于阈值同时旋转矩阵和单位矩阵差绝对值的最大元素也需要小于阈值。

fast_gicp::FastGICP::calculate_covariances

在computeTransformation中被调用计算协方差，基于kd-tree使用20个最近邻点计算协方差，由于采用平面对局部点云进行近似，将协方差SVD分解之后，将特征值改为$[1,1,ϵ]$。

fast_gicp::FastGICP::linearize

在step_lm中被调用计算$A$和$g$

fast_gicp::FastGICP::update_correspondences

在linearize中被调用构建配对关系计算信息矩阵。基于kd-tree在原点云和目标点云之间构建配对关系，根据点对的协方差矩阵计算匹配本身的协方差的逆${\Omega }_i=(C_i^B+T{C}_i^A{T}^T{)}^{-1}$。

fast_gicp::FastGICP::compute_error

在step_lm中被调用计算$F(x)$。对于每一个匹配点对，计算误差$e_i=b_i-T{a}_i$以及协方差修正之后的目标函数$E=\sum e_i^T\ast {\Omega }_i\ast e_i$

VGICP代码解析

VGICP是一种voxel-based distribution-to-multi-distribution算法，实现上继承自fast_gicp::FastGICP。大体框架和上一节的GICP类似，但是采用了基于Voxel的方法而不是基于KD-tree的方法来估计协方差，也因此在海森矩阵计算等方法存在差异。首先介绍Voxel相关操作，然后阐述VGICP算法和GICP算法在update_correspondences，linearize，compute_error三个函数中的不同。

GaussianVoxelMap类和相关函数

在设定的分辨率下，根据点云计算Voxel的参数。

create_voxelmap

将点云分配到Voexl中，对每个存在点云的Voxel计算点云分布的均值和协方差。

voxel_coord 和lookup_voxel

根据点云坐标计算对应Voxel和判断Voxel是否存在。

neighbor_offsets

设置Voxel的邻域搜索方式，也就是当前帧的一个点需要和前一帧的哪几个Voxel进行匹配，预设了三种模式DIRECT1，DIRECT7，DIRECT27 。DIRECT1为坐标对应Voxel，DIRECT7为对应Voxel和x，y，z方向的相邻Voxel，DIRECT27为以对应Voxel为中心的333的Voxel。

FastVGICP::update_correspondences

在linearize中被调用构建配对关系计算信息矩阵。基于Voxel构建distribution-to-multi-distribution的配对关系，根据Voxel的协方差矩阵计算匹配本身的协方差的逆${\Omega }_i=(C_i^B+T{C}_i^A{T}^T{)}^{-1}$。

FastVGICP::linearize和FastVGICP::compute_error

两个函数功能和GICP版本完全相同。和GICP的区别在于协方差的表示形式，和激光点对每个邻域Voxel都构成误差项，每对box的误差项由Voxel内点数量进行加权。