rodrigues公式

function	[outdout]=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


[mn] = 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 = [intheta]

        dm3din = [eye（3）;in‘/theta];

        omega = in/theta;

        %m2 = [omega;theta]

        dm2dm3 = [eye（3）/theta -in/theta^2; zeros（13） 1];

        alpha = cos（theta）;
        beta = sin（theta）;
        gamma = 1-cos（theta）;
        omegav=[[0 -omega（3） omega（2）];[omega（3） 0 -omega（1）];[-omega（2） omega（1） 0 ]];
        A = omega*omega‘;

        %m1 = [alpha;beta;gamma;omegav;A];

        dm1dm2 = zeros（214）;
        dm1dm2（14） = -sin（theta）;
        dm1dm2（24） = cos（theta）;
        dm1dm2（34） = sin（theta）;
        dm1dm2（4:121:3） = [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]‘;

        w1 = omega（1）;
        w2 = omega（2）;
        w3 = omega（3）;

        dm1dm2（13:211） = [2*w1;w2;w3;w2;0;0;w3;0;0];
        dm1dm2（13: 212） = [0;w1;0;w1;2*w2;w3;0;w3;0];
        dm1dm2（13:213） = [0;0;w1;0;0;w2;w1;w2;2*w3];

        R = eye（3）*alpha + omegav*beta + A*gamma;

        dRdm1 = zeros（921）;

        dRdm1（[1 5 9]1） = ones（31）;
        dRdm1（:2） = omegav（:）;
        dRdm1（:4:12） = beta*eye（9）;
        dRdm1（:3） = A（:）;
        dRdm1（:13:21） = gamma*eye（9）;

        dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;


    end;
    out = R;
    dout = dRdin;

    %% it is prob. a rot matr.
elseif （（m==n） & （m==3） & （norm（in‘ * in - eye（3）） < bigeps）...
        & （abs（det（in）-1） < bigeps））
    R = in;

    % project the rotation matrix to SO（3）;
    [USV] = svd（R）;
    R = U*V‘;

    tr = （trace（R）-1）/2;
    dtrdR = [1 0 0 0 1 0 0 0 1]/2;
    theta = real（acos（tr））;


    if sin（theta） >= 1e-4

        dthetadtr = -1/sqrt（1-tr^2）;

        dthetadR = dthetadtr * dtrdR;
        % var1 = [v