00001
00006
00007
00008
00009
00010
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
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
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
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
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
00210
00211
00212
00213
00214
00215
00216
00217
00218
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
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
00240
00241
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
00254
00255
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 }