Path: news.mathworks.com!newsfeed-00.mathworks.com!newscon02.news.prodigy.net!prodigy.net!wns11feed!worldnet.att.net!199.45.49.37!cyclone1.gnilink.net!spamkiller2.gnilink.net!gnilink.net!trndny09.POSTED!702e7bde!not-for-mail
From: James Tursa <aclassyguywithaknotac@hotmail.com>
Newsgroups: comp.soft-sys.matlab
Subject: Re: angle from rotation matrix
Message-ID: <2shcm3pkforks50k3jjkjo0bdkioir9jq2@4ax.com>
References: <fjvbqk$gbt$1@fred.mathworks.com> <fk0cpd$g0v$1@fred.mathworks.com> <fk0iq9$k0j$1@fred.mathworks.com> <fk59hv$n5r$1@fred.mathworks.com>
X-Newsreader: Forte Agent 3.3/32.846
MIME-Version: 1.0
Content-Type: text/plain; charset=us-ascii
Content-Transfer-Encoding: 7bit
Lines: 126
Date: Mon, 17 Dec 2007 10:33:11 GMT
NNTP-Posting-Host: 71.112.25.94
X-Complaints-To: abuse@verizon.net
X-Trace: trndny09 1197887591 71.112.25.94 (Mon, 17 Dec 2007 05:33:11 EST)
NNTP-Posting-Date: Mon, 17 Dec 2007 05:33:11 EST
Xref: news.mathworks.com comp.soft-sys.matlab:442714


On Mon, 17 Dec 2007 07:47:11 +0000 (UTC), "Roger Stafford"
<ellieandrogerxyzzy@mindspring.com.invalid> wrote:

>"Bruno Luong" <brunoluong@yahoo.com> wrote in message <fk0iq9$k0j
>$1@fred.mathworks.com>...
>> One can use either eig() or direct calculation from A:
>> 
>> %%%% First method 
>> 
>> D=eig(A);
>> [dummy k]=max(abs(D-1));
>> theta=angle(D(k))
>> 
>> %%%%% Second method, returned angle in [0,pi]
>> 
>> c=(trace(A)-1);
>> s=norm([A(3,2)-A(2,3);...
>>         A(1,3)-A(3,1); ...
>>         A(2,1)-A(1,2)]);
>> theta=atan2(s,c) % better than using acos or asin
>> 
>> Bruno
>---------
>Hello again, Bruno.
>
>  It has occurred to me that the second method of yours would not be robust 
>with respect to accuracy of the rotation axis when the rotation angle theta is 
>near pi.  In that case all the elements of your quantity u, computed from 
>matrix A, would necessarily be very small differences of typically large 
>quantities and subsequent division by norm(u) would yield excessive roundoff 
>errors in the normalized u.  If norm(u) were zero in this case, the answer 
>[1,0,0] would be completely wrong, in general.
>
>  I have not been able to think of an alternative method of doing the problem 
>in a robust manner, except by performing eigenvector analysis on it, as in 
>your first method, and in mine.  In your first method it looks as though you 
>would still need to compute u from the A matrix and take its dot product with 
>the eigenvector whose eigenvalue is 1 in order to determine whether or not 
>that dot product is equal to, and therefore has the same sign as, 2*sin(theta), 
>where theta has been determined from one of the complex eigenvalues.  If 
>not, the eigenvector should be reversed in direction, or the sign of theta 
>reversed, so that the rotation axis unit vector would match the rotation angle.
>
>Roger Stafford

For the particular case of theta near pi (and for cases near 0 for
that matter), you essentially get a skew symmetric matrix for the
direction cosine matrix, so the small quantities that are differenced
in the formation of u are of opposite sign. So you don't get a
cancellation error in the subtraction ... the values end up adding in
magnitude. It appears to be robust in a couple of tests I have run.
Here is the code I used:

ang = pi - 1e-14
rx = [1 0 0;0 cos(ang)  sin(ang);0 -sin(ang) cos(ang)];
%rx = eye(3);
ry = [cos(ang) 0 -sin(ang);0 1 0;sin(ang) 0 cos(ang)];
%ry = eye(3);
rz = [cos(ang)  sin(ang) 0;-sin(ang) cos(ang) 0; 0 0 1];
%rz = eye(3);
A = rx*ry*rz

c=(trace(A)-1);
uq=[A(3,2)-A(2,3);...
   A(1,3)-A(3,1); ...
   A(2,1)-A(1,2)]';
s=norm(uq);
theta=atan2(s,c)

if s>0
    u=uq/s
else
    warning('A close to identity, arbitrary result');
    u=[1 0 0]
end

I ran with each x, y, and z rotation individually,and also all three
together. For example, all three together gives the output:

ang =

   3.14159265358978

A =

   1.00000000000000  -0.00000000000001  -0.00000000000001
   0.00000000000001   1.00000000000000  -0.00000000000001
   0.00000000000001   0.00000000000001   1.00000000000000

theta =

    1.790337176247374e-014

u =

   0.57735026918962  -0.57735026918963   0.57735026918962

The individual cases seems to work just fine, and the all-three case
shown above seems to work just fine also. If you consider the original
angle used and the difference from pi we actually got and then
multiply this by sqrt(3) you would expect to get something close to
theta, and we do:

>> sqrt(3)*(pi - ang)

ans =

    1.769125671472879e-014

Not sure how close to expect these to match, but at least they are
close.

Also, you would expect to get a rotation vector equally spaced from
the x, y, and z axes, and we do.

I also ran with a small ang near zero and got similar results. Then I
compared with the quaternion code I posted earlier (using atan2
instead of acos ... thanks Bruno) and got exactly the same results
(except that the quaternion code is opposite sense in rotation angle).
I believe the quaternion code I posted is based on a robust peer
reviewed method (although the code I posted was just a quick
conversion from some Fortran code I happened to have and not written
well in MATLAB syntax), so I am quite encouraged that the two give the
same answer.

James Tursa