用SVD方式实现halcon里的vector_to_rigid算子

halcon里vector_to_rigid是计算两组二维点之间的刚体变换关系(旋转加平移)，得到变换矩阵HomMat2D

vector_to_rigid( : : Px, Py, Qx, Qy : HomMat2D)

 最优化问题为

 取两组2d点

x1:=[19,549]
x2:=[19,663]
x3:=[275,547]
x4:=[274,664]

y1:=[267,1134]
y2:=[331,1159]
y3:=[429,558]
y4:=[496,585]

X1:=[x1[0],x2[0],x3[0],x4[0]]
Y1:=[x1[1],x2[1],x3[1],x4[1]]

X2:=[y1[0],y2[0],y3[0],y4[0]]
Y2:=[y1[1],y2[1],y3[1],y4[1]]

使用SVD方法求解最优化问题

x1:=[19,549]
x2:=[19,663]
x3:=[275,547]
x4:=[274,664]

y1:=[267,1134]
y2:=[331,1159]
y3:=[429,558]
y4:=[496,585]

X1:=[x1[0],x2[0],x3[0],x4[0]]
Y1:=[x1[1],x2[1],x3[1],x4[1]]

X2:=[y1[0],y2[0],y3[0],y4[0]]
Y2:=[y1[1],y2[1],y3[1],y4[1]]

*对P和Q去质心化处理
create_matrix (2, |X1|, [X1,Y1], P)
mean_matrix (P, 'rows', PMean)
create_matrix(1,|X1|,1,Ones)
mult_matrix (PMean, Ones, 'AB', PSub)
sub_matrix(P,PSub,PShift)

create_matrix(2,|X2|,[X2,Y2],Q)
mean_matrix(Q,'rows', QMean)
create_matrix (1, |X2|, 0, Ones)
mult_matrix (QMean, Ones, 'AB', QSub)
sub_matrix (Q, QSub, QShift)

*得到步骤2里的W矩阵,这里是2维点，左右W是个2*2矩阵
create_matrix (2, 2, 0, W)
for Index := 0 to |X1|-1 by 1
    get_sub_matrix (PShift, 0, Index, 2, 1, PVec)
    get_sub_matrix (QShift, 0, Index, 2, 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(2,3,0,HomMat2DID)
set_sub_matrix (HomMat2DID, R, 0, 0)
set_sub_matrix(HomMat2DID, t, 0, 2)
get_full_matrix (HomMat2DID, HomMat2D1)


vector_to_rigid(X1, Y1, X2, Y2, HomMat2D2)

用SVD得到的HomMat2D1:

[0.279977, 0.960007, -241.861, -0.960007, 0.279977, 830.285]

用halcon算子vector_to_rigid得到的HomMat2D2：

[0.279977, 0.960007, -241.861, -0.960007, 0.279977, 830.285]

一模一样！