/** file: rodirgues.c ** author: Andrea Vedaldi ** description: Rodrigues formulas **/ /* AUTORIGHTS Copyright (C) 2006 Andrea Vedaldi This file is part of VLUtil. VLUtil is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. */ #include"rodrigues.h" #include #include /** @brief Rodrigues' formula **/ 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)] 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 } /** @brief Inverse Rodrigues formula ** ** This function computes the Rodrigues formula of the ** argument @a om_pt. The result is stored int the matrix ** @a R_pt. If @a dR_pt is non null, then the derivative ** of the Rodrigues formula is computed and stored into ** the matrix @a dR_pt. ** ** @param R_pt pointer to a 3x3 matrix (array of 9 doubles). ** @param dR_pt pointer to a 9x3 matrix (array of 27 doubles). ** @param om_pt pointer to a 3 vector (array of 3 dobules). **/ 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*(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[0] = 0.5*( R(1,0) + R(0,1) ) ; W_pt[0] = 0.5*( R(2,0) + R(0,2) ); W_pt[0] = 0.5*( R(0,1) + R(1,0) ); W_pt[0] = 0.5*( R(1,1) + R(1,1) ) - 1.0; W_pt[0] = 0.5*( R(2,1) + R(1,2) ); W_pt[0] = 0.5*( R(0,2) + R(2,0) ) ; W_pt[0] = 0.5*( R(1,2) + R(2,1) ) ; W_pt[0] = 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] = NAN ; } 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 }