Visual Servoing Platform  version 3.2.0 under development (2019-01-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 
58 {
59  buildFrom(tu);
60 }
61 
74 
91 
95 vpThetaUVector::vpThetaUVector(const double tux, const double tuy, const double tuz) : vpRotationVector(3)
96 {
97  buildFrom(tux, tuy, tuz);
98 }
99 
103 vpThetaUVector::vpThetaUVector(const std::vector<double> &tu)
104 {
105  buildFrom(tu);
106 }
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  {
169  if ((R[2][1]-R[1][2]) < 0) x = -x;
170  if(vpMath::sign(x)*vpMath::sign(y) != vpMath::sign(R[0][1]+R[1][0])) y = -y;
171  if(vpMath::sign(x)*vpMath::sign(z) != vpMath::sign(R[0][2]+R[2][0])) z = -z;
172  }
173  else if(y > z)
174  {
175  if((R[0][2]-R[2][0]) < 0) y = -y;
176  if(vpMath::sign(y)*vpMath::sign(x) != vpMath::sign(R[1][0]+R[0][1])) x = -x;
177  if(vpMath::sign(y)*vpMath::sign(z) != vpMath::sign(R[1][2]+R[2][1])) z = -z;
178  }
179  else
180  {
181  if((R[1][0]-R[0][1]) < 0) z = -z;
182  if(vpMath::sign(z)*vpMath::sign(x) != vpMath::sign(R[2][0]+R[0][2])) x = -x;
183  if(vpMath::sign(z)*vpMath::sign(y) != vpMath::sign(R[2][1]+R[1][2])) y = -y;
184  }
185  data[0] = theta*x;
186  data[1] = theta*y;
187  data[2] = theta*z;
188  }
189 
190  return *this;
191 }
196 {
197  vpRotationMatrix R(rzyx);
198 
199  buildFrom(R);
200  return *this;
201 }
206 {
207  vpRotationMatrix R(rzyz);
208 
209  buildFrom(R);
210  return *this;
211 }
216 {
217  vpRotationMatrix R(rxyz);
218 
219  buildFrom(R);
220  return *this;
221 }
222 
227 {
228  vpRotationMatrix R(q);
229 
230  buildFrom(R);
231  return *this;
232 }
233 
237 vpThetaUVector vpThetaUVector::buildFrom(const std::vector<double> &tu)
238 {
239  if (tu.size() != 3) {
240  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
241  tu.size()));
242  }
243  for (unsigned int i = 0; i < 3; i++)
244  data[i] = tu[i];
245 
246  return *this;
247 }
248 
253 {
254  if (tu.size() != 3) {
255  throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector",
256  tu.size()));
257  }
258  for (unsigned int i = 0; i < 3; i++)
259  data[i] = tu[i];
260 
261  return *this;
262 }
263 
286 {
287  for (unsigned int i = 0; i < dsize; i++)
288  data[i] = v;
289 
290  return *this;
291 }
292 
317 {
318  if (tu.size() != 3) {
319  throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector",
320  tu.size()));
321  }
322  for (unsigned int i = 0; i < 3; i++)
323  data[i] = tu[i];
324 
325  return *this;
326 }
327 
356 void vpThetaUVector::extract(double &theta, vpColVector &u) const
357 {
358  u.resize(3);
359 
360  theta = getTheta();
361  // if (theta == 0) {
362  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
363  u = 0;
364  return;
365  }
366  for (unsigned int i = 0; i < 3; i++)
367  u[i] = data[i] / theta;
368 }
369 
392 double vpThetaUVector::getTheta() const { return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); }
393 
418 {
419  vpColVector u(3);
420 
421  double theta = getTheta();
422  // if (theta == 0) {
423  if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
424  u = 0;
425  return u;
426  }
427  for (unsigned int i = 0; i < 3; i++)
428  u[i] = data[i] / theta;
429  return u;
430 }
431 
435 void vpThetaUVector::buildFrom(const double tux, const double tuy, const double tuz)
436 {
437  data[0] = tux;
438  data[1] = tuy;
439  data[2] = tuz;
440 }
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:84
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:158
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:72
double getTheta() const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:156
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:80
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:154
static int() sign(double x)
Definition: vpMath.h:262
Implementation of a rotation vector as axis-angle minimal representation.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244