Point cloud registration -- GICP principle and its application in PCL

summary ：gicp Probability information is introduced （ Use covariance matrix ）, Put forward icp The first mock exam , It can explain point to point and point to face icp, On the basis of the new model theory , A face to face icp.
Original paper ：《Generalized-ICP》

gicp The first mock exam （Generalized-ICP）

In the probability model, it is assumed that there are two point sets in the registration , $\hat{A}=\left\{\widehat{a_i}\right\}$ and $\hat{B}=\left\{\widehat{b_i}\right\}$, And suppose $A$ and $B$ Obey separately $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)$ Normal distribution . $\left\{C_i^A\right\}$ and $\left\{C_i^B\right\}$ They are the covariance matrix corresponding to the points . We assume that the matching is complete , Suppose the transformation matrix is ${\mathbf{T}}^{\ast }$, therefore ：
$${\hat{b}}_i={\mathbf{T}}^{\ast }{\hat{a}}_i\text{\hspace{1em}(1)}$$
For transformation matrix , $\mathbf{T}$ , Define the amount of error $ d_{i}^{(\mathbf{T})}= b_{i}-\mathbf{T} a_{i} $, Because of the assumption $ a_{i} $ and $ b_{i} $ It follows a normal distribution （NDT It is also assumed that the distribution is normal ）, therefore $d_i^{\left({\mathrm{T}}^{\ast }\right)}$ It also follows a normal distribution ：
$$\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)}$$

Using maximum likelihood estimation （ MLE） Calculation $\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)}$$

Further simplified to ：（ Here we derive from the maximum likelihood estimation , The specific process needs to be studied ）
$$\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)}$$

When ：
$$\begin{gathered} C_i^B=I \\ {C}_i^A=0\text{\hspace{1em}(5)} \end{gathered}$$
Get the standard 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)}$$
When ：
$$\begin{array}{rl}{C}_i^B& ={\mathbf{P}}_{\mathbf{i}}^{-1}\\ C_i^A& =0\end{array}\text{\hspace{1em}(7)}$$
Get point to face 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： Relative to point-to-point and point-to-face, the probability model is added （ Covariance matrix ）)

The point to plane algorithm is , Suppose that the point cloud has planar features , This means that 3D Spatial processing sampling 2D manifold .
Because the real world surface is at least piecewise differentiable , We can assume that our data set is locally planar . Besides , Because we sample manifolds from two different angles , Therefore, exactly the same points are usually not sampled （ namely , Correspondence can never be exact ）.
Essentially , Each measurement point provides only constraints along its surface normals . In order to model this structure , We consider that each sampling point is distributed with high covariance along its local plane , And in the direction of the surface normal （ Perpendicular to the plane ） With very low covariance distribution （ That is, the point cloud is distributed on the local plane ）. Suppose we locally fit the normal vector of a point on the plane e1 Is the edge X The shaft , link 1, Then the covariance matrix at this point becomes ：
$$\left(\begin{array}{lll}ϵ& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)\text{\hspace{1em}(9)}$$
$ϵ$ Is a very small constant along the normal direction .

Because in fact, the normal vector is not necessarily along x Axis direction , Therefore, coordinate conversion is required . hypothesis $b_i,a_i$ The corresponding normal vectors are $u_i,v_i$, Then their corresponding covariance matrix is ：
$$\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}$ by e1 To vi Rotation matrix .
The above covariance calculation process can be expressed as shown in the figure below , link 2：

After determining the covariance matrix, use the formula （4） That is to say plane to plane ICP Or call it GICP.
This is actually how to determine the covariance matrix .
Obviously it can be done through pca Calculate the covariance matrix （ Instead of the above solution process ）：
$$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 Pay attention to the formula when solving （4） There is an inverse process for covariance , Note that when the covariance matrix is singular , Replace with a small amount 0 value （ similar NDT Medium handling ）.

PCL in GICP Code application

#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;
}