CODEWITHSUNDEEP
matlabroboticsrotational-matrices
Yash Jain
Yash Jain

Reputation: 41

How to convert a rotation matrix to axis angle form?

theta=acos((trace(R)-1)/2);
if trace(R)==3
    vec = [0 0 0];
    axang=[0 0 0 0];
    vec(1)=R(3,2)-R(2,3);
    vec(2)=R(1,3)-R(3,1);
    vec(3)=R(2,1)-R(1,2);
    vec=(1/(2*sin(theta)))*vec;       
    axang = [vec, theta];
elseif trace(R)==-1
    vec=[0 0 0;0 0 0];
    axang=[0 0 0 0;0 0 0 0];
    X=[0 0];
    Y=[0 0];
    Z=[0 0];
    Y(1)=sqrt((R(2,2)+1)/2);
    Y(2)=-Y(1);
    X(1)=R(2,1)/(2*Y(1));
    X(2)=R(2,1)/(2*Y(2));
    Z(1)=R(2,3)/(2*Y(1));
    Z(2)=R(2,3)/(2*Y(2));
    vec(1,:)=[X(1) Y(1) Z(1)];
    vec(2,:)=[X(2) Y(2) Z(2)];
    axang(1,:)=[vec(1,:), theta];
    axang(2,:)=[vec(2,:), theta];
else 
    vec = [0 0 0];
    axang=[0 0 0 0];
    vec(1)=R(3,2)-R(2,3);
    vec(2)=R(1,3)-R(3,1);
    vec(3)=R(2,1)-R(1,2);
    vec=(1/(2*sin(theta)))*vec;       
    axang = [vec, theta];
end

So this was my code but it didn't work when the rotation matrix is 

R = [-1 0  0;
     0  -1 0;
     0  0  1]

What is wrong with the code ? axang is a vector that stores axis in the first three positions and the angle in the last position.

Upvotes: 1

Views: 2082

Answers (3)

M. Waheed Ramzan
M. Waheed Ramzan

Reputation: 71

In case of trace(R)==-1, the sign of axis term may be flipped. To get rid of it, following steps compute the axis angle vector.

  1. find X(1) = sqrt((R(1,1)+1)/2);
  2. if it is not zero, compute Y(1) = R(1,2)/(2*X(1)) and Y(2) = R(1,2)/(2*X(2)) and Z(1) = R(1,3)/(2*X(1)) Z(2) = R(1,3)/(2*X(2));

If X(1) = 0, then find Y(1) = sqrt((R(2,2)+1)/2) if Y(1) is not zero then find the other terms from Y(1)

else find Z(1) and find the other terms from Z(1)

Upvotes: 0

oliver
oliver

Reputation: 2843

In a similar vein to the above answer you may wish to look into the use of the MatLab tool Translation1 = se2(StructuringElement, TranslationOffset).

The variable TranslationOffset can be applied as an angle in the form of 60*pi/180 for example.

Upvotes: 0

m7913d
m7913d

Reputation: 11072

It seems to me that you are looking for a conversion of a rotation matrix to quaternions, which is a built-in feature of Matlab if you installed the Robotics System Toolbox, i.e. rotm2quat:

axang = rotm2quat(R)

Note that the output format is slightly different as documented by Matlab:

Unit quaternion, returned as an n-by-4 matrix containing n quaternions. Each quaternion, one per row, is of the form q = [w x y z], with w as the scalar number.

Therefore you may need to swap the columns as follows:

axang = axang(:, [2 3 4 1]);

Upvotes: 2

Related Questions