rodrigues.c

Go to the documentation of this file.

/*
Copyright (C) 2007-13 Andrea Vedaldi and Brian Fulkerson.
All rights reserved.

This file is part of the VLFeat library and is made available under
the terms of the BSD license (see the COPYING file).
*/

#include "generic.h"
#include "mathop.h"
#include "rodrigues.h"

#include <math.h>

void
vl_rodrigues(double* R_pt, double* dR_pt, const double* om_pt)
{
  /*
    Let

       th = |om|,  r=w/th,
       sth=sin(th),  cth=cos(th),
       ^om = hat(om)

    Then the rodrigues formula is an expansion of the exponential
    function:

     rodrigues(om) = exp ^om = I + ^r sth + ^r^2 (1 - cth).

    The derivative can be computed by elementary means and
    results:

    d(vec rodrigues(om))    sth  d ^r    1 - cth  d (^r)^2
    -------------------- =  ---- ----- + -------  -------- +
          d om^T             th  d r^T     th      d r^T

                          sth                     1 - cth
          + vec^r (cth - -----) + vec^r^2 (sth - 2-------)r^T
                          th                         th
  */

#define OM(i)   om_pt[(i)]
#define R(i,j)  R_pt[(i)+3*(j)]
#define DR(i,j) dR_pt[(i)+9*(j)]
#undef small

  const double small = 1e-6 ;

  double th = sqrt( OM(0)*OM(0) +
                    OM(1)*OM(1) +
                    OM(2)*OM(2) ) ;

  if( th < small ) {
    R(0,0) = 1.0 ; R(0,1) = 0.0 ; R(0,2) = 0.0 ;
    R(1,0) = 0.0 ; R(1,1) = 1.0 ; R(1,2) = 0.0 ;
    R(2,0) = 0.0 ; R(2,1) = 0.0 ; R(2,2) = 1.0 ;

    if(dR_pt) {
      DR(0,0) = 0  ; DR(0,1) = 0   ; DR(0,2) = 0 ;
      DR(1,0) = 0  ; DR(1,1) = 0   ; DR(1,2) = 1 ;
      DR(2,0) = 0  ; DR(2,1) = -1  ; DR(2,2) = 0 ;

      DR(3,0) = 0  ; DR(3,1) = 0   ; DR(3,2) = -1 ;
      DR(4,0) = 0  ; DR(4,1) = 0   ; DR(4,2) = 0 ;
      DR(5,0) = 1  ; DR(5,1) = 0   ; DR(5,2) = 0 ;

      DR(6,0) = 0  ; DR(6,1) = 1   ; DR(6,2) = 0 ;
      DR(7,0) = -1 ; DR(7,1) = 0   ; DR(7,2) = 0 ;
      DR(8,0) = 0  ; DR(8,1) = 0   ; DR(8,2) = 0 ;
    }
    return ;
  }

  {
    double x = OM(0) / th ;
    double y = OM(1) / th ;
    double z = OM(2) / th ;

    double xx = x*x ;
    double xy = x*y ;
    double xz = x*z ;
    double yy = y*y ;
    double yz = y*z ;
    double zz = z*z ;

    const double yx = xy ;
    const double zx = xz ;
    const double zy = yz ;

    double sth  = sin(th) ;
    double cth  = cos(th) ;
    double mcth = 1.0 - cth ;

    R(0,0) = 1          - mcth * (yy+zz) ;
    R(1,0) =     sth*z  + mcth * xy ;
    R(2,0) =   - sth*y  + mcth * xz ;

    R(0,1) =   - sth*z  + mcth * yx ;
    R(1,1) = 1          - mcth * (zz+xx) ;
    R(2,1) =     sth*x  + mcth * yz ;

    R(0,2) =     sth*y  + mcth * xz ;
    R(1,2) =   - sth*x  + mcth * yz ;
    R(2,2) = 1          - mcth * (xx+yy) ;

    if(dR_pt) {
      double a =  sth / th ;
      double b = mcth / th ;
      double c = cth - a ;
      double d = sth - 2*b ;

      DR(0,0) =                         - d * (yy+zz) * x ;
      DR(1,0) =        b*y   + c * zx   + d * xy      * x ;
      DR(2,0) =        b*z   - c * yx   + d * xz      * x ;

      DR(3,0) =        b*y   - c * zx   + d * xy      * x ;
      DR(4,0) =     -2*b*x              - d * (zz+xx) * x ;
      DR(5,0) =  a           + c * xx   + d * yz      * x ;

      DR(6,0) =        b*z   + c * yx   + d * zx      * x ;
      DR(7,0) = -a           - c * xx   + d * zy      * x ;
      DR(8,0) =     -2*b*x              - d * (yy+xx) * x ;

      DR(0,1) =     -2*b*y              - d * (yy+zz) * y ;
      DR(1,1) =        b*x   + c * zy   + d * xy      * y ;
      DR(2,1) = -a           - c * yy   + d * xz      * y ;

      DR(3,1) =        b*x   - c * zy   + d * xy      * y ;
      DR(4,1) =                         - d * (zz+xx) * y ;
      DR(5,1) =        b*z   + c * xy   + d * yz      * y ;

      DR(6,1) = a            + c * yy   + d * zx      * y ;
      DR(7,1) =        b*z   - c * xy   + d * zy      * y ;
      DR(8,1) =     -2*b*y              - d * (yy+xx) * y ;

      DR(0,2) =     -2*b*z              - d * (yy+zz) * z ;
      DR(1,2) =  a           + c * zz   + d * xy      * z ;
      DR(2,2) =        b*x   - c * yz   + d * xz      * z ;

      DR(3,2) =  -a          - c * zz   + d * xy      * z ;
      DR(4,2) =     -2*b*z              - d * (zz+xx) * z ;
      DR(5,2) =        b*y   + c * xz   + d * yz      * z ;

      DR(6,2) =        b*x   + c * yz   + d * zx      * z ;
      DR(7,2) =        b*y   - c * xz   + d * zy      * z ;
      DR(8,2) =                         - d * (yy+xx) * z ;
    }
  }

#undef OM
#undef R
#undef DR

}

VL_EXPORT
void vl_irodrigues(double* om_pt, double* dom_pt, const double* R_pt)
{
  /*
                    tr R - 1          1    [ R32 - R23 ]
      th = cos^{-1} --------,  r =  ------ [ R13 - R31 ], w = th r.
                        2           2 sth  [ R12 - R21 ]

      sth = sin(th)

       dw    th*cth-sth      dw     th   [di3 dj2 - di2 dj3]
      ---- = ---------- r,  ---- = ----- [di1 dj3 - di3 dj1].
      dRii     2 sth^2      dRij   2 sth [di1 dj2 - di2 dj1]

      trace(A) < -1 only for small num. errors.
  */

#define OM(i)    om_pt[(i)]
#define DOM(i,j) dom_pt[(i)+3*(j)]
#define R(i,j)   R_pt[(i)+3*(j)]
#define W(i,j)   W_pt[(i)+3*(j)]

  const double small = 1e-6 ;

  double th = acos
    (0.5*(VL_MAX(R(0,0)+R(1,1)+R(2,2),-1.0) - 1.0)) ;

  double sth = sin(th) ;
  double cth = cos(th) ;

  if(fabs(sth) < small && cth < 0) {
    /*
      we have this singularity when the rotation  is about pi (or -pi)
      we use the fact that in this case

      hat( sqrt(1-cth) * r )^2 = W = (0.5*(R+R') - eye(3))

      which gives

      (1-cth) rx^2 = 0.5 * (W(1,1)-W(2,2)-W(3,3))
      (1-cth) ry^2 = 0.5 * (W(2,2)-W(3,3)-W(1,1))
      (1-cth) rz^2 = 0.5 * (W(3,3)-W(1,1)-W(2,2))
    */

    double W_pt [9], x, y, z ;
    W_pt[0] = 0.5*( R(0,0) + R(0,0) ) - 1.0 ;
    W_pt[1] = 0.5*( R(1,0) + R(0,1) ) ;
    W_pt[2] = 0.5*( R(2,0) + R(0,2) );

    W_pt[3] = 0.5*( R(0,1) + R(1,0) );
    W_pt[4] = 0.5*( R(1,1) + R(1,1) ) - 1.0;
    W_pt[5] = 0.5*( R(2,1) + R(1,2) );

    W_pt[6] =  0.5*( R(0,2) + R(2,0) ) ;
    W_pt[7] =  0.5*( R(1,2) + R(2,1) ) ;
    W_pt[8] =  0.5*( R(2,2) + R(2,2) ) - 1.0 ;

    /* these are only absolute values */
    x = sqrt( 0.5 * (W(0,0)-W(1,1)-W(2,2)) ) ;
    y = sqrt( 0.5 * (W(1,1)-W(2,2)-W(0,0)) ) ;
    z = sqrt( 0.5 * (W(2,2)-W(0,0)-W(1,1)) ) ;

    /* set the biggest component to + and use the element of the
    ** matrix W to determine the sign of the other components
    ** then the solution is either (x,y,z) or its opposite */
    if( x >= y && x >= z ) {
      y = (W(1,0) >=0) ? y : -y ;
      z = (W(2,0) >=0) ? z : -z ;
    } else if( y >= x && y >= z ) {
      z = (W(2,1) >=0) ? z : -z ;
      x = (W(1,0) >=0) ? x : -x ;
    } else {
      x = (W(2,0) >=0) ? x : -x ;
      y = (W(2,1) >=0) ? y : -y ;
    }

    /* we are left to chose between (x,y,z) and (-x,-y,-z)
    ** unfortunately we cannot (as the rotation is too close to pi) and
    ** we just keep what we have. */
    {
      double scale = th / sqrt( 1 - cth ) ;
      OM(0) = scale * x ;
      OM(1) = scale * y ;
      OM(2) = scale * z ;

      if( dom_pt ) {
        int k ;
        for(k=0; k<3*9; ++k)
          dom_pt [k] = VL_NAN_D ;
      }
      return ;
    }

  } else {
    double a = (fabs(sth) < small) ? 1 : th/sin(th) ;
    double b ;
    OM(0) = 0.5*a*(R(2,1) - R(1,2)) ;
    OM(1) = 0.5*a*(R(0,2) - R(2,0)) ;
    OM(2) = 0.5*a*(R(1,0) - R(0,1)) ;

    if( dom_pt ) {
      if( fabs(sth) < small ) {
        a = 0.5 ;
        b = 0 ;
      } else {
        a = th/(2*sth) ;
        b = (th*cth - sth)/(2*sth*sth)/th ;
      }

      DOM(0,0) = b*OM(0) ;
      DOM(1,0) = b*OM(1) ;
      DOM(2,0) = b*OM(2) ;

      DOM(0,1) = 0 ;
      DOM(1,1) = 0 ;
      DOM(2,1) = a ;

      DOM(0,2) = 0 ;
      DOM(1,2) = -a ;
      DOM(2,2) = 0 ;

      DOM(0,3) = 0 ;
      DOM(1,3) = 0 ;
      DOM(2,3) = -a ;

      DOM(0,4) = b*OM(0) ;
      DOM(1,4) = b*OM(1) ;
      DOM(2,4) = b*OM(2) ;

      DOM(0,5) = a ;
      DOM(1,5) = 0 ;
      DOM(2,5) = 0 ;

      DOM(0,6) = 0 ;
      DOM(1,6) = a ;
      DOM(2,6) = 0 ;

      DOM(0,7) = -a ;
      DOM(1,7) = 0 ;
      DOM(2,7) = 0 ;

      DOM(0,8) = b*OM(0) ;
      DOM(1,8) = b*OM(1) ;
      DOM(2,8) = b*OM(2) ;
    }
  }

#undef OM
#undef DOM
#undef R
#undef W
}