问题陈述:已知:上图中颜色相同的点为两个点云之间的对应点,现在要利用3组对应点,来求解点云配准的最佳旋转、平移矩阵。这个变换叫做刚性变换或者欧式变换。
也就是要求解R,t:
B=R*A+t;
其中R,t是应用于数据集A的变换,以使其与数据集B对齐,尽可能最佳。
找到最佳刚性变换矩阵可以分解为以下步骤:
- 找到两个点集的重心;
- 将两个点集的重心都带到原点,然后找到最佳旋转矩阵R;
- 找到平移矩阵t
一、找到点云的重心
平移到原点是为了去除平移的影响,只考虑旋转。
二、求解R
之后就是使用奇异值分解去求解,旋转矩阵R。
其中,H是协方差矩阵。
三、求解t
平移矩阵
完成!
注意:只要存在至少3个点,所提出的解决方案就可以用于任何大小的数据集。当有超过3个点时,获得最小二乘解,使得以下误差最小化:
在这里,操作符|| || 是两个向量之间的欧几里德距离,即标量值。err只是两个数据集中各点之间的平方距离误差。