qprod.m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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 qp=qprod(q,p)
a=q(1);
v=reshape(q(2:4),3,1);
x=p(1);
u=reshape(p(2:4),3,1);

qp = [a*x-v'*u, (a*u+x*v)' + cross(v,u)'];