q2r.m

来自「移动机器人同时定位与地图创建最前沿技术」· M 代码 · 共 20 行

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SLAM Summer School 2006, Oxford.
% Practical 3. SLAM using Monocular Vision.
% Practical exercise.
% J.M.M. Montiel, Javier Civera, Andrew J. Davison.
% {josemari, jcivera}@unizar.es, [e-mail]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function R=q2r(q)

%q=q/norm(q);

x=q(2);
y=q(3);
z=q(4);
r=q(1);

R=[r*r+x*x-y*y-z*z  2*(x*y -r*z)     2*(z*x+r*y)
   2*(x*y+r*z)      r*r-x*x+y*y-z*z  2*(y*z-r*x)
   2*(z*x-r*y)      2*(y*z+r*x)      r*r-x*x-y*y+z*z];