function out = rv( in )
% rv 3D rotation vector <--> 3x3 coordinate rotation matrix
% E=rv(v) and v=rv(E) convert between a rotation vector v, whose magnitude
% and direction describe the angle and axis of rotation of a coordinate
% frame B relative to frame A, and the 3x3 coordinate rotation matrix E
% that transforms from A to B coordinates. For example, if v=[theta;0;0]
% then rv(v) produces the same matrix as rx(theta). If the argument is a
% 3x3 matrix then it is assumed to be E, otherwise it is assumed to be v.
% rv(E) expects E to be accurately orthonormal, and returns a column vector
% with a magnitude in the range [0,pi]. If the magnitude is exactly pi
% then the sign of the return value is unpredictable, since pi*u and -pi*u,
% where u is any unit vector, both represent the same rotation. rv(v) will
% accept a row or column vector of any magnitude.
if all(size(in)==[3 3])
out = Etov( in );
else
out = vtoE( [in(1);in(2);in(3)] );
end
function E = vtoE( v )
theta = norm(v);
if theta == 0
E = eye(3);
else
s = sin(theta);
c = cos(theta);
c1 = 2 * sin(theta/2)^2; % 1-cos(h) == 2sin^2(h/2)
u = v/theta;
E = c*eye(3) - s*skew(u) + c1*u*u';
end
function v = Etov( E )
% This function begins by extracting the skew-symmetric component of E,
% which, as can be seen from the previous function, is -s*skew(v/theta).
% For angles sufficiently far from pi, v is then calculated directly from
% this quantity. However, for angles close to pi, E is almost symmetric,
% and so extracting the skew-symmetric component becomes numerically
% ill-conditioned, and provides an increasingly inaccurate value for the
% direction of v. Therefore, the component c1*u*u' is extracted as well,
% and used to get an accurate value for the direction of v. If the angle
% is exactly pi then the sign of v will be indeterminate, since +v and -v
% both represent the same rotation, but the direction will still be
% accurate.
w = -skew(E); % w == s/theta * v
s = norm(w);
c = (trace(E)-1)/2;
theta = atan2(s,c);
if s == 0
v = [0;0;0];
elseif theta < 0.9*pi % a somewhat arbitrary threshold
v = theta/s * w;
else % extract v*v' component from E and
E = E - c * eye(3); % pick biggest column (best chance
E = E + E'; % to get sign right)
if E(1,1) >= E(2,2) && E(1,1) >= E(3,3)
if w(1) >= 0
v = E(:,1);
else
v = -E(:,1);
end
elseif E(2,2) >= E(3,3)
if w(2) >= 0
v = E(:,2);
else
v = -E(:,2);
end
else
if w(3) >= 0
v = E(:,3);
else
v = -E(:,3);
end
end
v = theta/norm(v) * v;
end