问题描述
我正在 2D 中实现 Kabsch algorithm。算法的结果是：平移、旋转、缩放。组合它们并应用在第一组点上应该将它们与第二组点“对齐”。 到目前为止，我的实施（进行中）：
// maps points 'A' onto points 'B'
public static (Vector2,Matrix2x2,float) Compute( List< Vector2 > pointsA,List< Vector2 > pointsB )
{
// scale : ratio of sums of "some" distances,e.g. to "neighbour" or to centroid...
var dsumA = 0f;
var dsumB = 0f;
for( var i = num; i --> 1; )
{
dsumA += ( pointsA[ i ] - pointsA[ i-1 ] ).sqrMagnitude;
dsumB += ( pointsB[ i ] - pointsB[ i-1 ] ).sqrMagnitude;
}
if( dsumA == 0f || dsumB == 0f )
return (Vector2.zero,Matrix2x2.IDENTITY,1f);
var scale = MathF32.Sqrt( dsumB / dsumA );
// rotation : based on singular-value-decomposition of cross-covariance matrix of given point-sets...
var centroidA = Vector2.zero;
var centroidB = Vector2.zero;
for( int i = num; i --> 0; ) // centroids
{
centroidA += pointsA[ i ];
centroidB += pointsB[ i ];
}
var invNum = 1f / num;
centroidA *= invNum;
centroidB *= invNum;
var cc = new Matrix2x2();
for( var i = num; i --> 0; ) // cross-covariance matrix : transposed( move-to-origin( A ) ) * move-to-origin( B )
{
var a = pointsA[ i ] - centroidA;
var b = pointsB[ i ] - centroidB;
cc.m00 += a.x * b.x; cc.m01 += a.x * b.y;
cc.m10 += a.y * b.x; cc.m11 += a.y * b.y;
}
(var u,_,var v) = cc.SingularValueDecomposition();
u.Transpose();
if( cc.GetDeterminant() < 0f ) // handling special "reflection" case
{
v.m01 *= -1f;
v.m11 *= -1f;
}
var rotation = v * u;
// transition : difference of centroids...
var translation = centroidB - rotation * ( centroidA * scale );
// result...
return (translation,rotation,scale);
}
要计算旋转矩阵，您需要计算协方差矩阵的 SVD（奇异值分解）。我在数学方面根本不“流利”:(所以我尝试了一些 SVD 实现 (svd(A) = U * S * V^T) 意思是 here。
我将“借用”实现的结果与几个在线 SVD 计算器进行了比较。矩阵 S 没问题但 U 和 V 中的几乎所有条目都有不同的符号。我尝试了 2 或 3 种不同的实现，所以我假设 这个问题在其他地方 - 可能是在构建协方差矩阵 :(，但我无法发现错误。
- Kabsh 本身的实现有什么注意事项吗？
- 或者如何修复 SVD（U 和 V 中的那些错误符号）？
我注意到协方差矩阵的行列式为负。会是这个吗？
非常感谢您的建议。
更新 #1
对于 SVD，我使用以下实现（对于其他实现，我得到了小错误 - 最终旋转几度）。
public (Matrix2x2,Matrix2x2) SingularValueDecomposition()
{
Matrix2x2 u;
Matrix2x2 s;
Matrix2x2 v;
s.m00 = ( MathF32.Hypot( m00 - m11,m01 + m10 ) +
MathF32.Hypot( m00 + m11,m01 - m10 ) ) * 0.5f;
s.m01 = 0f;
s.m10 = 0f;
s.m11 = MathF32.Abs( s.m00 - MathF32.Hypot( m00 - m11,m10 + m01 ));
v.m10 = ( s.m00 > s.m11 ) ? MathF32.Sin(( MathF32.atan2( 2f * ( m00 * m01 + m10 * m11 ),m00 * m00 - m01 * m01 + m10 * m10 - m11 * m11 )) / 2f ) : 0f;
v.m00 = MathF32.Sqrt( 1f - v.m10 * v.m10 );
v.m01 = -v.m10;
v.m11 = +v.m00;
u.m00 = s.m00 != 0f ? ( m00 * v.m00 + m01 * v.m10 ) / s.m00 : 1f;
u.m10 = s.m00 != 0f ? ( m10 * v.m00 + m11 * v.m10 ) / s.m00 : 0f;
u.m01 = s.m11 != 0f ? ( m00 * v.m01 + m01 * v.m11 ) / s.m11 : -u.m10;
u.m11 = s.m11 != 0f ? ( m10 * v.m01 + m11 * v.m11 ) / s.m11 : +u.m00;
return (u,s,v);
}
然而，U 和 V 中的那些符号仍然与我从几个在线 SVD 计算器得到的分解不同。
更新 #2
我能够修正旋转（旋转角度）以正确对齐点，但“修正”因输入点集而异。 有时有助于 180 度角。有时 360 度角...对于某些点集甚至 180 度以上的角度 :(。提到/校正的角度是从计算的“旋转”矩阵中提取的。
到目前为止我尝试过的所有 SVD 实现（3 or 4）都需要这种“更正”，所以 我“开始”认为问题出在其他地方...
平移和缩放接缝以正常工作。通过“固定”旋转，我可以正确地将“A”与“B”对齐。
更新 #3
我通过检查 U * S * Vt 是否得到原始协方差矩阵来测试 SVD 是否有效。
我在设置协方差矩阵时发现了一个问题（已修复，上面的代码已更新），但仍然出现旋转错误:(。 我也修复了翻译。
啊哈...这么简单的算法，我还是无法击败它:)
更新 #4...最后一个
对不起大家，我终于找到问题了:)。在某些时候（在我的测试应用程序中），其中一个输入集中的点顺序发生了变化。我纠正了它，它“神奇地”开始工作。我为后代更新/完善了上面的代码:)
解决方法
暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！
如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@）