ICP SVD 解法

整体流程

1. 有两组点，已经配好对，分别为：，每个点是一个列向量
2. 求中心，得到
3. 去中心，得到
4. 求旋转 R，依据优化公式：
   
5. 求平移 t，依据公式：
   

优化结果

1. 计算3x3 矩阵 H（如果是三维点云）
   

2. SVD 分解 H
   
   其中 U V 均为 3x3 正交矩阵

3. 得到最优 ：
   

优化推导

1. 展开
   
   用到的理论：

• R 为正交矩阵

2. 去除与 无关项，优化问题等价于：
   

3. 使用迹
   
   用到的理论：

4. 使用 3x3 H 矩阵化简：
   
   现在的问题就是，找到一个R，使RH的迹最大。

5. 使用辅助旋转矩阵 B：
   上述问题等价于
   
   B 为一个正交矩阵。

6. 有如下理论：
   
   （可由柯西不等式证明）
   则问题转化为，求一个，使可以表示为的形式

7. SVD 分解H
   
   当 时，
   令
   有
   因此优化结果为 。

相关证明

https://blog.csdn.net/zhouyelihua/article/details/77807541