source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/vlutil/toolbox/bits/rodrigues.c @ 37

Last change on this file since 37 was 37, checked in by (none), 14 years ago

Added original make3d
File size: 9.4 KB
Line 
1/** file:        rodirgues.c
2 ** author:      Andrea Vedaldi
3 ** description: Rodrigues formulas
4 **/
5
6/* AUTORIGHTS
7Copyright (C) 2006 Andrea Vedaldi
8     
9This file is part of VLUtil.
10
11VLUtil is free software; you can redistribute it and/or modify
12it under the terms of the GNU General Public License as published by
13the Free Software Foundation; either version 2, or (at your option)
14any later version.
15
16This program is distributed in the hope that it will be useful,
17but WITHOUT ANY WARRANTY; without even the implied warranty of
18MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19GNU General Public License for more details.
20
21You should have received a copy of the GNU General Public License
22along with this program; if not, write to the Free Software Foundation,
23Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
24*/
25
26#include"rodrigues.h"
27#include<math.h>
28#include<limits.h>
29
30/** @brief Rodrigues' formula
31 **/
32void 
33vl_rodrigues(double* R_pt, double* dR_pt, const double* om_pt)
34{
35  /*
36    Let
37     
38       th = |om|,  r=w/th, 
39       sth=sin(th),  cth=cos(th),
40       ^om = hat(om)
41
42    Then the rodrigues formula is an expansion of the exponential
43    function:
44     
45     rodrigues(om) = exp ^om = I + ^r sth + ^r^2 (1 - cth).
46
47    The derivative can be computed by elementary means and
48    results:
49
50    d(vec rodrigues(om))    sth  d ^r    1 - cth  d (^r)^2
51    -------------------- =  ---- ----- + -------  -------- +
52          d om^T             th  d r^T     th      d r^T
53                 
54                          sth                     1 - cth
55          + vec^r (cth - -----) + vec^r^2 (sth - 2-------)r^T
56                          th                         th
57  */
58
59#define OM(i)   om_pt[(i)]
60#define R(i,j)  R_pt[(i)+3*(j)]
61#define DR(i,j) dR_pt[(i)+9*(j)]
62
63  const double small = 1e-6 ;
64
65  double th = sqrt( OM(0)*OM(0) + 
66                    OM(1)*OM(1) +
67                    OM(2)*OM(2) ) ;
68 
69  if( th < small ) {
70    R(0,0) = 1.0 ; R(0,1) = 0.0 ; R(0,2) = 0.0 ;
71    R(1,0) = 0.0 ; R(1,1) = 1.0 ; R(1,2) = 0.0 ;
72    R(2,0) = 0.0 ; R(2,1) = 0.0 ; R(2,2) = 1.0 ;
73   
74    if(dR_pt) {
75      DR(0,0) = 0  ; DR(0,1) = 0   ; DR(0,2) = 0 ;
76      DR(1,0) = 0  ; DR(1,1) = 0   ; DR(1,2) = 1 ;
77      DR(2,0) = 0  ; DR(2,1) = -1  ; DR(2,2) = 0 ;
78
79      DR(3,0) = 0  ; DR(3,1) = 0   ; DR(3,2) = -1 ;
80      DR(4,0) = 0  ; DR(4,1) = 0   ; DR(4,2) = 0 ;
81      DR(5,0) = 1  ; DR(5,1) = 0   ; DR(5,2) = 0 ;
82
83      DR(6,0) = 0  ; DR(6,1) = 1   ; DR(6,2) = 0 ;
84      DR(7,0) = -1 ; DR(7,1) = 0   ; DR(7,2) = 0 ;
85      DR(8,0) = 0  ; DR(8,1) = 0   ; DR(8,2) = 0 ;
86    }
87    return ;
88  }
89
90  {
91    double x = OM(0) / th ;
92    double y = OM(1) / th ;
93    double z = OM(2) / th ;
94   
95    double xx = x*x ;
96    double xy = x*y ;
97    double xz = x*z ;
98    double yy = y*y ;
99    double yz = y*z ;
100    double zz = z*z ;
101   
102    const double yx = xy ;
103    const double zx = xz ;
104    const double zy = yz ;
105   
106    double sth  = sin(th) ;
107    double cth  = cos(th) ;
108    double mcth = 1.0 - cth ; 
109   
110    R(0,0) = 1          - mcth * (yy+zz) ;
111    R(1,0) =     sth*z  + mcth * xy ;
112    R(2,0) =   - sth*y  + mcth * xz ;
113   
114    R(0,1) =   - sth*z  + mcth * yx ;
115    R(1,1) = 1          - mcth * (zz+xx) ;
116    R(2,1) =     sth*x  + mcth * yz ;
117   
118    R(0,2) =     sth*y  + mcth * xz ;
119    R(1,2) =   - sth*x  + mcth * yz ;
120    R(2,2) = 1          - mcth * (xx+yy) ;
121   
122    if(dR_pt) {
123      double a =  sth / th ;
124      double b = mcth / th ;
125      double c = cth - a ;
126      double d = sth - 2*b ;
127     
128      DR(0,0) =                         - d * (yy+zz) * x ;
129      DR(1,0) =        b*y   + c * zx   + d * xy      * x ;
130      DR(2,0) =        b*z   - c * yx   + d * xz      * x ;
131     
132      DR(3,0) =        b*y   - c * zx   + d * xy      * x ;
133      DR(4,0) =     -2*b*x              - d * (zz+xx) * x ;
134      DR(5,0) =  a           + c * xx   + d * yz      * x ;
135     
136      DR(6,0) =        b*z   + c * yx   + d * zx      * x ;
137      DR(7,0) = -a           - c * xx   + d * zy      * x ;
138      DR(8,0) =     -2*b*x              - d * (yy+xx) * x ;
139     
140      DR(0,1) =     -2*b*y              - d * (yy+zz) * y ;
141      DR(1,1) =        b*x   + c * zy   + d * xy      * y ;
142      DR(2,1) = -a           - c * yy   + d * xz      * y ;
143     
144      DR(3,1) =        b*x   - c * zy   + d * xy      * y ;
145      DR(4,1) =                         - d * (zz+xx) * y ;
146      DR(5,1) =        b*z   + c * xy   + d * yz      * y ;
147     
148      DR(6,1) = a            + c * yy   + d * zx      * y ;
149      DR(7,1) =        b*z   - c * xy   + d * zy      * y ;
150      DR(8,1) =     -2*b*y              - d * (yy+xx) * y ;
151     
152      DR(0,2) =     -2*b*z              - d * (yy+zz) * z ;
153      DR(1,2) =  a           + c * zz   + d * xy      * z ;
154      DR(2,2) =        b*x   - c * yz   + d * xz      * z ;
155     
156      DR(3,2) =  -a          - c * zz   + d * xy      * z ;
157      DR(4,2) =     -2*b*z              - d * (zz+xx) * z ;
158      DR(5,2) =        b*y   + c * xz   + d * yz      * z ;
159     
160      DR(6,2) =        b*x   + c * yz   + d * zx      * z ;
161      DR(7,2) =        b*y   - c * xz   + d * zy      * z ;
162      DR(8,2) =                         - d * (yy+xx) * z ;
163    }
164  }
165
166#undef OM
167#undef R
168#undef DR
169
170}
171
172/** @brief Inverse Rodrigues formula
173 **
174 ** This function computes the Rodrigues formula of the
175 ** argument @a om_pt. The result is stored int the matrix
176 ** @a R_pt. If @a dR_pt is non null, then the derivative
177 ** of the Rodrigues formula is computed and stored into
178 ** the matrix @a dR_pt.
179 **
180 ** @param R_pt pointer to a 3x3 matrix (array of 9 doubles).
181 ** @param dR_pt pointer to a 9x3 matrix (array of 27 doubles).
182 ** @param om_pt pointer to a 3 vector (array of 3 dobules).
183 **/
184
185void vl_irodrigues(double* om_pt, double* dom_pt, const double* R_pt)
186{
187  /*
188                    tr R - 1          1    [ R32 - R23 ]
189      th = cos^{-1} --------,  r =  ------ [ R13 - R31 ], w = th r.
190                        2           2 sth  [ R12 - R21 ]
191     
192      sth = sin(th)
193     
194       dw    th*cth-sth      dw     th   [di3 dj2 - di2 dj3]
195      ---- = ---------- r,  ---- = ----- [di1 dj3 - di3 dj1].
196      dRii     2 sth^2      dRij   2 sth [di1 dj2 - di2 dj1]
197     
198      trace(A) < -1 only for small num. errors.
199  */
200
201#define OM(i)    om_pt[(i)]
202#define DOM(i,j) dom_pt[(i)+3*(j)]
203#define R(i,j)   R_pt[(i)+3*(j)]
204#define W(i,j)   W_pt[(i)+3*(j)]
205
206  const double small = 1e-6 ;
207
208  double th = acos
209    (0.5*(MAX(R(0,0)+R(1,1)+R(2,2),-1.0) - 1.0)) ;
210 
211  double sth = sin(th) ;
212  double cth = cos(th) ;
213
214  if(fabs(sth) < small && cth < 0) {
215    /*
216      we have this singularity when the rotation  is about pi (or -pi)
217      we use the fact that in this case
218
219      hat( sqrt(1-cth) * r )^2 = W = (0.5*(R+R') - eye(3))
220     
221      which gives
222
223      (1-cth) rx^2 = 0.5 * (W(1,1)-W(2,2)-W(3,3))
224      (1-cth) ry^2 = 0.5 * (W(2,2)-W(3,3)-W(1,1))
225      (1-cth) rz^2 = 0.5 * (W(3,3)-W(1,1)-W(2,2))
226    */
227   
228    double W_pt [9], x, y, z ;
229    W_pt[0] = 0.5*( R(0,0) + R(0,0) ) - 1.0 ;
230   
231    W_pt[0] = 0.5*( R(1,0) + R(0,1) ) ;
232    W_pt[0] = 0.5*( R(2,0) + R(0,2) );
233   
234    W_pt[0] = 0.5*( R(0,1) + R(1,0) );
235    W_pt[0] = 0.5*( R(1,1) + R(1,1) ) - 1.0;
236    W_pt[0] = 0.5*( R(2,1) + R(1,2) );
237 
238    W_pt[0] =  0.5*( R(0,2) + R(2,0) ) ;
239    W_pt[0] =  0.5*( R(1,2) + R(2,1) ) ;
240    W_pt[0] =  0.5*( R(2,2) + R(2,2) ) - 1.0 ;
241
242    /* these are only absolute values */
243    x = sqrt( 0.5 * (W(0,0)-W(1,1)-W(2,2)) ) ;
244    y = sqrt( 0.5 * (W(1,1)-W(2,2)-W(0,0)) ) ;
245    z = sqrt( 0.5 * (W(2,2)-W(0,0)-W(1,1)) ) ;
246   
247    /* set the biggest component to + and use the element of the
248    ** matrix W to determine the sign of the other components
249    ** then the solution is either (x,y,z) or its opposite */
250    if( x >= y && x >= z ) {
251      y = (W(1,0) >=0) ? y : -y ;
252      z = (W(2,0) >=0) ? z : -z ;     
253    } else if( y >= x && y >= z ) {
254      z = (W(2,1) >=0) ? z : -z ;
255      x = (W(1,0) >=0) ? x : -x ;
256    } else {
257      x = (W(2,0) >=0) ? x : -x ;
258      y = (W(2,1) >=0) ? y : -y ;
259    }
260
261    /* we are left to chose between (x,y,z) and (-x,-y,-z)
262    ** unfortunately we cannot (as the rotation is too close to pi) and
263    ** we just keep what we have. */
264    {
265      double scale = th / sqrt( 1 - cth ) ; 
266      OM(0) = scale * x ;
267      OM(1) = scale * y ;
268      OM(2) = scale * z ;
269     
270      if( dom_pt ) {
271        int k ;
272        for(k=0; k<3*9; ++k)
273          dom_pt[k] = NAN ;
274      }
275      return ;
276    }
277
278  } else {
279    double a = (fabs(sth) < small) ? 1 : th/sin(th) ;
280    double b ;
281    OM(0) = 0.5*a*(R(2,1) - R(1,2)) ;
282    OM(1) = 0.5*a*(R(0,2) - R(2,0)) ;
283    OM(2) = 0.5*a*(R(1,0) - R(0,1)) ;
284   
285    if( dom_pt ) {
286      if( fabs(sth) < small ) {
287        a = 0.5 ;
288        b = 0 ;
289      } else {
290        a = th/(2*sth) ;
291        b = (th*cth - sth)/(2*sth*sth)/th ;
292      }
293
294      DOM(0,0) = b*OM(0) ;
295      DOM(1,0) = b*OM(1) ;
296      DOM(2,0) = b*OM(2) ;
297       
298      DOM(0,1) = 0 ;
299      DOM(1,1) = 0 ;
300      DOM(2,1) = a ;
301
302      DOM(0,2) = 0 ;
303      DOM(1,2) = -a ;
304      DOM(2,2) = 0 ;
305
306      DOM(0,3) = 0 ;
307      DOM(1,3) = 0 ;
308      DOM(2,3) = -a ;
309     
310      DOM(0,4) = b*OM(0) ;
311      DOM(1,4) = b*OM(1) ;
312      DOM(2,4) = b*OM(2) ;
313       
314      DOM(0,5) = a ;
315      DOM(1,5) = 0 ;
316      DOM(2,5) = 0 ;
317
318      DOM(0,6) = 0 ;
319      DOM(1,6) = a ;
320      DOM(2,6) = 0 ;
321
322      DOM(0,7) = -a ;
323      DOM(1,7) = 0 ;
324      DOM(2,7) = 0 ;
325
326      DOM(0,8) = b*OM(0) ;
327      DOM(1,8) = b*OM(1) ;
328      DOM(2,8) = b*OM(2) ;
329    }
330  }
331
332#undef OM
333#undef DOM
334#undef R
335#undef W
336}
Note: See TracBrowser for help on using the repository browser.