报童问题求解最大利润_SVD求解ICP问题

下图是旋转并平移前后的两个坐标系，假设右侧坐标系变换到左侧坐标系的旋转矩阵是

，平移向量是

。四个黑点代表四个坐标，用

表示点在左边坐标系中的坐标，用

表示点在右边坐标系中的坐标。

把右侧坐标系中的坐标变换到左侧坐标系：

，那么它们的误差

利用多对点构造最小二乘：

，使误差最小的

和

就是最优的旋转矩阵和平移向量。

首先定义质心:

（使用质心坐标是为了将最小二乘中的

分开求解。

那么

其中

在公式中，最外层小括号内的第一项只与旋转矩阵

有关，第二项与

和平移向量

都相关。可以令第一项为0求得

，再令第二项为0求得

。

1.1 求旋转矩阵

求

​的去质心坐标

：

则根据上面公式的第一项，有：

其中使用到的理论有：

令

，则问题转化为求一个

使

的迹最大

又

，所以当

时，有

，此时的迹可以取得最大值。

将H进行SVD分解

，当

时，

，令

，则

故有：

1.2 求平移向量

参考资料：

SLAM第3章：求解位姿变换的SVD方法(3D-3D)_sinoai-CSDN博客

北麓牧羊人：ICP 的 SVD 解法

利用SVD的方法求解ICP（详细推导）_zhouyelihua-CSDN博客

git 代码：（待更新）

附录：证明

是A的第i列，有

其中，