Visual Servoing Platform version 3.5.0
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
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;
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}
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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:194
static double sinc(double x)
Definition: vpMath.cpp:210
static double mcosc(double cosx, double x)
Definition: vpMath.cpp:177
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.