记求刚性变换矩阵

无论是halcon还是python一样的数据输入方式

import numpy as np
from math import sqrt 
from math import fabs as Abs	
doc=App.newDocument("变换矩阵验证")
boxbf=doc.addObject("Part::Box","boxbf")
boxbf.Label = "之前"
boxbf.Width = 1
boxaf=doc.addObject("Part::Box","boxaf")
boxaf.Label = "之后"
boxaf.Width = 1
boxaf.Placement = App.Placement(App.Vector(3,4,5),App.Rotation(App.Vector(1,1,1),45))
doc.recompute()
def rigid_transform_3D(bf, af):
    assert bf.shape == af.shape and bf.shape[0] == 3 and bf.shape[1] == 3    
    centroid_bf = np.mean(bf, axis=1).reshape((3, 1))# 计算质心 axis=1 按列
    centroid_af = np.mean(af, axis=1).reshape((3, 1))    
    bf_centered = bf - centroid_bf# 去中心化
    af_centered = af - centroid_af    
    H = np.dot(bf_centered, af_centered.T)# 构建协方差矩阵 H    
    U, S, Vt = np.linalg.svd(H)# 通过SVD求解旋转矩阵R
    R = np.dot(Vt.T, U.T)   
    if np.linalg.det(R) < 0: # 特殊处理反射情况（确保行列式为正）
        Vt[2, :] *= -1
        R = np.dot(Vt.T, U.T)   
    t = centroid_af - np.dot(R, centroid_bf) # 计算平移向量t
    return R, t.flatten()
bfpart = doc.getObject("boxbf")
bfvtx1 = bfpart.getSubObject("Vertex1")
bfvtx2 = bfpart.getSubObject("Vertex5")
bfvtx3 = bfpart.getSubObject("Vertex6")

afpart = doc.getObject("boxaf")
afvtx1 = afpart.getSubObject("Vertex1")
afvtx2 = afpart.getSubObject("Vertex5")
afvtx3 = afpart.getSubObject("Vertex6")
if 1==1:
	#显示以上6点
	bfsp1=App.ActiveDocument.addObject("Part::Sphere","bfsp1")
	bfsp1.Radius =0.5
	bfsp1.Placement.Base=App.Vector(bfvtx1.X, bfvtx1.Y, bfvtx1.Z)
	bfsp1.ViewObject.ShapeColor = (255,0,0)
	bfsp2=App.ActiveDocument.addObject("Part::Sphere","bfsp2")
	bfsp2.Radius =0.5
	bfsp2.Placement.Base=App.Vector(bfvtx2.X, bfvtx2.Y, bfvtx2.Z)
	bfsp2.ViewObject.ShapeColor = (0,255,0)
	bfsp3=App.ActiveDocument.addObject("Part::Sphere","bfsp3")
	bfsp3.Radius =0.5
	bfsp3.Placement.Base=App.Vector(bfvtx3.X, bfvtx3.Y, bfvtx3.Z)
	bfsp3.ViewObject.ShapeColor = (0,0,255)
	#
	afsp1=App.ActiveDocument.addObject("Part::Sphere","afsp1")
	afsp1.Radius =0.5
	afsp1.Placement.Base=App.Vector(afvtx1.X, afvtx1.Y, afvtx1.Z)
	afsp1.ViewObject.ShapeColor = (255,0,0)
	afsp2=App.ActiveDocument.addObject("Part::Sphere","afsp2")
	afsp2.Radius =0.5
	afsp2.Placement.Base=App.Vector(afvtx2.X, afvtx2.Y, afvtx2.Z)
	afsp2.ViewObject.ShapeColor = (0,255,0)
	afsp3=App.ActiveDocument.addObject("Part::Sphere","afsp3")
	afsp3.Radius =0.5
	afsp3.Placement.Base=App.Vector(afvtx3.X, afvtx3.Y, afvtx3.Z)
	afsp3.ViewObject.ShapeColor = (0,0,255)
#bf = np.array([[bfvtx1.X, bfvtx1.Y, bfvtx1.Z],#错误的方式
#              [bfvtx2.X, bfvtx2.Y, bfvtx2.Z],
#              [bfvtx3.X, bfvtx3.Y, bfvtx3.Z]])
bf = np.array([[bfvtx1.X, bfvtx2.X, bfvtx3.X],#正确的方式
              [bfvtx1.Y, bfvtx2.Y, bfvtx3.Y],
              [bfvtx1.Z, bfvtx2.Z, bfvtx3.Z]])
af = np.array([[afvtx1.X, afvtx2.X, afvtx3.X],
              [afvtx1.Y, afvtx2.Y, afvtx3.Y],
              [afvtx1.Z, afvtx2.Z, afvtx3.Z]])
R, T = rigid_transform_3D(bf, af)
print("旋转矩阵 R:",R)
print("平移向量 T:",T)
mat=App.Matrix(R[0][0],R[0][1],R[0][2],T[0],R[1][0],R[1][1],R[1][2],T[1],R[2][0],R[2][1],R[2][2],T[2],0.0,0.0,0.0,1.0,)
print("刚性变换矩阵Mat",mat)
boxchk=doc.addObject("Part::Box","boxchk")
boxchk.Label = "验证"
boxchk.Width = 1
bfptx,bfpty,bfptz=0,0,0
bfsp=App.ActiveDocument.addObject("Part::Sphere","bfsp")
bfsp.Radius =0.5
bfsp.Placement.Base=App.Vector(bfptx,bfpty,bfptz)
bfsp.ViewObject.ShapeColor = (255,255,255)
bfvec=App.Vector(0,0,0)
afvec=mat*bfvec
afptx,afpty,afptz= afvec.x,afvec.y,afvec.z
afsp=App.ActiveDocument.addObject("Part::Sphere","afsp")
afsp.Radius =0.5
afsp.Placement.Base=App.Vector(afptx,afpty,afptz)
afsp.ViewObject.ShapeColor = (0,0,0)
print("afptx,afpty,afptz",afptx,afpty,afptz) #1.4987374399667093 5.26531491772512 2.405760295911869
boxchk.Placement=mat 
grp=doc.addObject('App::DocumentObjectGroup','grp')
grp.Label = '组'
grp.addObjects([bfsp1,bfsp2,bfsp3,afsp1,afsp2,afsp3,bfsp,afsp])
doc.recompute()