在手眼标定中需要使用罗德里格斯计算旋转矩阵对应的旋转轴
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 vector
theta = norm(in);
if theta < eps
R = 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;
else
if n==length(in) in=in'; end; %% make it a column vec. if necess.
%m3 = [in,theta]
dm3din = [eye