:ȡĵ

ʽ:SLAM_GetCenter(kf)

kf :Ϊ4*4ıʾλ˵ľ

˵:
1kfĴ洢ʽΪkf = [s*R,t;0,1] RΪλ˵3*3ת,tΪƫ,sΪ(һΪ1)
2ִгɹһ3*1ľʾռλã = -1/s * R^T * t

:

v = [1,2,3];
r = {SLAM_VToR<\SLAM_VToR>}(v);
t = {Rand<\Rand>}(10,3,1);
pose = [r,t;0,0,0,1];

c =SLAM_GetCenter(pose)//pose,سõ½
c =
[-5.94010275004538
 -3.04644618356797
 -1.06644424215915 ]