Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
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;
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.
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.