gusucode.com > vision工具箱matlab源码程序 > vision/+vision/+internal/+calibration/rodriguesMatrixToVector.m

    function rotationVector = rodriguesMatrixToVector(rotationMatrix)
% rodriguesMatrixToVector Convert a 3D rotation matrix into a rotation
% vector.
%
% rotationVector = rodriguesMatrixToVector(rotationMatrix) computes a 
% rotation vector (axis-angle representation) corresponding to a 3D 
% rotation matrix using the Rodrigues formula.
%
% rotationMatrix is a 3x3 3D rotation matrix.
%
% rotationVector is a 3-element rotation vector corresponding to the 
% rotationMatrix. The vector represents the axis of rotation in 3D, and its 
% magnitude is the rotation angle in radians.
%
% See also vision.internal.calibration.rodriguesVectorToMatrix

% References:
% [1] R. Hartley, A. Zisserman, "Multiple View Geometry in Computer
%     Vision," Cambridge University Press, 2003.
% 
% [2] E. Trucco, A. Verri. "Introductory Techniques for 3-D Computer
%     Vision," Prentice Hall, 1998.

% Copyright 2012 MathWorks, Inc.

%#codegen

% get the rotation matrix that is the closest approximation to the input
[U, ~, V] = svd(rotationMatrix);
rotationMatrix = U * V';

t = trace(rotationMatrix);
% t is the sum of the eigenvalues of the rotationMatrix.
% The eigenvalues are 1, cos(theta) + i sin(theta), cos(theta) - i sin(theta)
% t = 1 + 2 cos(theta), -1 <= t <= 3

theta = real(acos((t - 1) / 2));

r = [rotationMatrix(3,2) - rotationMatrix(2,3); ...
     rotationMatrix(1,3) - rotationMatrix(3,1); ...
     rotationMatrix(2,1) - rotationMatrix(1,2)];

threshold = cast(1e-4, 'like', rotationMatrix); 
if sin(theta) >= threshold
    % theta is not close to 0 or pi
    rotationVector = theta / (2 * sin(theta)) * r;
elseif t-1 > 0
    % theta is close to 0
    rotationVector = (.5 - (t - 3) / 12) * r;
else
    % theta is close to pi
    rotationVector = ...
        computeRotationVectorForAnglesCloseToPi(rotationMatrix, theta);
end

function rotationVector = ...
    computeRotationVectorForAnglesCloseToPi(rotationMatrix, theta)
% r = theta * v / |v|, where (w, v) is a unit quaternion.
% This formulation is derived by going from rotation matrix to unit
% quaternion to axis-angle

% choose the largest diagonal element to avoid a square root of a negative
% number
[~, a] = max(diag(rotationMatrix));
a = a(1);
b = mod(a, 3) + 1;
c = mod(a+1, 3) + 1;

% compute the axis vector
s = sqrt(rotationMatrix(a, a) - rotationMatrix(b, b) - rotationMatrix(c, c) + 1);
v = zeros(3, 1, 'like', rotationMatrix);
v(a) = s / 2;
v(b) = (rotationMatrix(b, a) + rotationMatrix(a, b)) / (2 * s);
v(c) = (rotationMatrix(c, a) + rotationMatrix(a, c)) / (2 * s);

rotationVector = theta * v / norm(v);