我在同一坐标系中有一个源和一个目标。源中有“ n”个点，目标中有“ n”个点（n> = 3）。对应关系也是已知的。 我想找到最佳的刚性变换矩阵（某些情况下小于6 DOF）。
我知道可以通过最小化源点和目标点之间的距离平方来解决。
我有以下两个问题。
1）在这些情况下，最佳的求解器是什么？ 2）如果四元数代表旋转的Levenberg-Marquardt算法，什么是计算Jacobian矩阵的最佳方法？
答案 0 :(得分：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)