Visual Servoing Platform  version 3.6.1 under development (2023-12-02)
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 
47 const double vpThetaUVector::minimum = 0.0001;
48 
67 
84 
101 vpThetaUVector::vpThetaUVector(double tux, double tuy, double tuz) : vpRotationVector(3) { buildFrom(tux, tuy, tuz); }
102 
106 vpThetaUVector::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  }
154  else /* theta near PI */
155  {
156  double x = 0;
157  if ((R[0][0] - c) > std::numeric_limits<double>::epsilon())
158  x = sqrt((R[0][0] - c) / (1 - c));
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  double z = 0;
165  if ((R[2][2] - c) > std::numeric_limits<double>::epsilon())
166  z = sqrt((R[2][2] - c) / (1 - c));
167 
168  if (x > y && x > z) {
169  if ((R[2][1] - R[1][2]) < 0)
170  x = -x;
171  if (vpMath::sign(x) * vpMath::sign(y) != vpMath::sign(R[0][1] + R[1][0]))
172  y = -y;
173  if (vpMath::sign(x) * vpMath::sign(z) != vpMath::sign(R[0][2] + R[2][0]))
174  z = -z;
175  }
176  else if (y > z) {
177  if ((R[0][2] - R[2][0]) < 0)
178  y = -y;
179  if (vpMath::sign(y) * vpMath::sign(x) != vpMath::sign(R[1][0] + R[0][1]))
180  x = -x;
181  if (vpMath::sign(y) * vpMath::sign(z) != vpMath::sign(R[1][2] + R[2][1]))
182  z = -z;
183  }
184  else {
185  if ((R[1][0] - R[0][1]) < 0)
186  z = -z;
187  if (vpMath::sign(z) * vpMath::sign(x) != vpMath::sign(R[2][0] + R[0][2]))
188  x = -x;
189  if (vpMath::sign(z) * vpMath::sign(y) != vpMath::sign(R[2][1] + R[1][2]))
190  y = -y;
191  }
192  data[0] = theta * x;
193  data[1] = theta * y;
194  data[2] = theta * z;
195  }
196 
197  return *this;
198 }
203 {
204  vpRotationMatrix R(rzyx);
205 
206  buildFrom(R);
207  return *this;
208 }
213 {
214  vpRotationMatrix R(rzyz);
215 
216  buildFrom(R);
217  return *this;
218 }
223 {
224  vpRotationMatrix R(rxyz);
225 
226  buildFrom(R);
227  return *this;
228 }
229 
234 {
235  vpRotationMatrix R(q);
236 
237  buildFrom(R);
238  return *this;
239 }
240 
244 vpThetaUVector vpThetaUVector::buildFrom(const std::vector<double> &tu)
245 {
246  if (tu.size() != 3) {
247  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
248  tu.size()));
249  }
250  for (unsigned int i = 0; i < 3; i++)
251  data[i] = tu[i];
252 
253  return *this;
254 }
255 
260 {
261  if (tu.size() != 3) {
262  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
263  tu.size()));
264  }
265  for (unsigned int i = 0; i < 3; i++)
266  data[i] = tu[i];
267 
268  return *this;
269 }
270 
293 {
294  for (unsigned int i = 0; i < dsize; i++)
295  data[i] = v;
296 
297  return *this;
298 }
299 
324 {
325  if (tu.size() != size()) {
326  throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector",
327  tu.size()));
328  }
329  for (unsigned int i = 0; i < size(); i++)
330  data[i] = tu[i];
331 
332  return *this;
333 }
334 
363 void vpThetaUVector::extract(double &theta, vpColVector &u) const
364 {
365  u.resize(3);
366 
367  theta = getTheta();
368  // if (theta == 0) {
369  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
370  u = 0;
371  return;
372  }
373  for (unsigned int i = 0; i < 3; i++)
374  u[i] = data[i] / theta;
375 }
376 
399 double vpThetaUVector::getTheta() const { return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); }
400 
425 {
426  vpColVector u(3);
427 
428  double theta = getTheta();
429  // if (theta == 0) {
430  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
431  u = 0;
432  return u;
433  }
434  for (unsigned int i = 0; i < 3; i++)
435  u[i] = data[i] / theta;
436  return u;
437 }
438 
442 void vpThetaUVector::buildFrom(double tux, double tuy, double tuz)
443 {
444  data[0] = tux;
445  data[1] = tuy;
446  data[2] = tuz;
447 }
448 
454 {
455  double a_2 = getTheta() / 2;
456  vpColVector a_hat = getU();
457  double b_2 = tu_b.getTheta() / 2;
458  vpColVector b_hat = tu_b.getU();
459 
460  vpColVector a_hat_sin_2 = a_hat * std::sin(a_2);
461  vpColVector b_hat_sin_2 = b_hat * std::sin(b_2);
462  double c = 2 * std::acos(std::cos(a_2) * std::cos(b_2) - vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2));
463  vpColVector d = std::sin(a_2) * std::cos(b_2) * a_hat + std::cos(a_2) * std::sin(b_2) * b_hat +
464  std::sin(a_2) * std::sin(b_2) * vpColVector::crossProd(a_hat, b_hat);
465  d = c * d / std::sin(c / 2);
466 
467  return vpThetaUVector(d);
468 }
469 
487 vpThetaUVector &vpThetaUVector::operator=(const std::initializer_list<double> &list)
488 {
489  if (list.size() > size()) {
490  throw(vpException(
492  "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
493  size(), list.size()));
494  }
495  std::copy(list.begin(), list.end(), data);
496  return *this;
497 }
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:134
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:269
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:1049
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: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