:ԪתתR

ʽ:SLAM_QToR(a)

a:Ϊһ4Ԫصľ

˵: 
1aд洢ԪʽΪ[c, m * s , n * s , l * s], c = cos(theta/2), s = sin(theta/2), [m,n,l]Ϊλת, theta ΪƵλתŶĽǶ
2aûһԶһ
3ִϷһ3*3ת

:
q = 1:4;
r = SLAM_QToR(q)//سõ½
r =
[-0.66666666666666    0.66666666666666    0.33333333333333
  0.13333333333333   -0.33333333333333    0.93333333333333
  0.73333333333333    0.66666666666666    0.13333333333333 ]