【SVD求解两组多维点之间的欧式变换矩阵】

---

本文介绍了如何利用SVD奇异值分解求解两组3D点之间的最优旋转R和平移t，以最小化点集变换后的误差。首先对点集进行质心处理，然后构建矩阵W并进行SVD分解，通过调整得到旋转矩阵R和向量t，最终形成变换矩阵。Halcon代码示例展示了具体实现过程。

*两组3d点P和Q,每组4个点
 
PX := [0.2,0.4,0.2,0.3]
PY := [0.4,0.6,0.8,0.6]
PZ := [0.6,0.8,0.6,0.5]
QX := [0.25,0.44,0.61,0.3]
QY := [0.32,0.56,0.82,0.4]
QZ := [0.4,0.18,0.6,0.51]
 
 
*对P和Q去质心化处理
create_matrix (3, |PX|, [PX,PY,PZ], P)
mean_matrix (P, 'rows', PMean)
create_matrix(1,|PX|,1,Ones)
mult_matrix (PMean, Ones, 'AB', PSub)
sub_matrix(P,PSub,PShift)
 
create_matrix(3,|QX|,[QX,QY,QZ],Q)
mean_matrix(Q,'rows', QMean)
create_matrix (1, |QX|, 0, Ones)
mult_matrix (QMean, Ones, 'AB', QSub)
sub_matrix (Q, QSub, QShift)
 
*得到步骤2里的W矩阵,这里是3维点，左右W是个3*3矩阵
create_matrix (3, 3, 0, W)
for Index := 0 to |PX|-1 by 1  
    get_sub_matrix (PShift, 0, Index, 3, 1, PVec)
    get_sub_matrix (QShift, 0, Index, 3, 1, QVec)
    transpose_matrix_mod (QVec)
    mult_matrix (PVec, QVec, 'AB', PQ)
    add_matrix_mod (W, PQ)    
endfor
 
*对W进行svd分解
svd_matrix (W, 'full', 'both', U, S, V)
 
 
*计算R
transpose_matrix_mod(U)
mult_matrix (V, U, 'AB', R)
 
*计算R的行列式是否为1
determinant_matrix (R, 'general', Value)
if (Value < 0)
    get_value_matrix (V, [0,1,2], [2,2,2], Value1)
    set_value_matrix (V, [0,1,2], [2,2,2], [-Value1[0],-Value1[1],-Value1[2]])
    mult_matrix (V, U, 'AB', R)
endif
 
*计算t
mult_matrix (R, PMean, 'AB', RPMean)
sub_matrix(QMean,RPMean,t)
 
*得到最后的变换矩阵3*4
create_matrix(3,4,0,HomMat3DID)
set_sub_matrix (HomMat3DID, R, 0, 0)
set_sub_matrix(HomMat3DID, t, 0, 3)
get_full_matrix (HomMat3DID, HomMat3D)