18 lines
337 B
Mathematica
18 lines
337 B
Mathematica
|
%%
|
|||
|
|
%Cartographer SLAM <EFBFBD><EFBFBD>̬<EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
%<EFBFBD><EFBFBD><EFBFBD>˻<EFBFBD>
|
|||
|
|
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:xiphix@126.com
|
|||
|
|
%<EFBFBD><EFBFBD>:xiphix
|
|||
|
|
|
|||
|
|
|
|||
|
|
function [q] = uv2q(v1, v2)
|
|||
|
|
%Finding quaternion representing the rotation from one vector to another
|
|||
|
|
nv1 = v1/norm(v1);
|
|||
|
|
nv2 = v2/norm(v2);
|
|||
|
|
if norm(nv1+nv2)==0
|
|||
|
|
q = [1 0 0 0]';
|
|||
|
|
else
|
|||
|
|
half = (nv1 + nv2)/norm(nv1 + nv2);
|
|||
|
|
q = [nv1'*half; cross(nv1, half)];
|
|||
|
|
end
|
|||
|
|
end
|