Visual Servoing Platform  version 3.2.1 under development (2019-08-21) under development (2019-08-21)
vpExponentialMap.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Exponential map.
33  *
34  * Authors:
35  * Fabien Spindler
36  * Francois Chaumette
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpExponentialMap.h>
41 
60 
80 {
81  double theta, si, co, sinc, mcosc, msinc;
85 
86  vpColVector v_dt = v * delta_t;
87 
88  u[0] = v_dt[3];
89  u[1] = v_dt[4];
90  u[2] = v_dt[5];
91  rd.buildFrom(u);
92 
93  theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
94  si = sin(theta);
95  co = cos(theta);
96  sinc = vpMath::sinc(si, theta);
97  mcosc = vpMath::mcosc(co, theta);
98  msinc = vpMath::msinc(si, theta);
99 
100  dt[0] = v_dt[0] * (sinc + u[0] * u[0] * msinc) + v_dt[1] * (u[0] * u[1] * msinc - u[2] * mcosc) +
101  v_dt[2] * (u[0] * u[2] * msinc + u[1] * mcosc);
102 
103  dt[1] = v_dt[0] * (u[0] * u[1] * msinc + u[2] * mcosc) + v_dt[1] * (sinc + u[1] * u[1] * msinc) +
104  v_dt[2] * (u[1] * u[2] * msinc - u[0] * mcosc);
105 
106  dt[2] = v_dt[0] * (u[0] * u[2] * msinc - u[1] * mcosc) + v_dt[1] * (u[1] * u[2] * msinc + u[0] * mcosc) +
107  v_dt[2] * (sinc + u[2] * u[2] * msinc);
108 
109  vpHomogeneousMatrix Delta;
110  Delta.insert(rd);
111  Delta.insert(dt);
112 
113  if (0) // test new version wrt old version
114  {
115  // old version
116  unsigned int i, j;
117 
118  double s;
119  // double u[3];
120  // vpRotationMatrix rd ;
121  // vpTranslationVector dt ;
122 
123  s = sqrt(v_dt[3] * v_dt[3] + v_dt[4] * v_dt[4] + v_dt[5] * v_dt[5]);
124  if (s > 1.e-15) {
125  for (i = 0; i < 3; i++)
126  u[i] = v_dt[3 + i] / s;
127  double sinu = sin(s);
128  double cosi = cos(s);
129  double mcosi = 1 - cosi;
130  rd[0][0] = cosi + mcosi * u[0] * u[0];
131  rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
132  rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
133  rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
134  rd[1][1] = cosi + mcosi * u[1] * u[1];
135  rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
136  rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
137  rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
138  rd[2][2] = cosi + mcosi * u[2] * u[2];
139 
140  dt[0] = v_dt[0] * (sinu / s + u[0] * u[0] * (1 - sinu / s)) +
141  v_dt[1] * (u[0] * u[1] * (1 - sinu / s) - u[2] * mcosi / s) +
142  v_dt[2] * (u[0] * u[2] * (1 - sinu / s) + u[1] * mcosi / s);
143 
144  dt[1] = v_dt[0] * (u[0] * u[1] * (1 - sinu / s) + u[2] * mcosi / s) +
145  v_dt[1] * (sinu / s + u[1] * u[1] * (1 - sinu / s)) +
146  v_dt[2] * (u[1] * u[2] * (1 - sinu / s) - u[0] * mcosi / s);
147 
148  dt[2] = v_dt[0] * (u[0] * u[2] * (1 - sinu / s) - u[1] * mcosi / s) +
149  v_dt[1] * (u[1] * u[2] * (1 - sinu / s) + u[0] * mcosi / s) +
150  v_dt[2] * (sinu / s + u[2] * u[2] * (1 - sinu / s));
151  } else {
152  for (i = 0; i < 3; i++) {
153  for (j = 0; j < 3; j++)
154  rd[i][j] = 0.0;
155  rd[i][i] = 1.0;
156  dt[i] = v_dt[i];
157  }
158  }
159  // end old version
160 
161  // Test of the new version
162  vpHomogeneousMatrix Delta_old;
163  Delta_old.insert(rd);
164  Delta_old.insert(dt);
165 
166  int pb = 0;
167  for (i = 0; i < 4; i++) {
168  for (j = 0; j < 4; j++)
169  if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5)
170  pb = 1;
171  }
172  if (pb == 1) {
173  printf("pb vpHomogeneousMatrix::expMap\n");
174  std::cout << " Delta : " << std::endl << Delta << std::endl;
175  std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
176  }
177  // end of the test
178  }
179 
180  return Delta;
181 }
182 
198 
217 {
218  vpColVector v(6);
219  unsigned int i;
220  double theta, si, co, sinc, mcosc, msinc, det;
221  vpThetaUVector u;
222  vpRotationMatrix Rd, a;
224 
225  M.extract(Rd);
226  u.buildFrom(Rd);
227  for (i = 0; i < 3; i++)
228  v[3 + i] = u[i];
229 
230  theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
231  si = sin(theta);
232  co = cos(theta);
233  sinc = vpMath::sinc(si, theta);
234  mcosc = vpMath::mcosc(co, theta);
235  msinc = vpMath::msinc(si, theta);
236 
237  // a below is not a pure rotation matrix, even if not so far from
238  // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
239  // with V = t.U
240 
241  a[0][0] = sinc + u[0] * u[0] * msinc;
242  a[0][1] = u[0] * u[1] * msinc - u[2] * mcosc;
243  a[0][2] = u[0] * u[2] * msinc + u[1] * mcosc;
244 
245  a[1][0] = u[0] * u[1] * msinc + u[2] * mcosc;
246  a[1][1] = sinc + u[1] * u[1] * msinc;
247  a[1][2] = u[1] * u[2] * msinc - u[0] * mcosc;
248 
249  a[2][0] = u[0] * u[2] * msinc - u[1] * mcosc;
250  a[2][1] = u[1] * u[2] * msinc + u[0] * mcosc;
251  a[2][2] = sinc + u[2] * u[2] * msinc;
252 
253  det = a[0][0] * a[1][1] * a[2][2] + a[1][0] * a[2][1] * a[0][2] + a[0][1] * a[1][2] * a[2][0] -
254  a[2][0] * a[1][1] * a[0][2] - a[1][0] * a[0][1] * a[2][2] - a[0][0] * a[2][1] * a[1][2];
255 
256  if (fabs(det) > 1.e-5) {
257  v[0] = (M[0][3] * a[1][1] * a[2][2] + M[1][3] * a[2][1] * a[0][2] + M[2][3] * a[0][1] * a[1][2] -
258  M[2][3] * a[1][1] * a[0][2] - M[1][3] * a[0][1] * a[2][2] - M[0][3] * a[2][1] * a[1][2]) /
259  det;
260  v[1] = (a[0][0] * M[1][3] * a[2][2] + a[1][0] * M[2][3] * a[0][2] + M[0][3] * a[1][2] * a[2][0] -
261  a[2][0] * M[1][3] * a[0][2] - a[1][0] * M[0][3] * a[2][2] - a[0][0] * M[2][3] * a[1][2]) /
262  det;
263  v[2] = (a[0][0] * a[1][1] * M[2][3] + a[1][0] * a[2][1] * M[0][3] + a[0][1] * M[1][3] * a[2][0] -
264  a[2][0] * a[1][1] * M[0][3] - a[1][0] * a[0][1] * M[2][3] - a[0][0] * a[2][1] * M[1][3]) /
265  det;
266  } else {
267  v[0] = M[0][3];
268  v[1] = M[1][3];
269  v[2] = M[2][3];
270  }
271 
272  // Apply the sampling time to the computed velocity
273  v /= delta_t;
274 
275  return (v);
276 }
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Definition: vpMath.cpp:170
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:135
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static vpHomogeneousMatrix direct(const vpColVector &v)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
static double msinc(double sinx, double x)
Definition: vpMath.cpp:153