Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpFeatureThetaU.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  * ThetaU visual feature.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <visp3/core/vpMath.h>
41 #include <visp3/visual_features/vpBasicFeature.h>
42 #include <visp3/visual_features/vpFeatureThetaU.h>
43 
44 // Exception
45 #include <visp3/core/vpException.h>
46 #include <visp3/visual_features/vpFeatureException.h>
47 
48 // Debug trace
49 #include <visp3/core/vpDebug.h>
50 
55 /*
56 
57 attributes and members directly related to the vpBasicFeature needs
58 other functionalities are useful but not mandatory
59 
60 */
61 
68 {
69  // feature dimension
70  dim_s = 3;
71  nbParameters = 3;
72 
73  // memory allocation
74  s.resize(dim_s);
75  if (flags == NULL)
76  flags = new bool[nbParameters];
77  for (unsigned int i = 0; i < nbParameters; i++)
78  flags[i] = false;
79 }
80 
89 {
90  // vpTRACE("0x%x", this);
91  init();
92 }
93 
102 {
103  // vpTRACE("0x%x", this);
104  init();
105 
106  // kind of rotation representation
107  rotation = r;
108 }
109 
130 {
131  init();
132 
133  buildFrom(tu);
134 }
135 
154 {
155  init();
156 
157  vpThetaUVector tu(R);
158  buildFrom(tu);
159 }
160 
181 {
182  init();
184  M.extract(R);
185  vpThetaUVector tu(R);
186  buildFrom(tu);
187 }
188 
206 {
207  s[0] = tu[0];
208  s[1] = tu[1];
209  s[2] = tu[2];
210  for (unsigned int i = 0; i < nbParameters; i++)
211  flags[i] = true;
212 }
213 
230 {
231  vpThetaUVector tu(R);
232  buildFrom(tu);
233 }
234 
254 {
256  M.extract(R);
257  vpThetaUVector tu(R);
258  buildFrom(tu);
259 }
260 
270 
279 void vpFeatureThetaU::set_TUx(const double tu_x)
280 {
281  s[0] = tu_x;
282  flags[0] = true;
283 }
292 void vpFeatureThetaU::set_TUy(const double tu_y)
293 {
294  s[1] = tu_y;
295  flags[1] = true;
296 }
305 void vpFeatureThetaU::set_TUz(const double tu_z)
306 {
307  s[2] = tu_z;
308  flags[2] = true;
309 }
310 
320 {
321  return rotation;
322 }
323 
330 double vpFeatureThetaU::get_TUx() const { return s[0]; }
331 
337 double vpFeatureThetaU::get_TUy() const { return s[1]; }
338 
344 double vpFeatureThetaU::get_TUz() const { return s[2]; }
345 
408 vpMatrix vpFeatureThetaU::interaction(const unsigned int select)
409 {
410 
411  vpMatrix L;
412  L.resize(0, 6);
413 
415  for (unsigned int i = 0; i < nbParameters; i++) {
416  if (flags[i] == false) {
417  switch (i) {
418  case 0:
419  vpTRACE("Warning !!! The interaction matrix is computed but Tu_x "
420  "was not set yet");
421  break;
422  case 1:
423  vpTRACE("Warning !!! The interaction matrix is computed but Tu_y "
424  "was not set yet");
425  break;
426  case 2:
427  vpTRACE("Warning !!! The interaction matrix is computed but Tu_z "
428  "was not set yet");
429  break;
430  default:
431  vpTRACE("Problem during the reading of the variable flags");
432  }
433  }
434  }
435  resetFlags();
436  }
437 
438  // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
439  vpColVector u(3);
440  for (unsigned int i = 0; i < 3; i++) {
441  u[i] = s[i] / 2.0;
442  }
443 
444  vpMatrix Lw(3, 3);
445  Lw = vpColVector::skew(u); /* [theta/2 u]_x */
446 
447  vpMatrix U2(3, 3);
448  U2.eye();
449 
450  double theta = sqrt(s.sumSquare());
451  if (theta >= 1e-6) {
452  for (unsigned int i = 0; i < 3; i++)
453  u[i] = s[i] / theta;
454 
455  vpMatrix skew_u;
456  skew_u = vpColVector::skew(u);
457  U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * skew_u * skew_u;
458  }
459 
460  if (rotation == cdRc) {
461  Lw += U2;
462  } else {
463  Lw -= U2;
464  }
465 
466  // This version is a simplification
467  if (vpFeatureThetaU::selectTUx() & select) {
468  vpMatrix Lx(1, 6);
469 
470  Lx[0][0] = 0;
471  Lx[0][1] = 0;
472  Lx[0][2] = 0;
473  for (int i = 0; i < 3; i++)
474  Lx[0][i + 3] = Lw[0][i];
475 
476  L = vpMatrix::stack(L, Lx);
477  }
478 
479  if (vpFeatureThetaU::selectTUy() & select) {
480  vpMatrix Ly(1, 6);
481 
482  Ly[0][0] = 0;
483  Ly[0][1] = 0;
484  Ly[0][2] = 0;
485  for (int i = 0; i < 3; i++)
486  Ly[0][i + 3] = Lw[1][i];
487 
488  L = vpMatrix::stack(L, Ly);
489  }
490 
491  if (vpFeatureThetaU::selectTUz() & select) {
492  vpMatrix Lz(1, 6);
493 
494  Lz[0][0] = 0;
495  Lz[0][1] = 0;
496  Lz[0][2] = 0;
497  for (int i = 0; i < 3; i++)
498  Lz[0][i + 3] = Lw[2][i];
499 
500  L = vpMatrix::stack(L, Lz);
501  }
502 
503  return L;
504 }
505 
567 vpColVector vpFeatureThetaU::error(const vpBasicFeature &s_star, const unsigned int select)
568 {
569 
570  if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
571  vpERROR_TRACE("s* should be zero ! ");
572  throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
573  }
574 
575  vpColVector e(0);
576 
577  if (vpFeatureThetaU::selectTUx() & select) {
578  vpColVector ex(1);
579  ex[0] = s[0];
580  e = vpColVector::stack(e, ex);
581  }
582 
583  if (vpFeatureThetaU::selectTUy() & select) {
584  vpColVector ey(1);
585  ey[0] = s[1];
586  e = vpColVector::stack(e, ey);
587  }
588 
589  if (vpFeatureThetaU::selectTUz() & select) {
590  vpColVector ez(1);
591  ez[0] = s[2];
592  e = vpColVector::stack(e, ez);
593  }
594  return e;
595 }
596 
623 void vpFeatureThetaU::print(const unsigned int select) const
624 {
625  std::cout << "ThetaU: ";
626  if (vpFeatureThetaU::selectTUx() & select) {
627  std::cout << s[0] << " ";
628  }
629  if (vpFeatureThetaU::selectTUy() & select) {
630  std::cout << s[1] << " ";
631  }
632  if (vpFeatureThetaU::selectTUz() & select) {
633  std::cout << s[2] << " ";
634  }
635  std::cout << std::endl;
636 }
637 
650 {
651  vpFeatureThetaU *feature;
652  if (rotation == cdRc)
653  feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc);
654  else // if (rotation == cRcd
655  feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd);
656 
657  return feature;
658 }
659 
666  const vpColor & /* color */, unsigned int /* thickness */) const
667 {
668  static int firsttime = 0;
669 
670  if (firsttime == 0) {
671  firsttime = 1;
672  vpERROR_TRACE("not implemented");
673  // Do not throw and error since it is not subject
674  // to produce a failure
675  }
676 }
682 void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
683  const vpColor & /* color */, unsigned int /* thickness */) const
684 {
685  static int firsttime = 0;
686 
687  if (firsttime == 0) {
688  firsttime = 1;
689  vpERROR_TRACE("not implemented");
690  // Do not throw and error since it is not subject
691  // to produce a failure
692  }
693 }
694 
719 unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; }
744 unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1]; }
769 unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2]; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void print(const unsigned int select=FEATURE_ALL) const
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
vpFeatureThetaU * duplicate() const
Feature duplication.
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
void set_TUz(const double tu_z)
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4462
unsigned int dim_s
Dimension of the visual feature.
double get_TUz() const
void setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r)
vpFeatureThetaURotationRepresentationType getFeatureThetaURotationType() const
static double sinc(double x)
Definition: vpMath.cpp:170
Implementation of a rotation matrix and operations on such kind of matrices.
double get_TUx() const
vpFeatureThetaURotationRepresentationType
class that defines what is a visual feature
static unsigned int selectTUx()
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:108
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
void extract(vpRotationMatrix &R) const
void buildFrom(vpThetaUVector &tu)
static const unsigned int FEATURE_LINE[32]
void set_TUy(const double tu_y)
double sumSquare() const
vpBasicFeatureDeallocatorType deallocate
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void stack(double d)
double get_TUy() const
static unsigned int selectTUz()
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
static vpMatrix skew(const vpColVector &v)
static unsigned int selectTUy()
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
void eye()
Definition: vpMatrix.cpp:360
Implementation of a rotation vector as axis-angle minimal representation.
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
void set_TUx(const double tu_x)