物体位姿估计

import numpy as np
K=np.array([[500,0,320],[0,500,240],[0,0,1]])
obj_pts=np.array([[0,0,0],[.1,0,0],[.1,.1,0],[0,.1,0],[0,0,.1],[.1,0,.1]])
Rt=np.array([[.9,-.1,.4],[.1,.99,.05],[-.4,.01,.91]]); tt=np.array([.3,.1,1.0])
pc=(Rt@obj_pts.T).T+tt; p2d=(K@pc.T).T; p2d=p2d[:,:2]/p2d[:,2:3]
p2d_n=p2d+np.random.randn(*p2d.shape)*.5
# DLT
fx,fy=K[0,0],K[1,1]; cxx,cyy=K[0,2],K[1,2]
A=[]
for i in range(len(obj_pts)):
    X,Y,Z=obj_pts[i]; xn=(p2d_n[i,0]-cxx)/fx; yn=(p2d_n[i,1]-cyy)/fy
    A.append([X,Y,Z,1,0,0,0,0,-xn*X,-xn*Y,-xn*Z,-xn])
    A.append([0,0,0,0,X,Y,Z,1,-yn*X,-yn*Y,-yn*Z,-yn])
A=np.array(A); _,_,Vt=np.linalg.svd(A); P=Vt[-1].reshape(3,4)
M=P[:,:3]; R2=np.linalg.solve(K,M); U,S,Vt2=np.linalg.svd(R2)
Re=U@Vt2; if np.linalg.det(Re)<0: Re=-Re
te=np.linalg.solve(K,P[:,3])/np.mean(S)
Rerr=np.arccos(np.clip((np.trace(Rt.T@Re)-1)/2,-1,1)); terr=np.linalg.norm(te-tt)
print("物体位姿估计"); print("="*60)
print(f"PnP: 旋转误差={np.degrees(Rerr):.2f}° 平移误差={terr*1000:.1f}mm")
# ICP
np.random.seed(42); src=np.random.randn(100,3)*.05+np.array([.5,.3,0])
Ricp=np.array([[.98,-.17,0],[.17,.98,0],[0,0,1]]); ticp=np.array([.02,-.01,.005])
tgt=(Ricp@src.T).T+ticp+np.random.randn(100,3)*.001
R=np.eye(3); t=np.zeros(3)
from scipy.spatial.distance import cdist
for _ in range(50):
    trf=(R@src.T).T+t; d=cdist(trf,tgt); idx=np.argmin(d,1); mt=tgt[idx]
    sc=trf-np.mean(trf,0); tc=mt-np.mean(mt,0); H=sc.T@tc
    U,_,Vt=np.linalg.svd(H); Rn=Vt.T@U.T
    if np.linalg.det(Rn)<0: Vt[-1]*=-1; Rn=Vt.T@U.T
    tn=np.mean(mt,0)-Rn@np.mean(trf,0); R=Rn@R; t=Rn@t+tn
Rierr=np.arccos(np.clip((np.trace(Ricp.T@R)-1)/2,-1,1)); tierr=np.linalg.norm(t-ticp)
print(f"ICP: 旋转误差={np.degrees(Rierr):.3f}° 平移误差={tierr*1000:.2f}mm")

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")