VLFeat.org

API docs

  • Home
    • Download and Install
    • API docs
    • Matlab docs
    • About VLFeat
  • Tutorials
    • SIFT
    • MSER
    • IKM
    • HIKM
    • AIB
    • Utils
  • Main Page
  • Related Pages
  • Data Structures
  • Files
  • Examples

rodrigues.c

00001 
00006 /* AUTORIGHTS
00007 Copyright 2007 (c) Andrea Vedaldi and Brian Fulkerson
00008 
00009 This file is part of VLFeat, available in the terms of the GNU
00010 General Public License version 2.
00011 */
00012 
00013 #include "generic.h"
00014 #include "rodrigues.h"
00015 
00016 #include <math.h>
00017 
00024 VL_EXPORT
00025 void 
00026 vl_rodrigues(double* R_pt, double* dR_pt, const double* om_pt)
00027 {
00028   /*
00029     Let
00030      
00031        th = |om|,  r=w/th,  
00032        sth=sin(th),  cth=cos(th), 
00033        ^om = hat(om)
00034 
00035     Then the rodrigues formula is an expansion of the exponential
00036     function:
00037      
00038      rodrigues(om) = exp ^om = I + ^r sth + ^r^2 (1 - cth).
00039 
00040     The derivative can be computed by elementary means and
00041     results:
00042 
00043     d(vec rodrigues(om))    sth  d ^r    1 - cth  d (^r)^2
00044     -------------------- =  ---- ----- + -------  -------- +
00045           d om^T             th  d r^T     th      d r^T
00046                   
00047                           sth                     1 - cth
00048           + vec^r (cth - -----) + vec^r^2 (sth - 2-------)r^T
00049                           th                         th
00050   */
00051 
00052 #define OM(i)   om_pt[(i)]
00053 #define R(i,j)  R_pt[(i)+3*(j)]
00054 #define DR(i,j) dR_pt[(i)+9*(j)]
00055 
00056   const double small = 1e-6 ;
00057 
00058   double th = sqrt( OM(0)*OM(0) + 
00059                     OM(1)*OM(1) +
00060                     OM(2)*OM(2) ) ;
00061   
00062   if( th < small ) {
00063     R(0,0) = 1.0 ; R(0,1) = 0.0 ; R(0,2) = 0.0 ;
00064     R(1,0) = 0.0 ; R(1,1) = 1.0 ; R(1,2) = 0.0 ;
00065     R(2,0) = 0.0 ; R(2,1) = 0.0 ; R(2,2) = 1.0 ;
00066     
00067     if(dR_pt) {
00068       DR(0,0) = 0  ; DR(0,1) = 0   ; DR(0,2) = 0 ;
00069       DR(1,0) = 0  ; DR(1,1) = 0   ; DR(1,2) = 1 ;
00070       DR(2,0) = 0  ; DR(2,1) = -1  ; DR(2,2) = 0 ;
00071 
00072       DR(3,0) = 0  ; DR(3,1) = 0   ; DR(3,2) = -1 ;
00073       DR(4,0) = 0  ; DR(4,1) = 0   ; DR(4,2) = 0 ;
00074       DR(5,0) = 1  ; DR(5,1) = 0   ; DR(5,2) = 0 ;
00075 
00076       DR(6,0) = 0  ; DR(6,1) = 1   ; DR(6,2) = 0 ;
00077       DR(7,0) = -1 ; DR(7,1) = 0   ; DR(7,2) = 0 ;
00078       DR(8,0) = 0  ; DR(8,1) = 0   ; DR(8,2) = 0 ;
00079     }
00080     return ;
00081   }
00082 
00083   {
00084     double x = OM(0) / th ;
00085     double y = OM(1) / th ;
00086     double z = OM(2) / th ;
00087     
00088     double xx = x*x ;
00089     double xy = x*y ;
00090     double xz = x*z ;
00091     double yy = y*y ;
00092     double yz = y*z ;
00093     double zz = z*z ;
00094     
00095     const double yx = xy ;
00096     const double zx = xz ;
00097     const double zy = yz ;
00098     
00099     double sth  = sin(th) ;
00100     double cth  = cos(th) ;
00101     double mcth = 1.0 - cth ; 
00102     
00103     R(0,0) = 1          - mcth * (yy+zz) ;
00104     R(1,0) =     sth*z  + mcth * xy ;
00105     R(2,0) =   - sth*y  + mcth * xz ;
00106     
00107     R(0,1) =   - sth*z  + mcth * yx ;
00108     R(1,1) = 1          - mcth * (zz+xx) ;
00109     R(2,1) =     sth*x  + mcth * yz ;
00110     
00111     R(0,2) =     sth*y  + mcth * xz ;
00112     R(1,2) =   - sth*x  + mcth * yz ;
00113     R(2,2) = 1          - mcth * (xx+yy) ;
00114     
00115     if(dR_pt) {
00116       double a =  sth / th ;
00117       double b = mcth / th ;
00118       double c = cth - a ;
00119       double d = sth - 2*b ;
00120       
00121       DR(0,0) =                         - d * (yy+zz) * x ;
00122       DR(1,0) =        b*y   + c * zx   + d * xy      * x ;
00123       DR(2,0) =        b*z   - c * yx   + d * xz      * x ;
00124       
00125       DR(3,0) =        b*y   - c * zx   + d * xy      * x ;
00126       DR(4,0) =     -2*b*x              - d * (zz+xx) * x ;
00127       DR(5,0) =  a           + c * xx   + d * yz      * x ;
00128       
00129       DR(6,0) =        b*z   + c * yx   + d * zx      * x ;
00130       DR(7,0) = -a           - c * xx   + d * zy      * x ;
00131       DR(8,0) =     -2*b*x              - d * (yy+xx) * x ;
00132       
00133       DR(0,1) =     -2*b*y              - d * (yy+zz) * y ;
00134       DR(1,1) =        b*x   + c * zy   + d * xy      * y ;
00135       DR(2,1) = -a           - c * yy   + d * xz      * y ;
00136       
00137       DR(3,1) =        b*x   - c * zy   + d * xy      * y ;
00138       DR(4,1) =                         - d * (zz+xx) * y ;
00139       DR(5,1) =        b*z   + c * xy   + d * yz      * y ;
00140       
00141       DR(6,1) = a            + c * yy   + d * zx      * y ;
00142       DR(7,1) =        b*z   - c * xy   + d * zy      * y ;
00143       DR(8,1) =     -2*b*y              - d * (yy+xx) * y ;
00144       
00145       DR(0,2) =     -2*b*z              - d * (yy+zz) * z ;
00146       DR(1,2) =  a           + c * zz   + d * xy      * z ;
00147       DR(2,2) =        b*x   - c * yz   + d * xz      * z ;
00148       
00149       DR(3,2) =  -a          - c * zz   + d * xy      * z ;
00150       DR(4,2) =     -2*b*z              - d * (zz+xx) * z ;
00151       DR(5,2) =        b*y   + c * xz   + d * yz      * z ;
00152       
00153       DR(6,2) =        b*x   + c * yz   + d * zx      * z ;
00154       DR(7,2) =        b*y   - c * xz   + d * zy      * z ;
00155       DR(8,2) =                         - d * (yy+xx) * z ;
00156     }
00157   }
00158 
00159 #undef OM
00160 #undef R
00161 #undef DR
00162 
00163 }
00164 
00177 VL_EXPORT
00178 void vl_irodrigues(double* om_pt, double* dom_pt, const double* R_pt)
00179 {
00180   /*
00181                     tr R - 1          1    [ R32 - R23 ]
00182       th = cos^{-1} --------,  r =  ------ [ R13 - R31 ], w = th r.
00183                         2           2 sth  [ R12 - R21 ]
00184      
00185       sth = sin(th)
00186      
00187        dw    th*cth-sth      dw     th   [di3 dj2 - di2 dj3]
00188       ---- = ---------- r,  ---- = ----- [di1 dj3 - di3 dj1].
00189       dRii     2 sth^2      dRij   2 sth [di1 dj2 - di2 dj1]
00190      
00191       trace(A) < -1 only for small num. errors.
00192   */
00193 
00194 #define OM(i)    om_pt[(i)]
00195 #define DOM(i,j) dom_pt[(i)+3*(j)]
00196 #define R(i,j)   R_pt[(i)+3*(j)]
00197 #define W(i,j)   W_pt[(i)+3*(j)]
00198 
00199   const double small = 1e-6 ;
00200 
00201   double th = acos
00202     (0.5*(VL_MAX(R(0,0)+R(1,1)+R(2,2),-1.0) - 1.0)) ;
00203   
00204   double sth = sin(th) ;
00205   double cth = cos(th) ;
00206 
00207   if(fabs(sth) < small && cth < 0) {
00208     /*
00209       we have this singularity when the rotation  is about pi (or -pi)
00210       we use the fact that in this case
00211 
00212       hat( sqrt(1-cth) * r )^2 = W = (0.5*(R+R') - eye(3)) 
00213       
00214       which gives
00215 
00216       (1-cth) rx^2 = 0.5 * (W(1,1)-W(2,2)-W(3,3))
00217       (1-cth) ry^2 = 0.5 * (W(2,2)-W(3,3)-W(1,1)) 
00218       (1-cth) rz^2 = 0.5 * (W(3,3)-W(1,1)-W(2,2)) 
00219     */
00220     
00221     double W_pt [9], x, y, z ;
00222     W_pt[0] = 0.5*( R(0,0) + R(0,0) ) - 1.0 ;    
00223     W_pt[0] = 0.5*( R(1,0) + R(0,1) ) ;
00224     W_pt[0] = 0.5*( R(2,0) + R(0,2) );
00225     
00226     W_pt[0] = 0.5*( R(0,1) + R(1,0) );
00227     W_pt[0] = 0.5*( R(1,1) + R(1,1) ) - 1.0;
00228     W_pt[0] = 0.5*( R(2,1) + R(1,2) );
00229   
00230     W_pt[0] =  0.5*( R(0,2) + R(2,0) ) ;
00231     W_pt[0] =  0.5*( R(1,2) + R(2,1) ) ;
00232     W_pt[0] =  0.5*( R(2,2) + R(2,2) ) - 1.0 ;
00233 
00234     /* these are only absolute values */
00235     x = sqrt( 0.5 * (W(0,0)-W(1,1)-W(2,2)) ) ;
00236     y = sqrt( 0.5 * (W(1,1)-W(2,2)-W(0,0)) ) ;
00237     z = sqrt( 0.5 * (W(2,2)-W(0,0)-W(1,1)) ) ;
00238     
00239     /* set the biggest component to + and use the element of the
00240     ** matrix W to determine the sign of the other components
00241     ** then the solution is either (x,y,z) or its opposite */
00242     if( x >= y && x >= z ) {
00243       y = (W(1,0) >=0) ? y : -y ;
00244       z = (W(2,0) >=0) ? z : -z ;      
00245     } else if( y >= x && y >= z ) {
00246       z = (W(2,1) >=0) ? z : -z ;
00247       x = (W(1,0) >=0) ? x : -x ;
00248     } else {
00249       x = (W(2,0) >=0) ? x : -x ;
00250       y = (W(2,1) >=0) ? y : -y ;
00251     }
00252 
00253     /* we are left to chose between (x,y,z) and (-x,-y,-z)
00254     ** unfortunately we cannot (as the rotation is too close to pi) and
00255     ** we just keep what we have. */
00256     {
00257       double scale = th / sqrt( 1 - cth ) ; 
00258       OM(0) = scale * x ;
00259       OM(1) = scale * y ;
00260       OM(2) = scale * z ;
00261       
00262       if( dom_pt ) {
00263         int k ;
00264         for(k=0; k<3*9; ++k)
00265           dom_pt [k] = VL_NAN_D ;
00266       }
00267       return ;
00268     }
00269 
00270   } else {
00271     double a = (fabs(sth) < small) ? 1 : th/sin(th) ;
00272     double b ;
00273     OM(0) = 0.5*a*(R(2,1) - R(1,2)) ;
00274     OM(1) = 0.5*a*(R(0,2) - R(2,0)) ;
00275     OM(2) = 0.5*a*(R(1,0) - R(0,1)) ;
00276     
00277     if( dom_pt ) {
00278       if( fabs(sth) < small ) {
00279         a = 0.5 ;
00280         b = 0 ;
00281       } else {
00282         a = th/(2*sth) ;
00283         b = (th*cth - sth)/(2*sth*sth)/th ;
00284       }
00285 
00286       DOM(0,0) = b*OM(0) ;
00287       DOM(1,0) = b*OM(1) ;
00288       DOM(2,0) = b*OM(2) ;
00289         
00290       DOM(0,1) = 0 ;
00291       DOM(1,1) = 0 ;
00292       DOM(2,1) = a ;
00293 
00294       DOM(0,2) = 0 ;
00295       DOM(1,2) = -a ;
00296       DOM(2,2) = 0 ;
00297 
00298       DOM(0,3) = 0 ;
00299       DOM(1,3) = 0 ;
00300       DOM(2,3) = -a ;
00301       
00302       DOM(0,4) = b*OM(0) ;
00303       DOM(1,4) = b*OM(1) ;
00304       DOM(2,4) = b*OM(2) ;
00305         
00306       DOM(0,5) = a ;
00307       DOM(1,5) = 0 ;
00308       DOM(2,5) = 0 ;
00309 
00310       DOM(0,6) = 0 ;
00311       DOM(1,6) = a ;
00312       DOM(2,6) = 0 ;
00313 
00314       DOM(0,7) = -a ;
00315       DOM(1,7) = 0 ;
00316       DOM(2,7) = 0 ;
00317 
00318       DOM(0,8) = b*OM(0) ;
00319       DOM(1,8) = b*OM(1) ;
00320       DOM(2,8) = b*OM(2) ;
00321     }
00322   }
00323 
00324 #undef OM
00325 #undef DOM
00326 #undef R
00327 #undef W
00328 }
Copyright © 2008 Andrea Vedaldi and Brian Fulkerson