ViSP  2.10.0
vpExponentialMap.cpp
1 /****************************************************************************
2  *
3  * $Id: vpExponentialMap.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Exponential map.
36  *
37  * Authors:
38  * Fabien Spindler
39  * Francois Chaumette
40  *
41  *****************************************************************************/
42 
43 #include <visp/vpExponentialMap.h>
44 
45 
66 {
67  return vpExponentialMap::direct(v, 1.0);
68 }
69 
90 vpExponentialMap::direct(const vpColVector &v, const double &delta_t)
91 {
92  double theta,si,co,sinc,mcosc,msinc;
93  vpThetaUVector u ;
94  vpRotationMatrix rd ;
96 
97  vpColVector v_dt = v * delta_t;
98 
99  u[0] = v_dt[3];
100  u[1] = v_dt[4];
101  u[2] = v_dt[5];
102  rd.buildFrom(u);
103 
104  theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
105  si = sin(theta);
106  co = cos(theta);
107  sinc = vpMath::sinc(si,theta);
108  mcosc = vpMath::mcosc(co,theta);
109  msinc = vpMath::msinc(si,theta);
110 
111  dt[0] = v_dt[0]*(sinc + u[0]*u[0]*msinc)
112  + v_dt[1]*(u[0]*u[1]*msinc - u[2]*mcosc)
113  + v_dt[2]*(u[0]*u[2]*msinc + u[1]*mcosc);
114 
115  dt[1] = v_dt[0]*(u[0]*u[1]*msinc + u[2]*mcosc)
116  + v_dt[1]*(sinc + u[1]*u[1]*msinc)
117  + v_dt[2]*(u[1]*u[2]*msinc - u[0]*mcosc);
118 
119  dt[2] = v_dt[0]*(u[0]*u[2]*msinc - u[1]*mcosc)
120  + v_dt[1]*(u[1]*u[2]*msinc + u[0]*mcosc)
121  + v_dt[2]*(sinc + u[2]*u[2]*msinc);
122 
123  vpHomogeneousMatrix Delta ;
124  Delta.insert(rd) ;
125  Delta.insert(dt) ;
126 
127 
128 
129  if (0) // test new version wrt old version
130  {
131  // old version
132  unsigned int i,j;
133 
134  double sinu,cosi,mcosi,s;
135  // double u[3];
136  // vpRotationMatrix rd ;
137  // vpTranslationVector dt ;
138 
139  s = sqrt(v_dt[3]*v_dt[3] + v_dt[4]*v_dt[4] + v_dt[5]*v_dt[5]);
140  if (s > 1.e-15)
141  {
142  for (i=0;i<3;i++) u[i] = v_dt[3+i]/s;
143  sinu = sin(s);
144  cosi = cos(s);
145  mcosi = 1-cosi;
146  rd[0][0] = cosi + mcosi*u[0]*u[0];
147  rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
148  rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
149  rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
150  rd[1][1] = cosi + mcosi*u[1]*u[1];
151  rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
152  rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
153  rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
154  rd[2][2] = cosi + mcosi*u[2]*u[2];
155 
156  dt[0] = v_dt[0]*(sinu/s + u[0]*u[0]*(1-sinu/s))
157  + v_dt[1]*(u[0]*u[1]*(1-sinu/s)-u[2]*mcosi/s)
158  + v_dt[2]*(u[0]*u[2]*(1-sinu/s)+u[1]*mcosi/s);
159 
160  dt[1] = v_dt[0]*(u[0]*u[1]*(1-sinu/s)+u[2]*mcosi/s)
161  + v_dt[1]*(sinu/s + u[1]*u[1]*(1-sinu/s))
162  + v_dt[2]*(u[1]*u[2]*(1-sinu/s)-u[0]*mcosi/s);
163 
164  dt[2] = v_dt[0]*(u[0]*u[2]*(1-sinu/s)-u[1]*mcosi/s)
165  + v_dt[1]*(u[1]*u[2]*(1-sinu/s)+u[0]*mcosi/s)
166  + v_dt[2]*(sinu/s + u[2]*u[2]*(1-sinu/s));
167  }
168  else
169  {
170  for (i=0;i<3;i++)
171  {
172  for(j=0;j<3;j++) rd[i][j] = 0.0;
173  rd[i][i] = 1.0;
174  dt[i] = v_dt[i];
175  }
176  }
177  // end old version
178 
179  // Test of the new version
180  vpHomogeneousMatrix Delta_old ;
181  Delta_old.insert(rd) ;
182  Delta_old.insert(dt) ;
183 
184  int pb = 0;
185  for (i=0;i<4;i++)
186  {
187  for(j=0;j<4;j++)
188  if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) pb = 1;
189  }
190  if (pb == 1)
191  {
192  printf("pb vpHomogeneousMatrix::expMap\n");
193  std::cout << " Delta : " << std::endl << Delta << std::endl;
194  std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
195  }
196  // end of the test
197  }
198 
199  return Delta ;
200 }
201 
219 {
220  return vpExponentialMap::inverse(M, 1.0);
221 }
222 
242 vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double &delta_t)
243 {
244  vpColVector v(6);
245  unsigned int i;
246  double theta,si,co,sinc,mcosc,msinc,det;
247  vpThetaUVector u ;
248  vpRotationMatrix Rd,a;
250 
251  M.extract(Rd);
252  u.buildFrom(Rd);
253  for (i=0;i<3;i++) v[3+i] = u[i];
254 
255  theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
256  si = sin(theta);
257  co = cos(theta);
258  sinc = vpMath::sinc(si,theta);
259  mcosc = vpMath::mcosc(co,theta);
260  msinc = vpMath::msinc(si,theta);
261 
262  // a below is not a pure rotation matrix, even if not so far from
263  // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
264  // with V = t.U
265 
266  a[0][0] = sinc + u[0]*u[0]*msinc;
267  a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
268  a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;
269 
270  a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
271  a[1][1] = sinc + u[1]*u[1]*msinc;
272  a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;
273 
274  a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
275  a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
276  a[2][2] = sinc + u[2]*u[2]*msinc;
277 
278  det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
279  + a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
280  - a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];
281 
282  if (fabs(det) > 1.e-5)
283  {
284  v[0] = (M[0][3]*a[1][1]*a[2][2]
285  + M[1][3]*a[2][1]*a[0][2]
286  + M[2][3]*a[0][1]*a[1][2]
287  - M[2][3]*a[1][1]*a[0][2]
288  - M[1][3]*a[0][1]*a[2][2]
289  - M[0][3]*a[2][1]*a[1][2])/det;
290  v[1] = (a[0][0]*M[1][3]*a[2][2]
291  + a[1][0]*M[2][3]*a[0][2]
292  + M[0][3]*a[1][2]*a[2][0]
293  - a[2][0]*M[1][3]*a[0][2]
294  - a[1][0]*M[0][3]*a[2][2]
295  - a[0][0]*M[2][3]*a[1][2])/det;
296  v[2] = (a[0][0]*a[1][1]*M[2][3]
297  + a[1][0]*a[2][1]*M[0][3]
298  + a[0][1]*M[1][3]*a[2][0]
299  - a[2][0]*a[1][1]*M[0][3]
300  - a[1][0]*a[0][1]*M[2][3]
301  - a[0][0]*a[2][1]*M[1][3])/det;
302  }
303  else
304  {
305  v[0] = M[0][3];
306  v[1] = M[1][3];
307  v[2] = M[2][3];
308  }
309 
310  // Apply the sampling time to the computed velocity
311  v /= delta_t;
312 
313  return(v);
314 }
315 
316 
317 /*
318  * Local variables:
319  * c-basic-offset: 2
320  * End:
321  */
static vpColVector inverse(const vpHomogeneousMatrix &M)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Definition: vpMath.h:301
The vpRotationMatrix considers the particular case of a rotation matrix.
void insert(const vpRotationMatrix &R)
static double mcosc(double cosx, double x)
Definition: vpMath.h:331
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Build a rotation matrix from an homogeneous matrix.
void extract(vpRotationMatrix &R) const
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
static vpHomogeneousMatrix direct(const vpColVector &v)
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
static double msinc(double sinx, double x)
Definition: vpMath.h:347