手眼标定

import numpy as np
def rot(ax,ang):
    c,s=np.cos(ang),np.sin(ang); ax=ax/np.linalg.norm(ax)
    x,y,z=ax; return np.array([[c+x*x*(1-c),x*y*(1-c)-z*s,x*z*(1-c)+y*s],[y*x*(1-c)+z*s,c+y*y*(1-c),y*z*(1-c)-x*s],[z*x*(1-c)-y*s,z*y*(1-c)+x*s,c+z*z*(1-c)]])
np.random.seed(42); Rx=rot([1,0,0],.1)@rot([0,1,0],-.05)@rot([0,0,1],.15)
tx=np.array([.05,-.03,.1]); X_true=np.eye(4); X_true[:3,:3]=Rx; X_true[:3,3]=tx
Xi=np.eye(4); Xi[:3,:3]=Rx.T; Xi[:3,3]=-Rx.T@tx
A_list,B_list=[],[]
for _ in range(15):
    ax=np.random.randn(3); ax=ax/np.linalg.norm(ax)*np.random.uniform(.2,.8)
    RA=rot(ax,np.random.uniform(.3,1.5)); tA=np.random.randn(3)*.05
    A=np.eye(4); A[:3,:3]=RA; A[:3,3]=tA; B=Xi@A@X_true
    B[:3,:3]+=np.random.randn(3,3)*.0005; B[:3,3]+=np.random.randn(3)*.005
    U,_,Vt=np.linalg.svd(B[:3,:3]); B[:3,:3]=U@Vt
    A_list.append(A); B_list.append(B)
M=np.zeros((4,4))
for i in range(len(A_list)):
    qA=np.zeros(4); qA[3]=1; # 简化：使用旋转矩阵特征
    # Kronecker法
n=len(A_list); Mk=np.zeros((9*n,9))
for i in range(n):
    RA=A_list[i][:3,:3]; RB=B_list[i][:3,:3]
    Mk[i*9:(i+1)*9]=np.kron(RA,np.eye(3))-np.kron(np.eye(3),RB.T)
_,_,Vt=np.linalg.svd(Mk); RX=Vt[-1].reshape(3,3)
U2,_,Vt2=np.linalg.svd(RX); RX=U2@Vt2
if np.linalg.det(RX)<0: RX=-RX
Ct,dt_arr=[],[]
for i in range(n):
    Ct.append(A_list[i][:3,:3]-np.eye(3)); dt_arr.append(RX@B_list[i][:3,3]-A_list[i][:3,3])
Ct=np.vstack(Ct); dt_arr=np.concatenate(dt_arr)
tX,_,_,_=np.linalg.lstsq(Ct,dt_arr,rcond=None)
X_est=np.eye(4); X_est[:3,:3]=RX; X_est[:3,3]=tX
R_err=np.arccos(np.clip((np.trace(X_true[:3,:3].T@X_est[:3,:3])-1)/2,-1,1))
t_err=np.linalg.norm(X_est[:3,3]-X_true[:3,3])
print("手眼标定"); print("="*60)
print(f"旋转误差:{np.degrees(R_err):.3f}° 平移误差:{t_err*1000:.3f}mm")
me=0
for i in range(n):
    e=np.linalg.norm(A_list[i]@X_est-X_est@B_list[i]); me=max(me,e)
print(f"AX=XB最大误差:{me:.6f}")

import numpy as np
np.random.seed(42)
print("感知系统噪声鲁棒性")
for sigma in [0.5,1.0,2.0,5.0]:
    K=np.array([[500,0,320],[0,500,240],[0,0,1]]); n=8
    pts3d=np.random.rand(n,3)*0.2; R=np.eye(3); t=np.array([0,0,1.])
    pc=(R@pts3d.T).T+t; p2d=(K@pc.T).T; p2d=p2d[:,:2]/p2d[:,2:3]
    noisy=p2d+np.random.randn(*p2d.shape)*sigma
    proj_err=np.mean(np.linalg.norm(noisy-p2d,axis=1))
    print(f"  σ={sigma:.1f}px: 2D误差={proj_err:.2f}px, 3D误差≈{proj_err/500*1000:.1f}mm")