% MATRIX2ANGLEAXIS - Homogeneous matrix to angle-axis description % % Usage: t = matrix2angleaxis(T) % % Argument: T - 4x4 Homogeneous transformation matrix % Returns: t - 3x1 column vector giving rotation axis with magnitude equal % to the rotation angle in radians. % % Note that only the top left 3x3 rotation component of T is used, any % translation component in T is ignored. % % See also: ANGLEAXIS2MATRIX, ANGLEAXIS2MATRIX2, ANGLEAXISROTATE, NEWANGLEAXIS, % NORMALISEANGLEAXIS % Copyright (c) 2008 Peter Kovesi % School of Computer Science & Software Engineering % The University of Western Australia % pk at csse uwa edu au % http://www.csse.uwa.edu.au/ % % Permission is hereby granted, free of charge, to any person obtaining a copy % of this software and associated documentation files (the "Software"), to deal % in the Software without restriction, subject to the following conditions: % % The above copyright notice and this permission notice shall be included in % all copies or substantial portions of the Software. % % The Software is provided "as is", without warranty of any kind. % 2008 - Original version % May 2013 - Code to trap small rotations added function t = matrix2angleaxis(T) % This code follows the implementation suggested by Hartley and Zisserman R = T(1:3, 1:3); % Extract rotation part of T % Trap case where rotation is very small. (See angleaxis2matrix.m) Reye = R-eye(3); if norm(Reye) < 1e-8 t = [T(3,2); T(1,3); T(2,1)]; return end % Otherwise find rotation axis as the eigenvector having unit eigenvalue % Solve (R-I)v = 0; [v,d] = eig(Reye); % The following code assumes the eigenvalues returned are not necessarily % sorted by size. This may be overcautious on my part. d = diag(abs(d)); % Extract eigenvalues [s, ind] = sort(d); % Find index of smallest one if d(ind(1)) > 0.001 % Hopefully it is close to 0 warning('Rotation matrix is dubious'); end axis = v(:,ind(1)); % Extract appropriate eigenvector if abs(norm(axis) - 1) > .0001 % Debug warning('non unit rotation axis'); end % Now determine the rotation angle twocostheta = trace(R)-1; twosinthetav = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; twosintheta = axis'*twosinthetav; theta = atan2(twosintheta, twocostheta); t = theta*axis;