Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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  if (v.size() != 6) {
83  "Cannot compute direct exponential map from a %d-dim velocity vector. Should be 6-dim.",
84  v.size()));
85  }
86  double theta, si, co, sinc, mcosc, msinc;
90 
91  vpColVector v_dt = v * delta_t;
92 
93  u[0] = v_dt[3];
94  u[1] = v_dt[4];
95  u[2] = v_dt[5];
96  rd.buildFrom(u);
97 
98  theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
99  si = sin(theta);
100  co = cos(theta);
101  sinc = vpMath::sinc(si, theta);
102  mcosc = vpMath::mcosc(co, theta);
103  msinc = vpMath::msinc(si, theta);
104 
105  dt[0] = v_dt[0] * (sinc + u[0] * u[0] * msinc) + v_dt[1] * (u[0] * u[1] * msinc - u[2] * mcosc) +
106  v_dt[2] * (u[0] * u[2] * msinc + u[1] * mcosc);
107 
108  dt[1] = v_dt[0] * (u[0] * u[1] * msinc + u[2] * mcosc) + v_dt[1] * (sinc + u[1] * u[1] * msinc) +
109  v_dt[2] * (u[1] * u[2] * msinc - u[0] * mcosc);
110 
111  dt[2] = v_dt[0] * (u[0] * u[2] * msinc - u[1] * mcosc) + v_dt[1] * (u[1] * u[2] * msinc + u[0] * mcosc) +
112  v_dt[2] * (sinc + u[2] * u[2] * msinc);
113 
114  vpHomogeneousMatrix Delta;
115  Delta.insert(rd);
116  Delta.insert(dt);
117 
118  if (0) // test new version wrt old version
119  {
120  // old version
121  unsigned int i, j;
122 
123  double s;
124  // double u[3];
125  // vpRotationMatrix rd ;
126  // vpTranslationVector dt ;
127 
128  s = sqrt(v_dt[3] * v_dt[3] + v_dt[4] * v_dt[4] + v_dt[5] * v_dt[5]);
129  if (s > 1.e-15) {
130  for (i = 0; i < 3; i++)
131  u[i] = v_dt[3 + i] / s;
132  double sinu = sin(s);
133  double cosi = cos(s);
134  double mcosi = 1 - cosi;
135  rd[0][0] = cosi + mcosi * u[0] * u[0];
136  rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
137  rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
138  rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
139  rd[1][1] = cosi + mcosi * u[1] * u[1];
140  rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
141  rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
142  rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
143  rd[2][2] = cosi + mcosi * u[2] * u[2];
144 
145  dt[0] = v_dt[0] * (sinu / s + u[0] * u[0] * (1 - sinu / s)) +
146  v_dt[1] * (u[0] * u[1] * (1 - sinu / s) - u[2] * mcosi / s) +
147  v_dt[2] * (u[0] * u[2] * (1 - sinu / s) + u[1] * mcosi / s);
148 
149  dt[1] = v_dt[0] * (u[0] * u[1] * (1 - sinu / s) + u[2] * mcosi / s) +
150  v_dt[1] * (sinu / s + u[1] * u[1] * (1 - sinu / s)) +
151  v_dt[2] * (u[1] * u[2] * (1 - sinu / s) - u[0] * mcosi / s);
152 
153  dt[2] = v_dt[0] * (u[0] * u[2] * (1 - sinu / s) - u[1] * mcosi / s) +
154  v_dt[1] * (u[1] * u[2] * (1 - sinu / s) + u[0] * mcosi / s) +
155  v_dt[2] * (sinu / s + u[2] * u[2] * (1 - sinu / s));
156  } else {
157  for (i = 0; i < 3; i++) {
158  for (j = 0; j < 3; j++)
159  rd[i][j] = 0.0;
160  rd[i][i] = 1.0;
161  dt[i] = v_dt[i];
162  }
163  }
164  // end old version
165 
166  // Test of the new version
167  vpHomogeneousMatrix Delta_old;
168  Delta_old.insert(rd);
169  Delta_old.insert(dt);
170 
171  int pb = 0;
172  for (i = 0; i < 4; i++) {
173  for (j = 0; j < 4; j++)
174  if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5)
175  pb = 1;
176  }
177  if (pb == 1) {
178  printf("pb vpHomogeneousMatrix::expMap\n");
179  std::cout << " Delta : " << std::endl << Delta << std::endl;
180  std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
181  }
182  // end of the test
183  }
184 
185  return Delta;
186 }
187 
203 
222 {
223  vpColVector v(6);
224  unsigned int i;
225  double theta, si, co, sinc, mcosc, msinc, det;
226  vpThetaUVector u;
227  vpRotationMatrix Rd, a;
229 
230  M.extract(Rd);
231  u.buildFrom(Rd);
232  for (i = 0; i < 3; i++)
233  v[3 + i] = u[i];
234 
235  theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
236  si = sin(theta);
237  co = cos(theta);
238  sinc = vpMath::sinc(si, theta);
239  mcosc = vpMath::mcosc(co, theta);
240  msinc = vpMath::msinc(si, theta);
241 
242  // a below is not a pure rotation matrix, even if not so far from
243  // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
244  // with V = t.U
245 
246  a[0][0] = sinc + u[0] * u[0] * msinc;
247  a[0][1] = u[0] * u[1] * msinc - u[2] * mcosc;
248  a[0][2] = u[0] * u[2] * msinc + u[1] * mcosc;
249 
250  a[1][0] = u[0] * u[1] * msinc + u[2] * mcosc;
251  a[1][1] = sinc + u[1] * u[1] * msinc;
252  a[1][2] = u[1] * u[2] * msinc - u[0] * mcosc;
253 
254  a[2][0] = u[0] * u[2] * msinc - u[1] * mcosc;
255  a[2][1] = u[1] * u[2] * msinc + u[0] * mcosc;
256  a[2][2] = sinc + u[2] * u[2] * msinc;
257 
258  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] -
259  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];
260 
261  if (fabs(det) > 1.e-5) {
262  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] -
263  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]) /
264  det;
265  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] -
266  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]) /
267  det;
268  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] -
269  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]) /
270  det;
271  } else {
272  v[0] = M[0][3];
273  v[1] = M[1][3];
274  v[2] = M[2][3];
275  }
276 
277  // Apply the sampling time to the computed velocity
278  v /= delta_t;
279 
280  return (v);
281 }
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
void extract(vpRotationMatrix &R) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Definition: vpMath.cpp:210
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:177
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:194