ViSP  2.10.0
vpFeatureThetaU.cpp
1 /****************************************************************************
2  *
3  * $Id: vpFeatureThetaU.cpp 4632 2014-02-03 17:06:40Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 
95  : rotation(r)
96 {
97  //vpTRACE("0x%x", this);
98  init() ;
99 
100  // kind of rotation representation
101  rotation = r;
102 }
103 
125  : rotation(r)
126 {
127  init() ;
128 
129  buildFrom(tu) ;
130 }
131 
151  : rotation(r)
152 {
153  init() ;
154 
155  vpThetaUVector tu(R) ;
156  buildFrom(tu) ;
157 }
158 
180  : rotation(r)
181 {
182  init() ;
183  vpRotationMatrix R ;
184  M.extract(R) ;
185  vpThetaUVector tu(R) ;
186  buildFrom(tu) ;
187 }
188 
205 void
207 {
208  s[0] = tu[0] ;
209  s[1] = tu[1] ;
210  s[2] = tu[2] ;
211  for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
212 }
213 
229 void
231 {
232  vpThetaUVector tu(R) ;
233  buildFrom(tu) ;
234 }
235 
254 void
256 {
257  vpRotationMatrix R ;
258  M.extract(R) ;
259  vpThetaUVector tu(R) ;
260  buildFrom(tu) ;
261 }
262 
271 void vpFeatureThetaU::set_TUx(const double tu_x)
272 {
273  s[0] = tu_x ;
274  flags[0] = true;
275 }
284 void vpFeatureThetaU::set_TUy(const double tu_y)
285 {
286  s[1] = tu_y ;
287  flags[1] = true;
288 }
297 void
298 vpFeatureThetaU::set_TUz(const double tu_z)
299 {
300  s[2] = tu_z ;
301  flags[2] = true;
302 }
303 
311 {
312  return s[0] ;
313 }
314 
321 {
322  return s[1] ;
323 }
324 
325 
331 double
333 {
334  return s[2] ;
335 }
336 
337 
401 vpMatrix
402 vpFeatureThetaU::interaction(const unsigned int select)
403 {
404 
405  vpMatrix L ;
406  L.resize(0,6) ;
407 
409  {
410  for (unsigned int i = 0; i < nbParameters; i++)
411  {
412  if (flags[i] == false)
413  {
414  switch(i){
415  case 0:
416  vpTRACE("Warning !!! The interaction matrix is computed but Tu_x was not set yet");
417  break;
418  case 1:
419  vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
420  break;
421  case 2:
422  vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
423  break;
424  default:
425  vpTRACE("Problem during the reading of the variable flags");
426  }
427  }
428  }
429  resetFlags();
430  }
431 
432  // Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
433  vpColVector u(3) ;
434  for (unsigned int i=0 ; i < 3 ; i++) {
435  u[i] = s[i]/2.0 ;
436  }
437 
438  vpMatrix Lw(3,3) ;
439  Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
440 
441  vpMatrix U2(3,3) ;
442  U2.setIdentity() ;
443 
444  double theta = sqrt(s.sumSquare()) ;
445  if (theta >= 1e-6) {
446  for (unsigned int i=0 ; i < 3 ; i++)
447  u[i] = s[i]/theta ;
448 
449  vpMatrix skew_u ;
450  skew_u = vpColVector::skew(u) ;
451  U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
452  }
453 
454  if (rotation == cdRc) {
455  Lw += U2;
456  }
457  else {
458  Lw -= U2;
459  }
460 
461  //This version is a simplification
462  if (vpFeatureThetaU::selectTUx() & select )
463  {
464  vpMatrix Lx(1,6) ;
465 
466  Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
467  for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
468 
469 
470  L = vpMatrix::stackMatrices(L,Lx) ;
471  }
472 
473  if (vpFeatureThetaU::selectTUy() & select )
474  {
475  vpMatrix Ly(1,6) ;
476 
477  Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
478  for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
479 
480  L = vpMatrix::stackMatrices(L,Ly) ;
481  }
482 
483  if (vpFeatureThetaU::selectTUz() & select )
484  {
485  vpMatrix Lz(1,6) ;
486 
487  Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
488  for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
489 
490  L = vpMatrix::stackMatrices(L,Lz) ;
491  }
492 
493  return L ;
494 }
495 
558  const unsigned int select)
559 {
560 
561  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
562  {
563  vpERROR_TRACE("s* should be zero ! ") ;
565  "s* should be zero !")) ;
566  }
567 
568  vpColVector e(0) ;
569 
570 
571  if (vpFeatureThetaU::selectTUx() & select )
572  {
573  vpColVector ex(1) ;
574  ex[0] = s[0] ;
575  e = vpMatrix::stackMatrices(e,ex) ;
576  }
577 
578  if (vpFeatureThetaU::selectTUy() & select )
579  {
580  vpColVector ey(1) ;
581  ey[0] = s[1] ;
582  e = vpMatrix::stackMatrices(e,ey) ;
583  }
584 
585  if (vpFeatureThetaU::selectTUz() & select )
586  {
587  vpColVector ez(1) ;
588  ez[0] = s[2] ;
589  e = vpMatrix::stackMatrices(e,ez) ;
590  }
591  return e ;
592 }
593 
620 void
621 vpFeatureThetaU::print(const unsigned int select) const
622 {
623  std::cout <<"ThetaU: ";
624  if (vpFeatureThetaU::selectTUx() & select ) {
625  std::cout << s[0] << " ";
626  }
627  if (vpFeatureThetaU::selectTUy() & select ) {
628  std::cout << s[1] << " ";
629  }
630  if (vpFeatureThetaU::selectTUz() & select ) {
631  std::cout << s[2] << " ";
632  }
633  std::cout << std::endl;
634 }
635 
648 {
649  vpFeatureThetaU *feature;
650  if (rotation == cdRc)
651  feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc) ;
652  else //if (rotation == cRcd
653  feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd) ;
654 
655  return feature ;
656 }
657 
663 void
665  const vpImage<unsigned char> &/* I */,
666  const vpColor &/* color */,
667  unsigned int /* thickness */) const
668 {
669  static int firsttime =0 ;
670 
671  if (firsttime==0)
672  {
673  firsttime=1 ;
674  vpERROR_TRACE("not implemented") ;
675  // Do not throw and error since it is not subject
676  // to produce a failure
677  }
678 }
684 void
686  const vpImage<vpRGBa> &/* I */,
687  const vpColor &/* color */,
688  unsigned int /* thickness */) const
689 {
690  static int firsttime =0 ;
691 
692  if (firsttime==0)
693  {
694  firsttime=1 ;
695  vpERROR_TRACE("not implemented") ;
696  // Do not throw and error since it is not subject
697  // to produce a failure
698  }
699 }
700 
701 /*
702  * Local variables:
703  * c-basic-offset: 2
704  * End:
705  */
vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r)
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
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:199
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:395
#define vpTRACE
Definition: vpDebug.h:418
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
Definition: vpMatrix.cpp:868
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:1230
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 stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3272
void buildFrom(vpThetaUVector &tu)
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:98
void set_TUx(const double tu_x)