根据给定的点对找到最佳的刚性变换

Question

我在同一坐标系中有一个源和一个目标。源中有“ n”个点，目标中有“ n”个点（n> = 3）。对应关系也是已知的。 我想找到最佳的刚性变换矩阵（某些情况下小于6 DOF）。

我知道可以通过最小化源点和目标点之间的距离平方来解决。

我有以下两个问题。

1）在这些情况下，最佳的求解器是什么？ 2）如果四元数代表旋转的Levenberg-Marquardt算法，什么是计算Jacobian矩阵的最佳方法？

Answer 1

给出点P []和对应点Q []，我们想找到平移T和旋转R以最小化

E = Sum{ <Q[i] - (R*P[i]+T)|Q[i] - (R*P[i]+T)>}

但这是

E = Sum{ <Q[i] - R*P[i] - T | Q[i] - R*P[i] - T>}

在给定R的情况下，使该值最小的T值为

T = mean { Q[i] - R*P[i]} = Qbar - R*Pbar

其中Qbar是Qs的平均值，而Pbar是Ps的平均值。

插入这个T值，我们得到

E = Sum{ <q[i] - R*p[i] | q[i] - R*p[i]>}
where q[i] = Q[i]-Qbar, p[i] = P[i]-Pbar

找到R以最小化E为orthogonal procrusetes problem。解决了R问题后，我们可以按上述方法计算T。

想要加权情况的解决方案时的修改很简单。首先，Pbar和Qbar应该是加权平均值，然后我们应该使用

q[i] = sqrt( weight[i]) * ( Q[i]-Qbar)
p[i] = sqrt( weight[i]) * ( P[i]-Pbar)