ViSP  2.8.0
vpGenericFeature.cpp
1 /****************************************************************************
2  *
3  * $Id: vpGenericFeature.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  * Generic feature (used to create new feature not implemented in ViSP).
36  *
37  * Authors:
38  * Eric Marchand
39  *
40  *****************************************************************************/
41 
42 
43 #include <visp/vpGenericFeature.h>
44 
45 
46 // Exception
47 #include <visp/vpException.h>
48 #include <visp/vpMatrixException.h>
49 #include <visp/vpFeatureException.h>
50 
51 // Debug trace
52 #include <visp/vpDebug.h>
53 
54 
55 
63 {
64 
65 }
66 
68 {
69  s = 0 ;
70 }
71 
72 
83 vpGenericFeature::vpGenericFeature()
84 {
85  /*
86  vpERROR_TRACE("You are not allow to use this constructor ") ;
87  vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
88  "constructor") ;
89  vpERROR_TRACE("And provide the dimension of the visual feature ") ;
90  throw(vpException(vpException::cannotUseConstructorError,
91  "You are not allow to use this constructor ")) ;
92  */
93 }
94 
95 
101 vpGenericFeature::vpGenericFeature(unsigned int dim_s)
102 {
103  this->dim_s = dim_s ;
104  s.resize(dim_s) ;
105  errorStatus = errorNotInitalized ;
106 }
107 
117 void
119 {
120  if (error.getRows() != dim_s)
121  {
122  vpERROR_TRACE("size mismatch between error dimension"
123  "and feature dimension");
125  "size mismatch between error dimension"
126  "and feature dimension"));
127 
128  }
129  errorStatus = errorInitialized ;
130  err = error ;
131 }
132 
133 
191  const unsigned int select)
192 {
193  if (s_star.get_s().getRows() != dim_s)
194  {
195  vpERROR_TRACE("size mismatch between s* dimension "
196  "and feature dimension");
198  "size mismatch between s* dimension "
199  "and feature dimension"));
200 
201  }
202 
203  vpColVector e(0) ;
204 
205  try
206  {
207  if (errorStatus == errorHasToBeUpdated)
208  {
209  vpERROR_TRACE("Error has no been updated since last iteration"
210  "you should have used vpGenericFeature::setError"
211  "in you visual servoing loop") ;
213  "Error has no been updated since last iteration"));
214  }
215  else
216  if (errorStatus == errorInitialized)
217  {
218  vpDEBUG_TRACE(25,"Error init: e=e.");
219  errorStatus = errorHasToBeUpdated ;
220  for (unsigned int i=0 ; i < dim_s ; i++)
221  if (FEATURE_LINE[i] & select )
222  {
223  vpColVector ex(1) ;
224  ex[i] = err[i] ;
225 
226  e = vpMatrix::stackMatrices(e,ex) ;
227  }
228  }
229  else
230  {
231  vpDEBUG_TRACE(25,"Error not init: e=s-s*.");
232 
233  for (unsigned int i=0 ; i < dim_s ; i++)
234  if (FEATURE_LINE[i] & select )
235  {
236  vpColVector ex(1) ;
237  ex[0] = s[i] - s_star[i] ;
238 
239  e = vpMatrix::stackMatrices(e,ex) ;
240  }
241 
242  }
243  }
244  catch(vpMatrixException me)
245  {
246  vpERROR_TRACE("caught a Matric related error") ;
247  std::cout <<std::endl << me << std::endl ;
248  throw(me) ;
249  }
250  catch(vpException me)
251  {
252  vpERROR_TRACE("caught another error") ;
253  std::cout <<std::endl << me << std::endl ;
254  throw(me) ;
255  }
256  return e ;
257 
258 }
259 
260 
261 
303 vpGenericFeature::error( const unsigned int select)
304 {
305  vpColVector e(0) ;
306 
307  try
308  {
309  if (errorStatus == errorHasToBeUpdated)
310  {
311  vpERROR_TRACE("Error has no been updated since last iteration"
312  "you should have used vpGenericFeature::setError"
313  "in you visual servoing loop") ;
315  "Error has no been updated since last iteration"));
316  }
317  else
318  if (errorStatus == errorInitialized)
319  {
320  errorStatus = errorHasToBeUpdated ;
321  for (unsigned int i=0 ; i < dim_s ; i++)
322  if (FEATURE_LINE[i] & select )
323  {
324  vpColVector ex(1) ;
325  ex[i] = err[i] ;
326 
327  e = vpMatrix::stackMatrices(e,ex) ;
328  }
329  }
330  else
331  {
332 
333  for (unsigned int i=0 ; i < dim_s ; i++)
334  if (FEATURE_LINE[i] & select )
335  {
336  vpColVector ex(1) ;
337  ex[i] = s[i] ;
338 
339  e = vpMatrix::stackMatrices(e,ex) ;
340  }
341 
342  }
343  }
344  catch(vpMatrixException me)
345  {
346  vpERROR_TRACE("caught a Matric related error") ;
347  std::cout <<std::endl << me << std::endl ;
348  throw(me) ;
349  }
350  catch(vpException me)
351  {
352  vpERROR_TRACE("caught another error") ;
353  std::cout <<std::endl << me << std::endl ;
354  throw(me) ;
355  }
356 
357  return e ;
358 
359 }
360 
361 
412 vpMatrix
413 vpGenericFeature::interaction(const unsigned int select)
414 {
415  if (L.getRows() == 0)
416  {
417  std::cout << "interaction matrix " << L << std::endl ;
418  vpERROR_TRACE("Interaction has not been initialized");
419  std::cout << "A possible reason (may be) is that you have set" << std::endl ;
420  std::cout << "the interaction matrix for s and compute a control " << std::endl ;
421  std::cout << "with Ls=s* (default) or vice versa" << std::endl ;
422 
424  "size mismatch between s* dimension "
425  "and feature dimension"));
426 
427  }
428 
429  vpMatrix Ls ;
430 
431  Ls.resize(0,6) ;
432 
433  for (unsigned int i=0 ; i < dim_s ; i++)
434  if (FEATURE_LINE[i] & select )
435  {
436  vpMatrix Lx(1,6) ; Lx = 0;
437 
438  for (int j=0 ; j < 6 ; j++)
439  Lx[0][j] = L[i][j] ;
440 
441  Ls = vpMatrix::stackMatrices(Ls,Lx) ;
442  }
443 
444  return Ls ;
445 }
446 
447 
457 void
459 {
460 
461  if (L.getRows() != dim_s)
462  {
463  std::cout << L.getRows() <<" " << dim_s << std::endl ;;
464  vpERROR_TRACE("size mismatch between interaction matrix size "
465  "and feature dimension");
467  "size mismatch between interaction matrix size "
468  "and feature dimension"));
469 
470  }
471 
472  this->L = L ;
473 
474 }
475 
485 void
487 {
488 
489  if (s.getRows() != dim_s)
490  {
491  vpERROR_TRACE("size mismatch between s dimension"
492  "and feature dimension");
494  "size mismatch between s dimension"
495  "and feature dimension"));
496 
497  }
498  this->s = s ;
499 }
500 
501 
511 void
513 {
514 
515  if (s.getRows() != dim_s)
516  {
517  vpERROR_TRACE("size mismatch between s dimension"
518  "and feature dimension");
520  "size mismatch between s dimension"
521  "and feature dimension"));
522 
523  }
524  s = this->s ;
525 }
526 
527 
541 void
542 vpGenericFeature::set_s(const double s0, const double s1, const double s2)
543 {
544 
545  if (3 != dim_s)
546  {
547  vpERROR_TRACE("size mismatch between number of parameters"
548  "and feature dimension");
550  "size mismatch between number of parameters"
551  "and feature dimension"));
552 
553  }
554  s[0] = s0 ; s[1] = s1 ; s[2] = s2 ;
555 }
556 
557 
571 void
572 vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
573 {
574 
575  if (3 != dim_s)
576  {
577  vpERROR_TRACE("size mismatch between number of parameters"
578  "and feature dimension");
580  "size mismatch between number of parameters"
581  "and feature dimension"));
582 
583  }
584  s0 = s[0] ; s1 = s[1] ; s2 = s[2] ;
585 }
586 
587 
599 void
600 vpGenericFeature::set_s(const double s0, const double s1)
601 {
602 
603  if (2 != dim_s)
604  {
605  vpERROR_TRACE("size mismatch between number of parameters"
606  "and feature dimension");
608  "size mismatch between number of parameters"
609  "and feature dimension"));
610 
611  }
612  s[0] = s0 ; s[1] = s1 ;
613 }
614 
615 
627 void
628 vpGenericFeature::get_s(double &s0, double &s1) const
629 {
630 
631  if (2 != dim_s)
632  {
633  vpERROR_TRACE("size mismatch between number of parameters"
634  "and feature dimension");
636  "size mismatch between number of parameters"
637  "and feature dimension"));
638 
639  }
640  s0 = s[0] ; s1 = s[1] ;
641 }
642 
643 
653 void
654 vpGenericFeature::set_s(const double s0)
655 {
656 
657  if (1 != dim_s)
658  {
659  vpERROR_TRACE("size mismatch between number of parameters"
660  "and feature dimension");
662  "size mismatch between number of parameters"
663  "and feature dimension"));
664 
665  }
666  s[0] = s0 ;
667 }
668 
669 
679 void
680 vpGenericFeature::get_s(double &s0) const
681 {
682 
683  if (1 != dim_s)
684  {
685  vpERROR_TRACE("size mismatch between number of parameters"
686  "and feature dimension");
688  "size mismatch between number of parameters"
689  "and feature dimension"));
690 
691  }
692  s0 = s[0];
693 }
694 
695 
717 void
718 vpGenericFeature::print(const unsigned int select) const
719 {
720 
721  std::cout <<"Generic Feature: " ;
722  for (unsigned int i=0 ; i < dim_s ; i++)
723  if (FEATURE_LINE[i] & select )
724  {
725  std::cout << " s["<<i << "]=" << s[i] ;
726  }
727 
728  std::cout <<std::endl ;
729 }
730 
732 {
733  vpGenericFeature *feature= new vpGenericFeature(dim_s) ;
734 
735  vpTRACE("dims = %d",dim_s) ;
736  return feature ;
737 }
738 
742 void
744  const vpImage<unsigned char> &/* I */,
745  const vpColor &/* color */,
746  unsigned int /* thickness */) const
747 {
748  static int firsttime =0 ;
749 
750  if (firsttime==0)
751  {
752  firsttime=1 ;
753  vpERROR_TRACE("not implemented") ;
754  // Do not throw and error since it is not subject
755  // to produce a failure
756  }
757 }
761 void
763  const vpImage<vpRGBa> &/* I */,
764  const vpColor &/* color */,
765  unsigned int /* thickness */) const
766 {
767  static int firsttime =0 ;
768 
769  if (firsttime==0)
770  {
771  firsttime=1 ;
772  vpERROR_TRACE("not implemented") ;
773  // Do not throw and error since it is not subject
774  // to produce a failure
775  }
776 }
777 
778 /*
779  * Local variables:
780  * c-basic-offset: 2
781  * End:
782  */
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
void set_s(const vpColVector &s)
set the value of all the features.
#define vpERROR_TRACE
Definition: vpDebug.h:379
#define vpTRACE
Definition: vpDebug.h:401
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
unsigned int dim_s
Dimension of the visual feature.
error that can be emited by ViSP classes.
Definition: vpException.h:75
feature list or desired feature list is empty
void setError(vpColVector &error)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
void get_s(vpColVector &s) const
get the value of all the features.
class that defines what is a visual feature
Error that can be emited by the vpBasicFeature class and its derivates.
virtual ~vpGenericFeature()
Generic class defining intrinsic camera parameters.
void setInteractionMatrix(const vpMatrix &L)
set the value of the interaction matrix.
static const unsigned int FEATURE_LINE[32]
static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B)
Stack two Matrices C = [ A B ]^T.
Definition: vpMatrix.cpp:2263
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void print(const unsigned int select=FEATURE_ALL) const
error that can be emited by the vpMatrix class and its derivates
vpGenericFeature * duplicate() const
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
vpColVector s
State of the visual feature.
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94