Kabsch算法求解变换矩阵RT

Kabsch算法求解变换矩阵RT

计算两组对应点之间的平移向量和旋转矩阵是一个很常见的问题,如下图,示意了针对3组对应点的变换。

上图中的对应点具有相同的颜色,我们希望找到最佳的旋转和平移,将数据集A中的点与数据集B对齐。这里,“最佳”或“最佳”是最小平方误差。这种变换有时被称为Euclidean或Rigid变换(欧几里得变换/刚体变换),因为它保留了形状和大小。这与仿射变换形成对比,仿射变换包括缩放和剪切。这个问题尤其出现在3D点云数据配准等任务中,其数据是从3D激光扫描仪或Kinect设备等硬件获得的。

其中是应用于数据集A的变换,以使其尽可能与数据集B对齐。找到最佳刚性变换矩阵可以分解为以下步骤:

  1. 找到两个数据集的质心
  2. 将两个数据集都移动到原点,然后计算旋转矩阵R
  3. 计算平移向量t

0. 平面方程向点坐标的转化

假设我们有三个平面的方程,即

解这个方程,就可以得到三个平面的交点的坐标

将平面方程中的平面法向量进行归一化(模长为1的向量),即

则可以通过原点坐标+向量,确定点的位置

1. Finding the centroids

这一步很简单,假设,则质心计算如下,其中分别是点云集中的点

2. Finding the optimal rotation

有几种方法可以在点之间找到最佳旋转,我发现最简单的方法是使用奇异值分解(SVD),因为它是一种在许多编程语言中广泛使用的函数(Matlab,Octave,C使用LAPACK,C ++使用OpenCV ……)

如果E是方阵,那么U、S、V将有同样的大小

为了找到最佳旋转,我们首先重新定位两个数据集,使两个质心都在原点,如下所示。

假设是进行质心平移后,代表的点的集合,是协方差矩阵

找到你必须要处理的旋转矩阵时有一个特例。有时SVD会返回一个“反射”矩阵,这在数值上是正确的,但在现实生活中实际上是无意义的。通过检查R的行列式(来自上面的SVD)并查看它是否为负(-1)来解决这个问题。如果是,那么V的第3列乘以-1。

3. Finding t

注意事项

  • 这个方法至少要求三个点,如果比三个点多,就要考虑使用最小二乘求解了
  • 这里的最小二乘是针对这些个对应点进行的优化,而不是针对原始的未经处理的三个平面的点云数据,如果是求出RT之后,以原始点云数据做ICP,效果可能会越来越差。
  • 对于平面方程有一些要求,比如符号一样,当然这个也可能是上边一段注释里提到的问题

参考文献

[1] http://nghiaho.com/?page_id=671

[2] https://blog.csdn.net/zizi7/article/details/88716429

文章作者:Jiadai Sun

最后更新:2019年09月25日 22:09:31

原始链接:https://sunjiadai.xyz/2019/06/24/Kabsch算法/

版权声明:本博客所有文章除特别声明外,均采用 BY-NC-SA 3.0 许可协议,转载请注明出处!


-----------本文结束-----------
0%