Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpThetaUVector.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 * Theta U parameterization for the rotation.
33 *
34*****************************************************************************/
35
42#include <cmath> // std::fabs
43#include <limits> // numeric_limits
44
45#include <visp3/core/vpThetaUVector.h>
46
47const double vpThetaUVector::minimum = 0.0001;
48
67
84
101vpThetaUVector::vpThetaUVector(double tux, double tuy, double tuz) : vpRotationVector(3) { buildFrom(tux, tuy, tuz); }
102
106vpThetaUVector::vpThetaUVector(const std::vector<double> &tu) { buildFrom(tu); }
107
112{
114
115 M.extract(R);
116 buildFrom(R);
117
118 return *this;
119}
125{
126 for (unsigned int i = 0; i < 3; i++)
127 data[i] = p[i + 3];
128
129 return *this;
130}
131
136{
137 double s, c, theta;
138
139 s = (R[1][0] - R[0][1]) * (R[1][0] - R[0][1]) + (R[2][0] - R[0][2]) * (R[2][0] - R[0][2]) +
140 (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]);
141 s = sqrt(s) / 2.0;
142 c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
143 theta = atan2(s, c); /* theta in [0, PI] since s > 0 */
144
145 // General case when theta != pi. If theta=pi, c=-1
146 if ((1 + c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
147 {
148 double sinc = vpMath::sinc(s, theta);
149
150 data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
151 data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
152 data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
153 } else /* theta near PI */
154 {
155 double x = 0;
156 if ((R[0][0] - c) > std::numeric_limits<double>::epsilon())
157 x = sqrt((R[0][0] - c) / (1 - c));
158
159 double y = 0;
160 if ((R[1][1] - c) > std::numeric_limits<double>::epsilon())
161 y = sqrt((R[1][1] - c) / (1 - c));
162
163 double z = 0;
164 if ((R[2][2] - c) > std::numeric_limits<double>::epsilon())
165 z = sqrt((R[2][2] - c) / (1 - c));
166
167 if (x > y && x > z) {
168 if ((R[2][1] - R[1][2]) < 0)
169 x = -x;
170 if (vpMath::sign(x) * vpMath::sign(y) != vpMath::sign(R[0][1] + R[1][0]))
171 y = -y;
172 if (vpMath::sign(x) * vpMath::sign(z) != vpMath::sign(R[0][2] + R[2][0]))
173 z = -z;
174 } else if (y > z) {
175 if ((R[0][2] - R[2][0]) < 0)
176 y = -y;
177 if (vpMath::sign(y) * vpMath::sign(x) != vpMath::sign(R[1][0] + R[0][1]))
178 x = -x;
179 if (vpMath::sign(y) * vpMath::sign(z) != vpMath::sign(R[1][2] + R[2][1]))
180 z = -z;
181 } else {
182 if ((R[1][0] - R[0][1]) < 0)
183 z = -z;
184 if (vpMath::sign(z) * vpMath::sign(x) != vpMath::sign(R[2][0] + R[0][2]))
185 x = -x;
186 if (vpMath::sign(z) * vpMath::sign(y) != vpMath::sign(R[2][1] + R[1][2]))
187 y = -y;
188 }
189 data[0] = theta * x;
190 data[1] = theta * y;
191 data[2] = theta * z;
192 }
193
194 return *this;
195}
200{
201 vpRotationMatrix R(rzyx);
202
203 buildFrom(R);
204 return *this;
205}
210{
211 vpRotationMatrix R(rzyz);
212
213 buildFrom(R);
214 return *this;
215}
220{
221 vpRotationMatrix R(rxyz);
222
223 buildFrom(R);
224 return *this;
225}
226
231{
232 vpRotationMatrix R(q);
233
234 buildFrom(R);
235 return *this;
236}
237
241vpThetaUVector vpThetaUVector::buildFrom(const std::vector<double> &tu)
242{
243 if (tu.size() != 3) {
244 throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
245 tu.size()));
246 }
247 for (unsigned int i = 0; i < 3; i++)
248 data[i] = tu[i];
249
250 return *this;
251}
252
257{
258 if (tu.size() != 3) {
259 throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
260 tu.size()));
261 }
262 for (unsigned int i = 0; i < 3; i++)
263 data[i] = tu[i];
264
265 return *this;
266}
267
290{
291 for (unsigned int i = 0; i < dsize; i++)
292 data[i] = v;
293
294 return *this;
295}
296
321{
322 if (tu.size() != size()) {
323 throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector",
324 tu.size()));
325 }
326 for (unsigned int i = 0; i < size(); i++)
327 data[i] = tu[i];
328
329 return *this;
330}
331
360void vpThetaUVector::extract(double &theta, vpColVector &u) const
361{
362 u.resize(3);
363
364 theta = getTheta();
365 // if (theta == 0) {
366 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
367 u = 0;
368 return;
369 }
370 for (unsigned int i = 0; i < 3; i++)
371 u[i] = data[i] / theta;
372}
373
396double vpThetaUVector::getTheta() const { return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); }
397
422{
423 vpColVector u(3);
424
425 double theta = getTheta();
426 // if (theta == 0) {
427 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
428 u = 0;
429 return u;
430 }
431 for (unsigned int i = 0; i < 3; i++)
432 u[i] = data[i] / theta;
433 return u;
434}
435
439void vpThetaUVector::buildFrom(double tux, double tuy, double tuz)
440{
441 data[0] = tux;
442 data[1] = tuy;
443 data[2] = tuz;
444}
445
451{
452 double a_2 = getTheta() / 2;
453 vpColVector a_hat = getU();
454 double b_2 = tu_b.getTheta() / 2;
455 vpColVector b_hat = tu_b.getU();
456
457 vpColVector a_hat_sin_2 = a_hat * std::sin(a_2);
458 vpColVector b_hat_sin_2 = b_hat * std::sin(b_2);
459 double c = 2 * std::acos(std::cos(a_2) * std::cos(b_2) - vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2));
460 vpColVector d = std::sin(a_2) * std::cos(b_2) * a_hat + std::cos(a_2) * std::sin(b_2) * b_hat +
461 std::sin(a_2) * std::sin(b_2) * vpColVector::crossProd(a_hat, b_hat);
462 d = c * d / std::sin(c / 2);
463
464 return vpThetaUVector(d);
465}
466
467#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
485vpThetaUVector &vpThetaUVector::operator=(const std::initializer_list<double> &list)
486{
487 if (list.size() > size()) {
488 throw(vpException(
490 "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
491 size(), list.size()));
492 }
493 std::copy(list.begin(), list.end(), data);
494 return *this;
495}
496#endif
double * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int dsize
Current array size (rowNum * colNum)
Definition vpArray2D.h:140
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.
static double dotProd(const vpColVector &a, const vpColVector &b)
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sinc(double x)
Definition vpMath.cpp:264
static int sign(double x)
Definition vpMath.h:342
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector operator*(const vpThetaUVector &tu_b) const
vpColVector getU() const
vpThetaUVector buildFrom(const vpRzyxVector &rzyx)
void extract(double &theta, vpColVector &u) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpThetaUVector & operator=(const vpColVector &tu)
double getTheta() const