我在同一坐标系中有一个Source和target。源中有'n'个点，目标中有'n'个点(n>=3(。通信也是已知的。我想找到最佳的刚性变换矩阵(在某些情况下为6自由度或更小(。
我知道这是通过最小化源点和目标点之间距离的平方来解决的。
我有以下两个问题。
1( 在这些情况下，最好的解决方案是什么？2( 在使用四元数表示旋转的Levenberg–Marquardt算法的情况下，计算雅可比矩阵的最佳方法是什么？
给定点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是Ps的Qs和Pbar的平均值。
插入这个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是正交的procrusetes问题。当对R求解时，我们可以如上所述计算T。
当需要加权情况的解决方案时，修改很简单。首先，Pbar和Qbar应该是加权平均值，然后我们应该使用
q[i] = sqrt( weight[i]) * ( Q[i]-Qbar)
p[i] = sqrt( weight[i]) * ( P[i]-Pbar)