Visual Servoing Platform  version 3.5.1 under development (2022-05-22)
vpThetaUVector.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  * Theta U parameterization for the rotation.
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
45 #include <cmath> // std::fabs
46 #include <limits> // numeric_limits
47 
48 #include <visp3/core/vpThetaUVector.h>
49 
50 const double vpThetaUVector::minimum = 0.0001;
51 
70 
87 
104 vpThetaUVector::vpThetaUVector(double tux, double tuy, double tuz) : vpRotationVector(3) { buildFrom(tux, tuy, tuz); }
105 
109 vpThetaUVector::vpThetaUVector(const std::vector<double> &tu) { buildFrom(tu); }
110 
115 {
117 
118  M.extract(R);
119  buildFrom(R);
120 
121  return *this;
122 }
128 {
129  for (unsigned int i = 0; i < 3; i++)
130  data[i] = p[i + 3];
131 
132  return *this;
133 }
134 
139 {
140  double s, c, theta;
141 
142  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]) +
143  (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]);
144  s = sqrt(s) / 2.0;
145  c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
146  theta = atan2(s, c); /* theta in [0, PI] since s > 0 */
147 
148  // General case when theta != pi. If theta=pi, c=-1
149  if ((1 + c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
150  {
151  double sinc = vpMath::sinc(s, theta);
152 
153  data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
154  data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
155  data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
156  } else /* theta near PI */
157  {
158  double x = 0;
159  if ((R[0][0] - c) > std::numeric_limits<double>::epsilon())
160  x = sqrt((R[0][0] - c) / (1 - c));
161 
162  double y = 0;
163  if ((R[1][1] - c) > std::numeric_limits<double>::epsilon())
164  y = sqrt((R[1][1] - c) / (1 - c));
165 
166  double z = 0;
167  if ((R[2][2] - c) > std::numeric_limits<double>::epsilon())
168  z = sqrt((R[2][2] - c) / (1 - c));
169 
170  if (x > y && x > z) {
171  if ((R[2][1] - R[1][2]) < 0)
172  x = -x;
173  if (vpMath::sign(x) * vpMath::sign(y) != vpMath::sign(R[0][1] + R[1][0]))
174  y = -y;
175  if (vpMath::sign(x) * vpMath::sign(z) != vpMath::sign(R[0][2] + R[2][0]))
176  z = -z;
177  } else if (y > z) {
178  if ((R[0][2] - R[2][0]) < 0)
179  y = -y;
180  if (vpMath::sign(y) * vpMath::sign(x) != vpMath::sign(R[1][0] + R[0][1]))
181  x = -x;
182  if (vpMath::sign(y) * vpMath::sign(z) != vpMath::sign(R[1][2] + R[2][1]))
183  z = -z;
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 
449 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
450 
467 vpThetaUVector &vpThetaUVector::operator=(const std::initializer_list<double> &list)
468 {
469  if (list.size() > size()) {
470  throw(vpException(
472  "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
473  size(), list.size()));
474  }
475  std::copy(list.begin(), list.end(), data);
476  return *this;
477 }
478 #endif
vpThetaUVector & operator=(const vpColVector &tu)
Implementation of a generic rotation vector.
vpColVector getU() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(double &theta, vpColVector &u) const
error that can be emited by ViSP classes.
Definition: vpException.h:71
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:293
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:185
void extract(vpRotationMatrix &R) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Definition: vpMath.cpp:211
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as quaternion angle minimal representation.
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:141
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
static int() sign(double x)
Implementation of a rotation vector as axis-angle minimal representation.
double getTheta() const