SVD求解两组多维点之间的欧式变换矩阵，及halcon代码实现

之前研究了二维点的仿射变换，用解矩阵的方式求解了两组二维点之间的变换矩阵。

学习了下SVD，看到可以用SVD求解两组多维点之间的欧式变换矩阵，当然也是个最优化问题。

这里的变换只有平移和旋转，没有缩放。

一、先说结论：

现在有两组点(2d,3d,或者多维都可以）,

 需要找到R和t，使得

一般没有完美解，需要找到最优解R和t，使得P'变换后的点和P误差最小。

操作步骤：

1，,求两组点质心位置,得到两组点去质心坐标

2,得到矩阵W

3,对W进行奇异值分解

4.得到最优R和T

二、用halcon代码来实现下

举例测试两组点P和Q

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]

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)

得到的变换矩阵为

[-0.65053, 0.436583, 0.621455, -0.0714635, 0.519911, 0.852471, -0.0546402, -0.095308, -0.553627, 0.287556, -0.781542, 0.890677]

三、证明过程

    证明过程内容有点多，晚点整理整理再写

参考文章

三维重建（4）之SVD求解三维变换矩阵Rt（旋转+平移）_svd分解求旋转平移矩阵_明月醉窗台的博客-CSDN博客

使用SVD来求解优化问题最优值 - 知乎