本文仅仅分析了一下两组点云做好匹配好了之后， 如何解算RT， 并不涉及匹配过程，详细的匹配，下次再出一篇博客

ICP 求解旋转矩阵 ( R ) 的步骤

给定两个点云集合 ( P = {p_1, p_2, …, p_n} )（源点云）和 ( Q = {q_1, q_2, …, q_n} )（目标点云），ICP 的目标是通过找到旋转矩阵 ( R ) 和平移向量 ( t )，使得源点云在经过刚体变换后，最小化与目标点云之间的距离平方和：

$$E(R,t)=\sum _{i=1}^n\Vert q_i-(R{p}_i+t){\Vert }^2$$

其中：

• ( R $\in$ SO(3) ) 是 3D 旋转矩阵。
• ( t $\in$ $\mathbb{R}$ ) 是 3D 平移向量。

求解旋转矩阵 ( R ) 的步骤

1. 计算质心

计算两个点云集合的质心 ( ${\mu }_P$) 和 ( ${\mu }_Q$)：
$${\mu }_P=\frac{1}{n}\sum _{i=1}^n{p}_i,{\mu }_Q=\frac{1}{n}\sum _{i=1}^n{q}_i$$

2. 去质心

对每个点都减去相应的质心，得到去质心后的点集 ( P’ ) 和 ( Q’ )：
$$p_i^{\prime }=p_i-{\mu }_P,q_i^{\prime }=q_i-{\mu }_Q$$

3. 计算协方差矩阵 ( H )

这里可以解释一下为什么求解协方差矩阵，原因参考的SLAM 14讲的第七章

使用去质心后的点集 ( P’ ) 和 ( Q’ )，构建协方差矩阵 ( H )，注意是每对点的外积的和：
$$H=\sum _{i=1}^n{p}_i^{\prime }{q_i^{\prime }}^T$$

此处 ( H ) 是 3x3 的矩阵，它包含了两个点云之间的协方差信息。

4. SVD 分解

对协方差矩阵 ( H ) 进行奇异值分解：
$$H=U\Sigma V^T$$

其中：

• ( U ) 和 ( V ) 是 3x3 的正交矩阵。
• ( $\Sigma$) 是 3x3 的对角矩阵，包含奇异值。

5. 计算旋转矩阵 ( R )

旋转矩阵 ( R ) 可以通过 SVD 的结果求得：
$$R=V{U}^T$$

但是，如果 ( $\det (R)=-1$)，则说明产生了反射矩阵，此时需要对 ( V ) 的最后一列取反：
$$R=V\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]U^T$$

平移向量 ( t )

旋转矩阵 ( R ) 求解完毕后，平移向量 ( t ) 可以通过以下公式计算：
$$t={\mu }_Q-R{\mu }_P$$

目标函数总结

通过最小化目标函数 ( E(R, t) )，我们可以逐步通过迭代方式求解旋转矩阵 ( R ) 和平移向量 ( t )，使得源点云经过变换后与目标点云对齐。这就是 ICP 算法求解刚体变换的核心步骤。