文章目录

• gicp统一模型（Generalized-ICP）
• plane to plane ICP(gicp：相对于点到点和点到面加入概率模型（协方差阵）)
• PCL中GICP代码应用

gicp统一模型（Generalized-ICP）

在概率模型中假设存在配准中两个点集, and ,并且假设 and 分别服从 and 正态分布. and 分别是点对应的协方差阵. 我们假设以及匹配完成，假设变换矩阵为 , 因此：

对于变换矩阵, , 定义误差量 , 因为假设 and 服从正态分布（NDT也是假设服从正态分布）， 因此 也服从正态分布：

使用最大似然估计（ MLE）计算 ：

进一步简化为：（这里从最大似然估计推导，具体过程需要研究）

当：

就得到标准ICP:

当：

得到点到面的ICP:

plane to plane ICP(gicp：相对于点到点和点到面加入概率模型（协方差阵）)

点到平面算法的做法是，假设点云具有平面特征，这意味着在3D空间处理采样2D流形。
由于现实世界的曲面至少是分段可微的，我们可以假设我们的数据集是局部平面的。此外，由于我们从两个不同的角度对流形进行采样，因此通常不会对完全相同的点进行采样（即，对应关系永远不会是精确的）。
本质上，每个测量点仅提供沿其曲面法线的约束。为了对这种结构进行建模，我们考虑每个采样点沿其局部平面以高协方差分布，而在曲面法线方向（垂直于平面方向）以极低协方差分布（即点云分布在局部平面上）。假设局部拟合平面上某一点的法向量e1是沿X轴的,链接1，则该点协方差矩阵变为：

是沿着法线方向极小的常数。

因为实际上法向量并不一定是沿x轴方向，所以需要进行坐标转换。假设对应的法向量分别为,则它们对应的协方差阵为：

为e1到vi旋转矩阵。
上述协方差计算过程可以表述如下图,链接2：

确定协方差阵后利用公式（4）即为plane to plane ICP或者叫GICP。
这里其实就是怎么确定协方差阵。
显然可以通过pca计算协方差阵（代替上述求解过程）：

pca求解时要注意公式（4）对协方差有个求逆过程，需要注意当协方差阵奇异时，用微小量替代0值（类似NDT中处理方式）。

PCL中GICP代码应用

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/gicp.h>
int gicp(const pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud, 
	const pcl::PointCloud<pcl::PointXYZ>::Ptr tgt_cloud, 
	 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_source)
{

	pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
	gicp.setInputSource(src_cloud);
	gicp.setInputTarget(tgt_cloud);
	//gicp.setMaximumIterations(max_iter);
	gicp.align(*transformed_source);
	return 1;
}