从3组对应点中求得最佳的旋转和平移变换

 问题陈述：已知：上图中颜色相同的点为两个点云之间的对应点，现在要利用3组对应点，来求解点云配准的最佳旋转、平移矩阵。这个变换叫做刚性变换或者欧式变换。

也就是要求解R，t：

B=R*A+t；

其中R，t是应用于数据集A的变换，以使其与数据集B对齐，尽可能最佳。

找到最佳刚性变换矩阵可以分解为以下步骤：

1. 找到两个点集的重心；
2. 将两个点集的重心都带到原点，然后找到最佳旋转矩阵R；
3. 找到平移矩阵t

一、找到点云的重心

平移到原点是为了去除平移的影响，只考虑旋转。 

 

二、求解R

之后就是使用奇异值分解去求解，旋转矩阵R。 

其中，H是协方差矩阵。

 三、求解t

平移矩阵

 完成！

注意：只要存在至少3个点，所提出的解决方案就可以用于任何大小的数据集。当有超过3个点时，获得最小二乘解，使得以下误差最小化：

 在这里，操作符|| || 是两个向量之间的欧几里德距离，即标量值。err只是两个数据集中各点之间的平方距离误差。