ViSP  2.8.0
vpFeatureThetaU.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureThetaU.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * ThetaU visual feature.
36  *
37  * Authors:
38  * Eric Marchand
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 
44 #include <visp/vpBasicFeature.h>
45 #include <visp/vpFeatureThetaU.h>
46 #include <visp/vpMath.h>
47 
48 // Exception
49 #include <visp/vpException.h>
50 #include <visp/vpMatrixException.h>
51 #include <visp/vpFeatureException.h>
52 
53 // Debug trace
54 #include <visp/vpDebug.h>
55 
56 
61 /*
62 
63 attributes and members directly related to the vpBasicFeature needs
64 other functionalities are useful but not mandatory
65 
66 */
67 
73 void
75 {
76  //feature dimension
77  dim_s = 3 ;
78  nbParameters = 3;
79 
80  // memory allocation
81  s.resize(dim_s) ;
82  if (flags == NULL)
83  flags = new bool[nbParameters];
84  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
85 }
86 
96 {
97  //vpTRACE("0x%x", this);
98  init() ;
99 
100  // kind of rotation representation
101  rotation = r;
102 }
103 
126 {
127  init() ;
128 
129  buildFrom(tu) ;
130 
131  // kind of rotation representation
132  rotation = r;
133 }
134 
155 {
156  init() ;
157 
158  vpThetaUVector tu(R) ;
159  buildFrom(tu) ;
160 
161  // kind of rotation representation
162  rotation = r;
163 }
164 
187 {
188  init() ;
189  vpRotationMatrix R ;
190  M.extract(R) ;
191  vpThetaUVector tu(R) ;
192  buildFrom(tu) ;
193 
194  // kind of rotation representation
195  rotation = r;
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 
280 void vpFeatureThetaU::set_TUx(const double tu_x)
281 {
282  s[0] = tu_x ;
283  flags[0] = true;
284 }
293 void vpFeatureThetaU::set_TUy(const double tu_y)
294 {
295  s[1] = tu_y ;
296  flags[1] = true;
297 }
306 void
307 vpFeatureThetaU::set_TUz(const double tu_z)
308 {
309  s[2] = tu_z ;
310  flags[2] = true;
311 }
312 
320 {
321  return s[0] ;
322 }
323 
330 {
331  return s[1] ;
332 }
333 
334 
340 double
342 {
343  return s[2] ;
344 }
345 
346 
410 vpMatrix
411 vpFeatureThetaU::interaction(const unsigned int select)
412 {
413 
414  vpMatrix L ;
415  L.resize(0,6) ;
416 
418  {
419  for (unsigned int i = 0; i < nbParameters; i++)
420  {
421  if (flags[i] == false)
422  {
423  switch(i){
424  case 0:
425  vpTRACE("Warning !!! The interaction matrix is computed but Tu_x was not set yet");
426  break;
427  case 1:
428  vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
429  break;
430  case 2:
431  vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
432  break;
433  default:
434  vpTRACE("Problem during the reading of the variable flags");
435  }
436  }
437  }
438  resetFlags();
439  }
440 
441  // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
442  vpColVector u(3) ;
443  for (unsigned int i=0 ; i < 3 ; i++) {
444  u[i] = s[i]/2.0 ;
445  }
446 
447  vpMatrix Lw(3,3) ;
448  Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
449 
450  vpMatrix U2(3,3) ;
451  U2.setIdentity() ;
452 
453  double theta = sqrt(s.sumSquare()) ;
454  if (theta >= 1e-6) {
455  for (unsigned int i=0 ; i < 3 ; i++)
456  u[i] = s[i]/theta ;
457 
458  vpMatrix skew_u ;
459  skew_u = vpColVector::skew(u) ;
460  U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
461  }
462 
463  if (rotation == cdRc) {
464  Lw += U2;
465  }
466  else {
467  Lw -= U2;
468  }
469 
470  //This version is a simplification
471  if (vpFeatureThetaU::selectTUx() & select )
472  {
473  vpMatrix Lx(1,6) ;
474 
475  Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
476  for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
477 
478 
479  L = vpMatrix::stackMatrices(L,Lx) ;
480  }
481 
482  if (vpFeatureThetaU::selectTUy() & select )
483  {
484  vpMatrix Ly(1,6) ;
485 
486  Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
487  for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
488 
489  L = vpMatrix::stackMatrices(L,Ly) ;
490  }
491 
492  if (vpFeatureThetaU::selectTUz() & select )
493  {
494  vpMatrix Lz(1,6) ;
495 
496  Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
497  for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
498 
499  L = vpMatrix::stackMatrices(L,Lz) ;
500  }
501 
502  return L ;
503 }
504 
567  const unsigned int select)
568 {
569 
570  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
571  {
572  vpERROR_TRACE("s* should be zero ! ") ;
574  "s* should be zero !")) ;
575  }
576 
577  vpColVector e(0) ;
578 
579 
580  if (vpFeatureThetaU::selectTUx() & select )
581  {
582  vpColVector ex(1) ;
583  ex[0] = s[0] ;
584  e = vpMatrix::stackMatrices(e,ex) ;
585  }
586 
587  if (vpFeatureThetaU::selectTUy() & select )
588  {
589  vpColVector ey(1) ;
590  ey[0] = s[1] ;
591  e = vpMatrix::stackMatrices(e,ey) ;
592  }
593 
594  if (vpFeatureThetaU::selectTUz() & select )
595  {
596  vpColVector ez(1) ;
597  ez[0] = s[2] ;
598  e = vpMatrix::stackMatrices(e,ez) ;
599  }
600  return e ;
601 }
602 
629 void
630 vpFeatureThetaU::print(const unsigned int select) const
631 {
632  std::cout <<"ThetaU: ";
633  if (vpFeatureThetaU::selectTUx() & select ) {
634  std::cout << s[0] << " ";
635  }
636  if (vpFeatureThetaU::selectTUy() & select ) {
637  std::cout << s[1] << " ";
638  }
639  if (vpFeatureThetaU::selectTUz() & select ) {
640  std::cout << s[2] << " ";
641  }
642  std::cout << std::endl;
643 }
644 
657 {
658  vpFeatureThetaU *feature;
659  if (rotation == cdRc)
660  feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc) ;
661  else //if (rotation == cRcd
662  feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd) ;
663 
664  return feature ;
665 }
666 
672 void
674  const vpImage<unsigned char> &/* I */,
675  const vpColor &/* color */,
676  unsigned int /* thickness */) const
677 {
678  static int firsttime =0 ;
679 
680  if (firsttime==0)
681  {
682  firsttime=1 ;
683  vpERROR_TRACE("not implemented") ;
684  // Do not throw and error since it is not subject
685  // to produce a failure
686  }
687 }
693 void
695  const vpImage<vpRGBa> &/* I */,
696  const vpColor &/* color */,
697  unsigned int /* thickness */) const
698 {
699  static int firsttime =0 ;
700 
701  if (firsttime==0)
702  {
703  firsttime=1 ;
704  vpERROR_TRACE("not implemented") ;
705  // Do not throw and error since it is not subject
706  // to produce a failure
707  }
708 }
709 
710 /*
711  * Local variables:
712  * c-basic-offset: 2
713  * End:
714  */
vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r)
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void print(const unsigned int select=FEATURE_ALL) const
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
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)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:379
#define vpTRACE
Definition: vpDebug.h:401
void set_TUz(const double tu_z)
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
unsigned int dim_s
Dimension of the visual feature.
double get_TUz() const
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:760
static double sinc(double x)
Definition: vpMath.h:301
The vpRotationMatrix considers the particular case of a rotation matrix.
double get_TUx() const
vpFeatureThetaURotationRepresentationType
class that defines what is a visual feature
static unsigned int selectTUz()
void setIdentity(const double &val=1.0)
Definition: vpMatrix.cpp:1110
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
static double sqr(double x)
Definition: vpMath.h:106
Error that can be emited by the vpBasicFeature class and its derivates.
static unsigned int selectTUx()
static unsigned int selectTUy()
Generic class defining intrinsic camera parameters.
void extract(vpRotationMatrix &R) const
void buildFrom(vpThetaUVector &tu)
static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2263
void set_TUy(const double tu_y)
vpBasicFeatureDeallocatorType deallocate
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double get_TUy() const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
static vpMatrix skew(const vpColVector &v)
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
Class that consider the case of the parameterization for the rotation.
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:94
void set_TUx(const double tu_x)