How to convert a rotation matrix to axis angle form? - matlab

Question

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.

Answer 1

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]);

Answer 2

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.

Answer 3

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.
find X(1) = sqrt((R(1,1)+1)/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)