Visual Servoing Platform  version 3.6.1 under development (2024-12-10)
vpExponentialMap.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Exponential map.
32  */
33 
34 #include <visp3/core/vpExponentialMap.h>
35 
36 BEGIN_VISP_NAMESPACE
55 
75 {
76  const unsigned int v_size = 6;
77  const unsigned int index_0 = 0;
78  const unsigned int index_1 = 1;
79  const unsigned int index_2 = 2;
80  const unsigned int index_3 = 3;
81  const unsigned int index_4 = 4;
82  const unsigned int index_5 = 5;
83 
84  if (v.size() != v_size) {
86  "Cannot compute direct exponential map from a %d-dim velocity vector. Should be 6-dim.",
87  v.size()));
88  }
89  double theta, si, co, sinc, mcosc, msinc;
93 
94  vpColVector v_dt = v * delta_t;
95 
96  u[index_0] = v_dt[index_3];
97  u[index_1] = v_dt[index_4];
98  u[index_2] = v_dt[index_5];
99  rd.buildFrom(u);
100 
101  theta = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2]));
102  si = sin(theta);
103  co = cos(theta);
104  sinc = vpMath::sinc(si, theta);
105  mcosc = vpMath::mcosc(co, theta);
106  msinc = vpMath::msinc(si, theta);
107 
108  dt[index_0] = ((v_dt[index_0] * (sinc + (u[index_0] * u[index_0] * msinc))) +
109  (v_dt[index_1] * ((u[index_0] * u[index_1] * msinc) - (u[index_2] * mcosc)))) +
110  (v_dt[index_2] * ((u[index_0] * u[index_2] * msinc) + (u[index_1] * mcosc)));
111 
112  dt[index_1] = ((v_dt[index_0] * ((u[index_0] * u[index_1] * msinc) + (u[index_2] * mcosc))) +
113  (v_dt[index_1] * (sinc + (u[index_1] * u[index_1] * msinc)))) +
114  (v_dt[index_2] * ((u[index_1] * u[index_2] * msinc) - (u[index_0] * mcosc)));
115 
116  dt[index_2] = ((v_dt[index_0] * ((u[index_0] * u[index_2] * msinc) - (u[index_1] * mcosc))) +
117  (v_dt[index_1] * ((u[index_1] * u[index_2] * msinc) + (u[index_0] * mcosc)))) +
118  (v_dt[index_2] * (sinc + (u[index_2] * u[index_2] * msinc)));
119 
120  vpHomogeneousMatrix Delta;
121  Delta.insert(rd);
122  Delta.insert(dt);
123 
124  return Delta;
125 }
126 
142 
161 {
162  vpColVector v(6);
163  unsigned int i;
164  double theta, si, co, sinc, mcosc, msinc, det;
165  vpThetaUVector u;
166  vpRotationMatrix Rd, a;
168  const unsigned int index_0 = 0;
169  const unsigned int index_1 = 1;
170  const unsigned int index_2 = 2;
171  const unsigned int index_3 = 3;
172  const unsigned int val_3 = 3;
173 
174  M.extract(Rd);
175  u.buildFrom(Rd);
176  for (i = 0; i < val_3; ++i) {
177  v[index_3 + i] = u[i];
178  }
179 
180  theta = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2]));
181  si = sin(theta);
182  co = cos(theta);
183  sinc = vpMath::sinc(si, theta);
184  mcosc = vpMath::mcosc(co, theta);
185  msinc = vpMath::msinc(si, theta);
186 
187  // a below is not a pure rotation matrix, even if not so far from
188  // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
189  // with V = t.U
190 
191  a[index_0][index_0] = sinc + (u[index_0] * u[index_0] * msinc);
192  a[index_0][index_1] = (u[index_0] * u[index_1] * msinc) - (u[index_2] * mcosc);
193  a[index_0][index_2] = (u[index_0] * u[index_2] * msinc) + (u[index_1] * mcosc);
194 
195  a[index_1][index_0] = (u[index_0] * u[index_1] * msinc) + (u[index_2] * mcosc);
196  a[index_1][index_1] = sinc + (u[index_1] * u[index_1] * msinc);
197  a[index_1][index_2] = (u[index_1] * u[index_2] * msinc) - (u[index_0] * mcosc);
198 
199  a[index_2][index_0] = (u[index_0] * u[index_2] * msinc) - (u[index_1] * mcosc);
200  a[index_2][index_1] = (u[index_1] * u[index_2] * msinc) + (u[index_0] * mcosc);
201  a[index_2][index_2] = sinc + (u[index_2] * u[index_2] * msinc);
202 
203  det = (((((a[index_0][index_0] * a[index_1][index_1] * a[index_2][index_2]) + (a[index_1][index_0] * a[index_2][index_1] * a[index_0][index_2])) + (a[index_0][index_1] * a[index_1][index_2] * a[index_2][index_0])) -
204  (a[index_2][index_0] * a[index_1][index_1] * a[index_0][index_2])) - (a[index_1][index_0] * a[index_0][index_1] * a[index_2][index_2])) - (a[index_0][index_0] * a[index_2][index_1] * a[index_1][index_2]);
205 
206  if (fabs(det) > 1.e-5) {
207  v[index_0] = ((((((M[index_0][index_3] * a[index_1][index_1] * a[index_2][index_2]) + (M[index_1][index_3] * a[index_2][index_1] * a[index_0][index_2])) + (M[index_2][index_3] * a[index_0][index_1] * a[index_1][index_2])) -
208  (M[index_2][index_3] * a[index_1][index_1] * a[index_0][index_2])) - (M[index_1][index_3] * a[index_0][index_1] * a[index_2][index_2])) - (M[index_0][index_3] * a[index_2][index_1] * a[index_1][index_2])) /
209  det;
210  v[index_1] = ((((((a[index_0][index_0] * M[index_1][index_3] * a[index_2][index_2]) + (a[index_1][index_0] * M[index_2][index_3] * a[index_0][index_2])) + (M[index_0][index_3] * a[index_1][index_2] * a[index_2][index_0])) -
211  (a[index_2][index_0] * M[index_1][index_3] * a[index_0][index_2])) - (a[index_1][index_0] * M[index_0][index_3] * a[index_2][index_2])) - (a[index_0][index_0] * M[index_2][index_3] * a[index_1][index_2])) /
212  det;
213  v[index_2] = ((((((a[index_0][index_0] * a[index_1][index_1] * M[index_2][index_3]) + (a[index_1][index_0] * a[index_2][index_1] * M[index_0][index_3])) + (a[index_0][index_1] * M[index_1][index_3] * a[index_2][index_0])) -
214  (a[index_2][index_0] * a[index_1][index_1] * M[index_0][index_3])) - (a[index_1][index_0] * a[index_0][index_1] * M[index_2][index_3])) - (a[index_0][index_0] * a[index_2][index_1] * M[index_1][index_3])) /
215  det;
216  }
217  else {
218  v[index_0] = M[index_0][index_3];
219  v[index_1] = M[index_1][index_3];
220  v[index_2] = M[index_2][index_3];
221  }
222 
223  // Apply the sampling time to the computed velocity
224  v /= delta_t;
225 
226  return v;
227 }
228 END_VISP_NAMESPACE
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
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:251
static double sinc(double x)
Definition: vpMath.cpp:268
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.