点云配准--gicp原理与其在pcl中的使用

总结：gicp引入了概率信息（使用协方差阵），提出了icp的统一模型，既可以解释点到点和点到面的icp，也在新模型理论的基础上，提出了一种面到面的icp。
论文原文：《Generalized-ICP》

gicp统一模型（Generalized-ICP）

在概率模型中假设存在配准中两个点集, $\hat{A}=\left\{\widehat{a_i}\right\}$ and $\hat{B}=\left\{\widehat{b_i}\right\}$,并且假设 $A$ and $B$ 分别服从 $a_i\sim \mathcal{N}\left(\widehat{a_i},C_i^A\right)$and $b_i\sim \mathcal{N}\left({\hat{b}}_i,C_i^B\right)$正态分布. $\left\{C_i^A\right\}$ and $\left\{C_i^B\right\}$ 分别是点对应的协方差阵. 我们假设以及匹配完成，假设变换矩阵为 ${\mathbf{T}}^{\ast }$, 因此：
$${\hat{b}}_i={\mathbf{T}}^{\ast }{\hat{a}}_i\text{\hspace{1em}(1)}$$
对于变换矩阵, $\mathbf{T}$ , 定义误差量 $ d_{i}^{(\mathbf{T})}= b_{i}-\mathbf{T} a_{i} $, 因为假设 $ a_{i} $ and $ b_{i} $ 服从正态分布（NDT也是假设服从正态分布）， 因此 $d_i^{\left({\mathrm{T}}^{\ast }\right)}$ 也服从正态分布：
$$\begin{array}{rl}{d}_i^{\left({\mathbf{T}}^{\ast }\right)}& \sim \mathcal{N}\left({\hat{b}}_i-\left({\mathbf{T}}^{\ast }\right){\hat{a}}_i,C_i^B+\left({\mathbf{T}}^{\ast }\right)C_i^A{\left({\mathbf{T}}^{\ast }\right)}^T\right)\\ & =\mathcal{N}\left(0,C_i^B+\left({\mathbf{T}}^{\ast }\right)C_i^A{\left({\mathbf{T}}^{\ast }\right)}^T\right)\end{array}\text{\hspace{1em}(2)}$$

使用最大似然估计（ MLE）计算 $\mathbf{T}$ ：
$$\mathbf{T}=\underset{\mathbf{T}}{\mathrm{argmax}}\prod _{i}p\left(d_i^{(\mathrm{T})}\right)=\underset{\mathbf{T}}{\mathrm{argmax}}\sum _i\log \left(p\left(d_i^{(\mathrm{T})}\right)\right)\text{\hspace{1em}(3)}$$

进一步简化为：（这里从最大似然估计推导，具体过程需要研究）
$$\mathbf{T}=\underset{\mathrm{T}}{\mathrm{argmin}}\sum _i{d}_i^{(\mathbf{T}{)}^T}{\left(C_i^B+\mathbf{T}{C}_i^A{\mathbf{T}}^T\right)}^{-1}{d}_i^{(\mathbf{T})}\text{\hspace{1em}(4)}$$

当：
$$\begin{gathered} C_i^B=I \\ {C}_i^A=0\text{\hspace{1em}(5)} \end{gathered}$$
就得到标准ICP:
$$\begin{array}{rl}\mathbf{T}& =\underset{\mathbf{T}}{\mathrm{argmin}}\sum _i{d}_i^{(\mathrm{T}{)}^T}{d}_i^{(\mathrm{T})}\\ & =\underset{\mathbf{T}}{\mathrm{argmin}}\sum _i{\Vert d_i^{(\mathrm{T})}\Vert }^2\end{array}\text{\hspace{1em}(6)}$$
当：
$$\begin{array}{rl}{C}_i^B& ={\mathbf{P}}_{\mathbf{i}}^{-1}\\ C_i^A& =0\end{array}\text{\hspace{1em}(7)}$$
得到点到面的ICP:
$$\mathbf{T}=\underset{\mathbf{T}}{\mathrm{argmin}}\left\{\sum _i{\Vert {\mathbf{P}}_{\mathbf{i}}\cdot d_i\Vert }^2\right\}\text{\hspace{1em}(8)}$$

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

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

因为实际上法向量并不一定是沿x轴方向，所以需要进行坐标转换。假设$b_i,a_i$对应的法向量分别为$u_i,v_i$,则它们对应的协方差阵为：
$$\begin{array}{l}{C}_i^B={\mathbf{R}}_{{\mu }_i}\cdot \left(\begin{array}{ccc}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\cdot {\mathbf{R}}_{{\mu }_i}^T\\ C_i^A={\mathbf{R}}_{{\nu }_i}\cdot \left(\begin{array}{ccc}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\cdot {\mathbf{R}}_{{\nu }_i}^T\end{array}$$
${\mathbf{R}}_{{\nu }_i}$为e1到vi旋转矩阵。
上述协方差计算过程可以表述如下图,链接2：

确定协方差阵后利用公式（4）即为plane to plane ICP或者叫GICP。
这里其实就是怎么确定协方差阵。
显然可以通过pca计算协方差阵（代替上述求解过程）：
$$C=\frac{1}{N}\cdot \sum _{i=1}^N\cdot \left(p_i-\bar{p}\right)\cdot {\left(p_i-\bar{p}\right)}^T$$
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;
}