在手眼标定中需要使用罗德里格斯计算旋转矩阵对应的旋转轴
rodrigues.m文件内容如下:
function [out,dout]=rodrigues(in)% RODRIGUES Transform rotation matrix into rotation vector and viceversa.
%
% Sintax: [OUT]=RODRIGUES(IN)
% If IN is a 3x3 rotation matrix then OUT is the
% corresponding 3x1 rotation vector
% if IN is a rotation 3-vector then OUT is the
% corresponding 3x3 rotation matrix
%%%
%% Copyright (c) March 1993 -- Pietro Perona
%% California Institute of Technology
%%%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!%% BUG when norm(om)=pi fixed -- April 6th, 1997;
%% Jean-Yves Bouguet%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
%% Jean-Yves Bouguet% BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007[m,n] = size(in);
%bigeps = 10e+4*eps;
bigeps = 10e+20*eps;if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vectortheta = norm(in);if theta < epsR = eye(3);%if nargout > 1,dRdin = [0 0 0;0 0 1;0 -1 0;0 0 -1;0 0 0;1 0 0;0 1 0;-1 0 0;0 0 0];%end;elseif n==length(in) in=in'; end; %% make it a column vec. if necess.%m3 = [in,theta]dm3din = [eye