Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureThetaU.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * ThetaU visual feature.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 
40 #include <visp3/visual_features/vpBasicFeature.h>
41 #include <visp3/visual_features/vpFeatureThetaU.h>
42 #include <visp3/core/vpMath.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 
51 
56 /*
57 
58 attributes and members directly related to the vpBasicFeature needs
59 other functionalities are useful but not mandatory
60 
61 */
62 
68 void
70 {
71  //feature dimension
72  dim_s = 3 ;
73  nbParameters = 3;
74 
75  // memory allocation
76  s.resize(dim_s) ;
77  if (flags == NULL)
78  flags = new bool[nbParameters];
79  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
80 }
81 
90  : rotation(vpFeatureThetaU::cdRc)
91 {
92  //vpTRACE("0x%x", this);
93  init() ;
94 }
95 
104  : rotation(r)
105 {
106  //vpTRACE("0x%x", this);
107  init() ;
108 
109  // kind of rotation representation
110  rotation = r;
111 }
112 
134  : rotation(r)
135 {
136  init() ;
137 
138  buildFrom(tu) ;
139 }
140 
160  : rotation(r)
161 {
162  init() ;
163 
164  vpThetaUVector tu(R) ;
165  buildFrom(tu) ;
166 }
167 
189  : rotation(r)
190 {
191  init() ;
192  vpRotationMatrix R ;
193  M.extract(R) ;
194  vpThetaUVector tu(R) ;
195  buildFrom(tu) ;
196 }
197 
214 void
216 {
217  s[0] = tu[0] ;
218  s[1] = tu[1] ;
219  s[2] = tu[2] ;
220  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
221 }
222 
238 void
240 {
241  vpThetaUVector tu(R) ;
242  buildFrom(tu) ;
243 }
244 
263 void
265 {
266  vpRotationMatrix R ;
267  M.extract(R) ;
268  vpThetaUVector tu(R) ;
269  buildFrom(tu) ;
270 }
271 
281 {
282  rotation = r;
283 }
284 
293 void vpFeatureThetaU::set_TUx(const double tu_x)
294 {
295  s[0] = tu_x ;
296  flags[0] = true;
297 }
306 void vpFeatureThetaU::set_TUy(const double tu_y)
307 {
308  s[1] = tu_y ;
309  flags[1] = true;
310 }
319 void
320 vpFeatureThetaU::set_TUz(const double tu_z)
321 {
322  s[2] = tu_z ;
323  flags[2] = true;
324 }
325 
335 {
336  return rotation;
337 }
338 
346 {
347  return s[0] ;
348 }
349 
356 {
357  return s[1] ;
358 }
359 
360 
366 double
368 {
369  return s[2] ;
370 }
371 
372 
436 vpMatrix
437 vpFeatureThetaU::interaction(const unsigned int select)
438 {
439 
440  vpMatrix L ;
441  L.resize(0,6) ;
442 
444  {
445  for (unsigned int i = 0; i < nbParameters; i++)
446  {
447  if (flags[i] == false)
448  {
449  switch(i){
450  case 0:
451  vpTRACE("Warning !!! The interaction matrix is computed but Tu_x was not set yet");
452  break;
453  case 1:
454  vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
455  break;
456  case 2:
457  vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
458  break;
459  default:
460  vpTRACE("Problem during the reading of the variable flags");
461  }
462  }
463  }
464  resetFlags();
465  }
466 
467  // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
468  vpColVector u(3) ;
469  for (unsigned int i=0 ; i < 3 ; i++) {
470  u[i] = s[i]/2.0 ;
471  }
472 
473  vpMatrix Lw(3,3) ;
474  Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
475 
476  vpMatrix U2(3,3) ;
477  U2.eye() ;
478 
479  double theta = sqrt(s.sumSquare()) ;
480  if (theta >= 1e-6) {
481  for (unsigned int i=0 ; i < 3 ; i++)
482  u[i] = s[i]/theta ;
483 
484  vpMatrix skew_u ;
485  skew_u = vpColVector::skew(u) ;
486  U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
487  }
488 
489  if (rotation == cdRc) {
490  Lw += U2;
491  }
492  else {
493  Lw -= U2;
494  }
495 
496  //This version is a simplification
497  if (vpFeatureThetaU::selectTUx() & select )
498  {
499  vpMatrix Lx(1,6) ;
500 
501  Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
502  for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
503 
504 
505  L = vpMatrix::stack(L,Lx) ;
506  }
507 
508  if (vpFeatureThetaU::selectTUy() & select )
509  {
510  vpMatrix Ly(1,6) ;
511 
512  Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
513  for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
514 
515  L = vpMatrix::stack(L,Ly) ;
516  }
517 
518  if (vpFeatureThetaU::selectTUz() & select )
519  {
520  vpMatrix Lz(1,6) ;
521 
522  Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
523  for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
524 
525  L = vpMatrix::stack(L,Lz) ;
526  }
527 
528  return L ;
529 }
530 
593  const unsigned int select)
594 {
595 
596  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
597  {
598  vpERROR_TRACE("s* should be zero ! ") ;
600  "s* should be zero !")) ;
601  }
602 
603  vpColVector e(0) ;
604 
605 
606  if (vpFeatureThetaU::selectTUx() & select )
607  {
608  vpColVector ex(1) ;
609  ex[0] = s[0] ;
610  e = vpColVector::stack(e,ex) ;
611  }
612 
613  if (vpFeatureThetaU::selectTUy() & select )
614  {
615  vpColVector ey(1) ;
616  ey[0] = s[1] ;
617  e = vpColVector::stack(e,ey) ;
618  }
619 
620  if (vpFeatureThetaU::selectTUz() & select )
621  {
622  vpColVector ez(1) ;
623  ez[0] = s[2] ;
624  e = vpColVector::stack(e,ez) ;
625  }
626  return e ;
627 }
628 
655 void
656 vpFeatureThetaU::print(const unsigned int select) const
657 {
658  std::cout <<"ThetaU: ";
659  if (vpFeatureThetaU::selectTUx() & select ) {
660  std::cout << s[0] << " ";
661  }
662  if (vpFeatureThetaU::selectTUy() & select ) {
663  std::cout << s[1] << " ";
664  }
665  if (vpFeatureThetaU::selectTUz() & select ) {
666  std::cout << s[2] << " ";
667  }
668  std::cout << std::endl;
669 }
670 
683 {
684  vpFeatureThetaU *feature;
685  if (rotation == cdRc)
686  feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc) ;
687  else //if (rotation == cRcd
688  feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd) ;
689 
690  return feature ;
691 }
692 
698 void
700  const vpImage<unsigned char> &/* I */,
701  const vpColor &/* color */,
702  unsigned int /* thickness */) const
703 {
704  static int firsttime =0 ;
705 
706  if (firsttime==0)
707  {
708  firsttime=1 ;
709  vpERROR_TRACE("not implemented") ;
710  // Do not throw and error since it is not subject
711  // to produce a failure
712  }
713 }
719 void
721  const vpImage<vpRGBa> &/* I */,
722  const vpColor &/* color */,
723  unsigned int /* thickness */) const
724 {
725  static int firsttime =0 ;
726 
727  if (firsttime==0)
728  {
729  firsttime=1 ;
730  vpERROR_TRACE("not implemented") ;
731  // Do not throw and error since it is not subject
732  // to produce a failure
733  }
734 }
735 
760 unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0] ; }
785 unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1] ; }
810 unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2] ; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void print(const unsigned int select=FEATURE_ALL) const
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
vpFeatureThetaU * duplicate() const
Feature duplication.
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
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:391
void set_TUz(const double tu_z)
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
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:169
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:414
static double sqr(double x)
Definition: vpMath.h:110
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
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:194
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:225
void set_TUx(const double tu_x)