Re: angle from rotation matrix

Hi James,

Here is the complete code:

Run it on another computer:
median(D,E,Q): 1.241267e-016 2.077037e-016 1.359740e-016
mean(D,E,Q): 9.230310e-014 6.622440e-016 1.184177e-011

Bruno

function error=test
%%%%%%%%%%%%%%%%%%%%%%%%

ntest=100000;

error=zeros(ntest,3);
for n=1:ntest
theta=2*pi*(rand()-0.5);

u=randn(3,1); u=u/norm(u);

I=eye(3);
R=[ 0 -u(3) u(2);
u(3) 0 -u(1);
-u(2) u(1) 0];
T=u*u';
A=cos(theta)*I + (1-cos(theta))*T + sin(theta)*R;

[thetadir udir]=DirectMethod(A);
[thetaeig ueig]=EigenMethod(A);
[thetaqua uqua]=QuaternionMethod(A);

error(n,1)=min(norm(u-udir),norm(u+udir));
error(n,2)=min(norm(u-ueig),norm(u+ueig));
error(n,3)=min(norm(u-uqua),norm(u+uqua));

end

median(error,1);
mean(error,1);

figure(1);
semilogy(error);
sres=arrayfun(@(k) ['median=' num2str(median(error(:,k)))
'/' ...
'mean=' num2str(mean(error(:,k)))], ...
(1:3), 'UniformOutput', false);
legend(['(D) ' sres{1}], ...
['(E) ' sres{2}], ...
['(Q) ' sres{3}]);

end

function [theta u]=DirectMethod(A)

d=diag(A);
t=sum(d);
uR=[A(3,2)-A(2,3);...
A(1,3)-A(3,1); ...
A(2,1)-A(1,2)];
uR2=uR.^2;
s2=sum(uR2);
a=(3-t)+s2;
if a>0
theta=atan2(sqrt(s2),t-1);
u2=(sum((A+A'-t*eye(3)).^2,2)+((4*d-2*t)+1))+uR2;
absu=sqrt(u2.*(u2>0))/sqrt(a);
signu=sign(uR);
signu(signu==0)=1; % we don't want the nil case
u=absu.*signu;
u=u/norm(u);
else
warning('Rotation matrix close to indentity');
u=[1 0 0]';
theta=0;
end
end

function [theta u]=EigenMethod(A)
[P D]=eig(A);
d=diag(D);
[dummy k]=min(d-1);
u=P(:,k);
[dummy k]=max(abs(d-1));
theta=angle(d(k));
t = [A(3,2)-A(2,3);A(1,3)-A(3,1);A(2,1)-A(1,2)];
if any(sign(t).*sign(u)==-sign(sin(theta)))
theta=-theta;
end
end

function [theta u]=QuaternionMethod(A)
[theta v] = angleaxis(dc2quat(A));
u=v';
end

function q = dc2quat(dc)
% dc2quat quaternion direction cosine matrix angle axis
%**********************************************************************
%
% dc2quat calculates the quaternion corresponding to a direction
% cosine matrix. I believe this is based on the algorithm
% given by A. R. Klumpp, "Singularity-Free Extraction of a
% Quaternion from a Direction-Cosine Matrix," Journal of
% Spacecraft and Rockets, Vol. 13, Dec. 1976, pp. 754-755.
% Assumes input dc is orthonormal.
%
% Input: dc = 3x3 direction cosine matrix
%
% Output: q = quaternion, q(1) = scalar, q(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
%
% Programmer: James Tursa
%
%**********************************************************************

q = [0 0 0 0];
tr = dc(1,1) + dc(2,2) + dc(3,3);
ii = 0;
nn = 0;
q(1) = tr;
for kk=1:3
if( dc(kk,kk) > q(1) )
ii = kk;
q(1) = dc(kk,kk);
end
end

tr = sqrt(1 + 2*q(1) - tr);

order = [2 3 1];
for mm=1:3
kk = order(mm);
nn = nn + 1;
jj = 6 - nn - kk;
x = ii * (nn - ii);
if( x == 0)
q(1) = (dc(jj,kk) - dc(kk,jj)) / tr;
q(nn+1) = q(1);
else
q(jj+kk-ii+1) = (dc(jj,kk) + dc(kk,jj)) / tr;
end
end

if( ii == 0 )
q(1) = tr;
else
q(ii+1) = tr;
end
q(2:4) = -q(2:4);
if( q(1) == 0 )
q = 0.5 * q;
else
q = 0.5 * sign(q(1)) * q;
end

%\
% MAKES QUATERNION A POSITIVE ROTATION
%/

if( q(1) <= 0 )
q = -q;
end

%\
% NORMALIZE QUATERNION (KEEPS ROUTINE STABLE)
%/

q = q / norm(q);

return

end

function dc = quat2dc(q)
% quat2dc quaternion direction cosine matrix angle axis
%*******************************************************************
%
% quat2dc calculates the dirction cosine matrix
corresponding to a
% quaternion. Assumes input quaternion is normalized.
%
% Input: q = quaternion, q(1) = scalar, q(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
%
% Output: dc = 3x3 direction cosine matrix
%
% Programmer: James Tursa
%
%*******************************************************************

q11 = q(1)^2;
q12 = q(1)*q(2);
q13 = q(1)*q(3);
q14 = q(1)*q(4);
q22 = q(2)^2;
q23 = q(2)*q(3);
q24 = q(2)*q(4);
q33 = q(3)^2;
q34 = q(3)*q(4);
q44 = q(4)^2;

dc = zeros(3);
dc(1,1) = q11 + q22 - q33 - q44;
dc(2,1) = 2 * (q23 - q14);
dc(3,1) = 2 * (q24 + q13);
dc(1,2) = 2 * (q23 + q14);
dc(2,2) = q11 - q22 + q33 - q44;
dc(3,2) = 2 * (q34 - q12);
dc(1,3) = 2 * (q24 - q13);
dc(2,3) = 2 * (q34 + q12);
dc(3,3) = q11 - q22 - q33 + q44;

return
end

function [a v] = angleaxis(x)
% angleaxis quaternion direction cosine matrix angle axis
%*******************************************************************
%
% angleaxis calculates the rotation angle and rotation axis
of the
% input quaternion or direction cosine matrix.
%
% Input: x = quaternion, x(1) = scalar, x(2:4) = vector
% Rotation sense = Successive rotations are right multiplies.
% Assumes x is normalized.
%
% or
%
% x = direction cosine matrix.
% Assumes x is orthonormalized.
%
% Output: a = rotation angle (radians)
% v = rotation axis (1x3 unit vector)
%
% Programmer: James Tursa
%
%*******************************************************************

if( numel(x) == 9 )
q = dc2quat(x);
else
q = x;
end
a = 2 * acos(q(1));
if( nargout == 2 )
if( a == 0 )
v = [1 0 0];
else
v = q(2:4)/sin(a/2);
end
end

return
end
.

Relevant Pages

  • Re: angle from rotation matrix
    ... One method is to convert the rotation matrix (i.e., ... angle and axis directly from the quaternion elements. ... dc2quat.m --> Converts a direction cosine matrix into a quaternion. ...
    (comp.soft-sys.matlab)
  • Re: Quaternion newbie needs help
    ... Quaternions have been promoted by Hamilton, but as reported by Petzold, "James Clerk Maxwell used quaternions in a limited, rather unorthodox manner in his influential Treatise on Electricity and Magnetism, which suggested to some readers that quaternions were more useful without the scalar part." ... we can use a quaternion to store Temperature at a given 3D point. ... It just happens that using quaternions, and some process on them we can define useful rotation about an arbitrary axis if the scalar is the cosinus of half the angle of rotation, and the vector, the direction of the axis times the scalar value of the sinus of half the angle of rotation. ... WPF doesn't really require you to know all that. ...
    (microsoft.public.dotnet.languages.csharp)
  • Re: Rotation always relative to the world
    ... consists of 4 numbers and represents a rotation about an arbitrary ... You can multiply quaternions together to create a new quaternion that ... ArcBall translates 2D mouse input into 3D quaternion math. ... click outside of the sphere, the method will snap the vector to the nearest ...
    (microsoft.public.win32.programmer.directx.managed)
  • Re: Quaternion newbie needs help
    ... Unfortunately, as is the case for most of the WPF documentation, there is little in the way of guidance as to how to use the Quaternion structure. ... However, as you can see from the article above, the x, y, and z values relate to the vector describing the axis of rotation, and the w value relates to the amount of rotation. ... While most of the time you will want to rely on the Matrix3D to set up a single transform applied to a full world of objects, you can rotate individual vectors by the Quaternion using the Conjugatemethod. ...
    (microsoft.public.dotnet.languages.csharp)
  • Re: Quaternions and the stock market
    ... > transforming vectors directly by quaternion multiplication. ... Rodrigues rotation formula is _still_ ... >>> Gimbal lock removes a degree of freedom and common using eulerian ... >>> coordinate system or by constructing rotation transformations using the ...
    (sci.math)