Visual Servoing Platform  version 3.6.1 under development (2024-04-18)
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 
40 #include <cmath> // std::fabs
41 #include <limits> // numeric_limits
42 
43 #include <visp3/core/vpThetaUVector.h>
44 
45 const double vpThetaUVector::minimum = 0.0001;
46 
65 
82 
99 vpThetaUVector::vpThetaUVector(double tux, double tuy, double tuz) : vpRotationVector(3) { buildFrom(tux, tuy, tuz); }
100 
104 vpThetaUVector::vpThetaUVector(const std::vector<double> &tu) { buildFrom(tu); }
105 
110 {
112 
113  M.extract(R);
114  buildFrom(R);
115 
116  return *this;
117 }
123 {
124  for (unsigned int i = 0; i < 3; ++i) {
125  data[i] = p[i + 3];
126  }
127 
128  return *this;
129 }
130 
135 {
136  double s, c, theta;
137 
138  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])) +
139  ((R[2][1] - R[1][2]) * (R[2][1] - R[1][2]));
140  s = sqrt(s) / 2.0;
141  c = ((R[0][0] + R[1][1] + R[2][2]) - 1.0) / 2.0;
142  theta = atan2(s, c); /* theta in [0, PI] since s > 0 */
143 
144  // General case when theta != pi. If theta=pi, c=-1
145  if ((1 + c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
146  {
147  double sinc = vpMath::sinc(s, theta);
148 
149  data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
150  data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
151  data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
152  }
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 
160  double y = 0;
161  if ((R[1][1] - c) > std::numeric_limits<double>::epsilon()) {
162  y = sqrt((R[1][1] - c) / (1 - c));
163  }
164 
165  double z = 0;
166  if ((R[2][2] - c) > std::numeric_limits<double>::epsilon()) {
167  z = sqrt((R[2][2] - c) / (1 - c));
168  }
169 
170  if ((x > y) && (x > z)) {
171  if ((R[2][1] - R[1][2]) < 0) {
172  x = -x;
173  }
174  if ((vpMath::sign(x) * vpMath::sign(y)) != (vpMath::sign(R[0][1] + R[1][0]))) {
175  y = -y;
176  }
177  if ((vpMath::sign(x) * vpMath::sign(z)) != (vpMath::sign(R[0][2] + R[2][0]))) {
178  z = -z;
179  }
180  }
181  else if (y > z) {
182  if ((R[0][2] - R[2][0]) < 0) {
183  y = -y;
184  }
185  if ((vpMath::sign(y) * vpMath::sign(x)) != (vpMath::sign(R[1][0] + R[0][1]))) {
186  x = -x;
187  }
188  if ((vpMath::sign(y) * vpMath::sign(z)) != (vpMath::sign(R[1][2] + R[2][1]))) {
189  z = -z;
190  }
191  }
192  else {
193  if ((R[1][0] - R[0][1]) < 0) {
194  z = -z;
195  }
196  if ((vpMath::sign(z) * vpMath::sign(x)) != (vpMath::sign(R[2][0] + R[0][2]))) {
197  x = -x;
198  }
199  if ((vpMath::sign(z) * vpMath::sign(y)) != (vpMath::sign(R[2][1] + R[1][2]))) {
200  y = -y;
201  }
202  }
203  data[0] = theta * x;
204  data[1] = theta * y;
205  data[2] = theta * z;
206  }
207 
208  return *this;
209 }
214 {
215  vpRotationMatrix R(rzyx);
216 
217  buildFrom(R);
218  return *this;
219 }
224 {
225  vpRotationMatrix R(rzyz);
226 
227  buildFrom(R);
228  return *this;
229 }
234 {
235  vpRotationMatrix R(rxyz);
236 
237  buildFrom(R);
238  return *this;
239 }
240 
245 {
246  vpRotationMatrix R(q);
247 
248  buildFrom(R);
249  return *this;
250 }
251 
255 vpThetaUVector vpThetaUVector::buildFrom(const std::vector<double> &tu)
256 {
257  if (tu.size() != 3) {
258  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
259  tu.size()));
260  }
261  for (unsigned int i = 0; i < 3; ++i) {
262  data[i] = tu[i];
263  }
264 
265  return *this;
266 }
267 
272 {
273  if (tu.size() != 3) {
274  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
275  tu.size()));
276  }
277  for (unsigned int i = 0; i < 3; ++i) {
278  data[i] = tu[i];
279  }
280 
281  return *this;
282 }
283 
305 {
306  for (unsigned int i = 0; i < dsize; ++i) {
307  data[i] = v;
308  }
309 
310  return *this;
311 }
312 
336 {
337  if (tu.size() != size()) {
338  throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector",
339  tu.size()));
340  }
341 
342  unsigned int l_size = size();
343  for (unsigned int i = 0; i < l_size; ++i) {
344  data[i] = tu[i];
345  }
346 
347  return *this;
348 }
349 
378 void vpThetaUVector::extract(double &theta, vpColVector &u) const
379 {
380  u.resize(3);
381 
382  theta = getTheta();
383  // --comment: if (theta == 0)
384  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
385  u = 0;
386  return;
387  }
388  for (unsigned int i = 0; i < 3; ++i) {
389  u[i] = data[i] / theta;
390  }
391 }
392 
415 double vpThetaUVector::getTheta() const { return sqrt((data[0] * data[0]) + (data[1] * data[1]) + (data[2] * data[2])); }
416 
441 {
442  vpColVector u(3);
443 
444  double theta = getTheta();
445  // --comment: if theta equals 0
446  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
447  u = 0;
448  return u;
449  }
450  for (unsigned int i = 0; i < 3; ++i) {
451  u[i] = data[i] / theta;
452  }
453  return u;
454 }
455 
459 void vpThetaUVector::buildFrom(double tux, double tuy, double tuz)
460 {
461  data[0] = tux;
462  data[1] = tuy;
463  data[2] = tuz;
464 }
465 
471 {
472  double a_2 = getTheta() / 2;
473  vpColVector a_hat = getU();
474  double b_2 = tu_b.getTheta() / 2;
475  vpColVector b_hat = tu_b.getU();
476 
477  vpColVector a_hat_sin_2 = a_hat * std::sin(a_2);
478  vpColVector b_hat_sin_2 = b_hat * std::sin(b_2);
479  double c = 2 * std::acos((std::cos(a_2) * std::cos(b_2)) - (vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2)));
480  vpColVector d = ((std::sin(a_2) * std::cos(b_2) * a_hat) + (std::cos(a_2) * std::sin(b_2) * b_hat)) +
481  std::sin(a_2) * std::sin(b_2) * vpColVector::crossProd(a_hat, b_hat);
482  d = (c * d) / std::sin(c / 2);
483 
484  return vpThetaUVector(d);
485 }
486 
487 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
505 vpThetaUVector &vpThetaUVector::operator=(const std::initializer_list<double> &list)
506 {
507  if (list.size() > size()) {
508  throw(vpException(
510  "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
511  size(), list.size()));
512  }
513  std::copy(list.begin(), list.end(), data);
514  return *this;
515 }
516 #endif
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:135
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:326
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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)
Definition: vpColVector.h:1056
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:269
static int sign(double x)
Definition: vpMath.h:422
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
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.
Definition: vpRxyzVector.h:176
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:177
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:175
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector operator*(const vpThetaUVector &tu_b) const
vpColVector getU() const
void extract(double &theta, vpColVector &u) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpThetaUVector & operator=(const vpColVector &tu)
double getTheta() const