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