Visual Servoing Platform  version 3.6.1 under development (2024-06-18)
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 
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.build(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))) +
106  (v_dt[1] * ((u[0] * u[1] * msinc) - (u[2] * mcosc)))) +
107  (v_dt[2] * ((u[0] * u[2] * msinc) + (u[1] * mcosc)));
108 
109  dt[1] = ((v_dt[0] * ((u[0] * u[1] * msinc) + (u[2] * mcosc))) +
110  (v_dt[1] * (sinc + (u[1] * u[1] * msinc)))) +
111  (v_dt[2] * ((u[1] * u[2] * msinc) - (u[0] * mcosc)));
112 
113  dt[2] = ((v_dt[0] * ((u[0] * u[2] * msinc) - (u[1] * mcosc))) +
114  (v_dt[1] * ((u[1] * u[2] * msinc) + (u[0] * mcosc)))) +
115  (v_dt[2] * (sinc + (u[2] * u[2] * msinc)));
116 
117  vpHomogeneousMatrix Delta;
118  Delta.insert(rd);
119  Delta.insert(dt);
120 
121  if (0) // test new version wrt old version
122  {
123  // old version
124  unsigned int i, j;
125 
126  double s;
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  }
133  double sinu = sin(s);
134  double cosi = cos(s);
135  double mcosi = 1 - cosi;
136  rd[0][0] = cosi + (mcosi * u[0] * u[0]);
137  rd[0][1] = (-sinu * u[2]) + (mcosi * u[0] * u[1]);
138  rd[0][2] = (sinu * u[1]) + (mcosi * u[0] * u[2]);
139  rd[1][0] = (sinu * u[2]) + (mcosi * u[1] * u[0]);
140  rd[1][1] = cosi + (mcosi * u[1] * u[1]);
141  rd[1][2] = (-sinu * u[0]) + (mcosi * u[1] * u[2]);
142  rd[2][0] = (-sinu * u[1]) + (mcosi * u[2] * u[0]);
143  rd[2][1] = (sinu * u[0]) + (mcosi * u[2] * u[1]);
144  rd[2][2] = cosi + (mcosi * u[2] * u[2]);
145 
146  dt[0] = (v_dt[0] * ((sinu / s) + (u[0] * u[0] * (1 - (sinu / s))))) +
147  (v_dt[1] * ((u[0] * u[1] * (1 - (sinu / s))) - ((u[2] * mcosi) / s))) +
148  (v_dt[2] * ((u[0] * u[2] * (1 - (sinu / s))) + ((u[1] * mcosi) / s)));
149 
150  dt[1] = (v_dt[0] * ((u[0] * u[1] * (1 - (sinu / s))) + ((u[2] * mcosi) / s))) +
151  (v_dt[1] * ((sinu / s) + (u[1] * u[1] * (1 - (sinu / s))))) +
152  (v_dt[2] * ((u[1] * u[2] * (1 - (sinu / s))) - ((u[0] * mcosi) / s)));
153 
154  dt[2] = (v_dt[0] * ((u[0] * u[2] * (1 - (sinu / s))) - ((u[1] * mcosi) / s))) +
155  (v_dt[1] * ((u[1] * u[2] * (1 - (sinu / s))) + ((u[0] * mcosi) / s))) +
156  (v_dt[2] * ((sinu / s) + (u[2] * u[2] * (1 - (sinu / s)))));
157  }
158  else {
159  for (i = 0; i < 3; ++i) {
160  for (j = 0; j < 3; ++j) {
161  rd[i][j] = 0.0;
162  }
163  rd[i][i] = 1.0;
164  dt[i] = v_dt[i];
165  }
166  }
167  // end old version
168 
169  // Test of the new version
170  vpHomogeneousMatrix Delta_old;
171  Delta_old.insert(rd);
172  Delta_old.insert(dt);
173 
174  int pb = 0;
175  for (i = 0; i < 4; ++i) {
176  for (j = 0; j < 4; ++j) {
177  if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) {
178  pb = 1;
179  }
180  }
181  }
182  if (pb == 1) {
183  printf("pb vpHomogeneousMatrix::expMap\n");
184  std::cout << " Delta : " << std::endl << Delta << std::endl;
185  std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
186  }
187  // end of the test
188  }
189 
190  return Delta;
191 }
192 
208 
227 {
228  vpColVector v(6);
229  unsigned int i;
230  double theta, si, co, sinc, mcosc, msinc, det;
231  vpThetaUVector u;
232  vpRotationMatrix Rd, a;
234 
235  M.extract(Rd);
236  u.build(Rd);
237  for (i = 0; i < 3; ++i) {
238  v[3 + i] = u[i];
239  }
240 
241  theta = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]));
242  si = sin(theta);
243  co = cos(theta);
244  sinc = vpMath::sinc(si, theta);
245  mcosc = vpMath::mcosc(co, theta);
246  msinc = vpMath::msinc(si, theta);
247 
248  // a below is not a pure rotation matrix, even if not so far from
249  // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
250  // with V = t.U
251 
252  a[0][0] = sinc + (u[0] * u[0] * msinc);
253  a[0][1] = (u[0] * u[1] * msinc) - (u[2] * mcosc);
254  a[0][2] = (u[0] * u[2] * msinc) + (u[1] * mcosc);
255 
256  a[1][0] = (u[0] * u[1] * msinc) + (u[2] * mcosc);
257  a[1][1] = sinc + (u[1] * u[1] * msinc);
258  a[1][2] = (u[1] * u[2] * msinc) - (u[0] * mcosc);
259 
260  a[2][0] = (u[0] * u[2] * msinc) - (u[1] * mcosc);
261  a[2][1] = (u[1] * u[2] * msinc) + (u[0] * mcosc);
262  a[2][2] = sinc + (u[2] * u[2] * msinc);
263 
264  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])) -
265  (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]);
266 
267  if (fabs(det) > 1.e-5) {
268  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])) -
269  (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])) /
270  det;
271  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])) -
272  (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])) /
273  det;
274  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])) -
275  (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])) /
276  det;
277  }
278  else {
279  v[0] = M[0][3];
280  v[1] = M[1][3];
281  v[2] = M[2][3];
282  }
283 
284  // Apply the sampling time to the computed velocity
285  v /= delta_t;
286 
287  return v;
288 }
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:342
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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:253
static double sinc(double x)
Definition: vpMath.cpp:270
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:235
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix & build(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector & build(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.