Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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 
58 {
59  buildFrom(tu);
60 }
73 
90 
107 vpThetaUVector::vpThetaUVector(const double tux, const double tuy, const double tuz) : vpRotationVector(3)
108 {
109  buildFrom(tux, tuy, tuz);
110 }
111 
115 vpThetaUVector::vpThetaUVector(const std::vector<double> &tu)
116 {
117  buildFrom(tu);
118 }
119 
124 {
126 
127  M.extract(R);
128  buildFrom(R);
129 
130  return *this;
131 }
137 {
138  for (unsigned int i = 0; i < 3; i++)
139  data[i] = p[i + 3];
140 
141  return *this;
142 }
143 
148 {
149  double s, c, theta;
150 
151  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]) +
152  (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]);
153  s = sqrt(s) / 2.0;
154  c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
155  theta = atan2(s, c); /* theta in [0, PI] since s > 0 */
156 
157  // General case when theta != pi. If theta=pi, c=-1
158  if ((1 + c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
159  {
160  double sinc = vpMath::sinc(s, theta);
161 
162  data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
163  data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
164  data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
165  } else /* theta near PI */
166  {
167  double x = 0;
168  if ( (R[0][0]-c) > std::numeric_limits<double>::epsilon() )
169  x = sqrt((R[0][0]-c)/(1-c));
170 
171  double y = 0;
172  if ( (R[1][1]-c) > std::numeric_limits<double>::epsilon() )
173  y = sqrt((R[1][1]-c)/(1-c));
174 
175  double z = 0;
176  if ( (R[2][2]-c) > std::numeric_limits<double>::epsilon() )
177  z = sqrt((R[2][2]-c)/(1-c));
178 
179  if(x > y && x > z)
180  {
181  if ((R[2][1]-R[1][2]) < 0) x = -x;
182  if(vpMath::sign(x)*vpMath::sign(y) != vpMath::sign(R[0][1]+R[1][0])) y = -y;
183  if(vpMath::sign(x)*vpMath::sign(z) != vpMath::sign(R[0][2]+R[2][0])) z = -z;
184  }
185  else if(y > z)
186  {
187  if((R[0][2]-R[2][0]) < 0) y = -y;
188  if(vpMath::sign(y)*vpMath::sign(x) != vpMath::sign(R[1][0]+R[0][1])) x = -x;
189  if(vpMath::sign(y)*vpMath::sign(z) != vpMath::sign(R[1][2]+R[2][1])) z = -z;
190  }
191  else
192  {
193  if((R[1][0]-R[0][1]) < 0) z = -z;
194  if(vpMath::sign(z)*vpMath::sign(x) != vpMath::sign(R[2][0]+R[0][2])) x = -x;
195  if(vpMath::sign(z)*vpMath::sign(y) != vpMath::sign(R[2][1]+R[1][2])) y = -y;
196  }
197  data[0] = theta*x;
198  data[1] = theta*y;
199  data[2] = theta*z;
200  }
201 
202  return *this;
203 }
208 {
209  vpRotationMatrix R(rzyx);
210 
211  buildFrom(R);
212  return *this;
213 }
218 {
219  vpRotationMatrix R(rzyz);
220 
221  buildFrom(R);
222  return *this;
223 }
228 {
229  vpRotationMatrix R(rxyz);
230 
231  buildFrom(R);
232  return *this;
233 }
234 
239 {
240  vpRotationMatrix R(q);
241 
242  buildFrom(R);
243  return *this;
244 }
245 
249 vpThetaUVector vpThetaUVector::buildFrom(const std::vector<double> &tu)
250 {
251  if (tu.size() != 3) {
252  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
253  tu.size()));
254  }
255  for (unsigned int i = 0; i < 3; i++)
256  data[i] = tu[i];
257 
258  return *this;
259 }
260 
265 {
266  if (tu.size() != 3) {
267  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
268  tu.size()));
269  }
270  for (unsigned int i = 0; i < 3; i++)
271  data[i] = tu[i];
272 
273  return *this;
274 }
275 
298 {
299  for (unsigned int i = 0; i < dsize; i++)
300  data[i] = v;
301 
302  return *this;
303 }
304 
329 {
330  if (tu.size() != size()) {
331  throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector",
332  tu.size()));
333  }
334  for (unsigned int i = 0; i < size(); i++)
335  data[i] = tu[i];
336 
337  return *this;
338 }
339 
368 void vpThetaUVector::extract(double &theta, vpColVector &u) const
369 {
370  u.resize(3);
371 
372  theta = getTheta();
373  // if (theta == 0) {
374  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
375  u = 0;
376  return;
377  }
378  for (unsigned int i = 0; i < 3; i++)
379  u[i] = data[i] / theta;
380 }
381 
404 double vpThetaUVector::getTheta() const { return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); }
405 
430 {
431  vpColVector u(3);
432 
433  double theta = getTheta();
434  // if (theta == 0) {
435  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
436  u = 0;
437  return u;
438  }
439  for (unsigned int i = 0; i < 3; i++)
440  u[i] = data[i] / theta;
441  return u;
442 }
443 
447 void vpThetaUVector::buildFrom(const double tux, const double tuy, const double tuz)
448 {
449  data[0] = tux;
450  data[1] = tuy;
451  data[2] = tuz;
452 }
453 
454 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
455 
472 vpThetaUVector &vpThetaUVector::operator=(const std::initializer_list<double> &list)
473 {
474  if (list.size() > size()) {
475  throw(vpException(vpException::dimensionError, "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values", size(), list.size()));
476  }
477  std::copy(list.begin(), list.end(), data);
478  return *this;
479 }
480 #endif
vpThetaUVector & operator=(const vpColVector &tu)
Implementation of a generic rotation vector.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpColVector getU() 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:291
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:185
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Definition: vpMath.cpp:170
Implementation of a rotation matrix and operations on such kind of matrices.
void extract(double &theta, vpColVector &u) const
void extract(vpRotationMatrix &R) const
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
double getTheta() const
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)
Definition: vpMath.h:268
Implementation of a rotation vector as axis-angle minimal representation.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310