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?????????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））?????????&?（abs（det（in）-1）?????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