%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 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, ajd@doc.ic.ac.uk%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%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)'];