Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpGenericFeature.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  * Generic feature (used to create new feature not implemented in ViSP).
32  *
33  * Authors:
34  * Eric Marchand
35  *
36  *****************************************************************************/
37 
38 
39 #include <visp3/visual_features/vpGenericFeature.h>
40 
41 
42 // Exception
43 #include <visp3/core/vpException.h>
44 #include <visp3/visual_features/vpFeatureException.h>
45 
46 // Debug trace
47 #include <visp3/core/vpDebug.h>
48 
49 
50 
58 {
59 
60 }
61 
63 {
64  s = 0 ;
65 }
66 
67 
78 vpGenericFeature::vpGenericFeature()
79  : L(), err(), errorStatus(errorNotInitalized)
80 {
81  /*
82  vpERROR_TRACE("You are not allow to use this constructor ") ;
83  vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
84  "constructor") ;
85  vpERROR_TRACE("And provide the dimension of the visual feature ") ;
86  throw(vpException(vpException::cannotUseConstructorError,
87  "You are not allow to use this constructor ")) ;
88  */
89 }
90 
91 
97 vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s)
98  : L(), err(), errorStatus(errorNotInitalized)
99 {
100  this->dim_s = dimension_gen_s ;
101  s.resize(dimension_gen_s) ;
102 }
103 
113 void
115 {
116  if (error_vector.getRows() != dim_s)
117  {
118  vpERROR_TRACE("size mismatch between error dimension"
119  "and feature dimension");
121  "size mismatch between error dimension"
122  "and feature dimension"));
123 
124  }
125  errorStatus = errorInitialized ;
126  err = error_vector ;
127 }
128 
129 
187  const unsigned int select)
188 {
189  if (s_star.get_s().getRows() != dim_s)
190  {
191  vpERROR_TRACE("size mismatch between s* dimension "
192  "and feature dimension");
194  "size mismatch between s* dimension "
195  "and feature dimension"));
196 
197  }
198 
199  vpColVector e(0) ;
200 
201  try
202  {
203  if (errorStatus == errorHasToBeUpdated)
204  {
205  vpERROR_TRACE("Error has no been updated since last iteration"
206  "you should have used vpGenericFeature::setError"
207  "in you visual servoing loop") ;
209  "Error has no been updated since last iteration"));
210  }
211  else
212  if (errorStatus == errorInitialized)
213  {
214  vpDEBUG_TRACE(25,"Error init: e=e.");
215  errorStatus = errorHasToBeUpdated ;
216  for (unsigned int i=0 ; i < dim_s ; i++)
217  if (FEATURE_LINE[i] & select )
218  {
219  vpColVector ex(1) ;
220  ex[i] = err[i] ;
221 
222  e = vpColVector::stack(e,ex) ;
223  }
224  }
225  else
226  {
227  vpDEBUG_TRACE(25,"Error not init: e=s-s*.");
228 
229  for (unsigned int i=0 ; i < dim_s ; i++)
230  if (FEATURE_LINE[i] & select )
231  {
232  vpColVector ex(1) ;
233  ex[0] = s[i] - s_star[i] ;
234 
235  e = vpColVector::stack(e,ex) ;
236  }
237 
238  }
239  }
240  catch(...) {
241  throw;
242  }
243  return e ;
244 
245 }
246 
247 
248 
290 vpGenericFeature::error( const unsigned int select)
291 {
292  vpColVector e(0) ;
293 
294  try
295  {
296  if (errorStatus == errorHasToBeUpdated)
297  {
298  vpERROR_TRACE("Error has no been updated since last iteration"
299  "you should have used vpGenericFeature::setError"
300  "in you visual servoing loop") ;
302  "Error has no been updated since last iteration"));
303  }
304  else
305  if (errorStatus == errorInitialized)
306  {
307  errorStatus = errorHasToBeUpdated ;
308  for (unsigned int i=0 ; i < dim_s ; i++)
309  if (FEATURE_LINE[i] & select )
310  {
311  vpColVector ex(1) ;
312  ex[i] = err[i] ;
313 
314  e = vpColVector::stack(e,ex) ;
315  }
316  }
317  else
318  {
319 
320  for (unsigned int i=0 ; i < dim_s ; i++)
321  if (FEATURE_LINE[i] & select )
322  {
323  vpColVector ex(1) ;
324  ex[i] = s[i] ;
325 
326  e = vpColVector::stack(e,ex) ;
327  }
328 
329  }
330  }
331  catch(...) {
332  throw;
333  }
334 
335  return e ;
336 
337 }
338 
339 
390 vpMatrix
391 vpGenericFeature::interaction(const unsigned int select)
392 {
393  if (L.getRows() == 0)
394  {
395  std::cout << "interaction matrix " << L << std::endl ;
396  vpERROR_TRACE("Interaction has not been initialized");
397  std::cout << "A possible reason (may be) is that you have set" << std::endl ;
398  std::cout << "the interaction matrix for s and compute a control " << std::endl ;
399  std::cout << "with Ls=s* (default) or vice versa" << std::endl ;
400 
402  "size mismatch between s* dimension "
403  "and feature dimension"));
404 
405  }
406 
407  vpMatrix Ls ;
408 
409  Ls.resize(0,6) ;
410 
411  for (unsigned int i=0 ; i < dim_s ; i++)
412  if (FEATURE_LINE[i] & select )
413  {
414  vpMatrix Lx(1,6) ; Lx = 0;
415 
416  for (int j=0 ; j < 6 ; j++)
417  Lx[0][j] = L[i][j] ;
418 
419  Ls = vpMatrix::stack(Ls,Lx) ;
420  }
421 
422  return Ls ;
423 }
424 
425 
435 void
437 {
438  if (L_.getRows() != dim_s)
439  {
440  std::cout << L_.getRows() <<" " << dim_s << std::endl ;;
441  vpERROR_TRACE("size mismatch between interaction matrix size "
442  "and feature dimension");
444  "size mismatch between interaction matrix size "
445  "and feature dimension"));
446  }
447 
448  this->L = L_ ;
449 }
450 
460 void
462 {
463 
464  if (s_vector.getRows() != dim_s)
465  {
466  vpERROR_TRACE("size mismatch between s dimension"
467  "and feature dimension");
469  "size mismatch between s dimension"
470  "and feature dimension"));
471  }
472  this->s = s_vector ;
473 }
474 
475 
485 void
487 {
488  if (s_vector.getRows() != dim_s)
489  {
490  vpERROR_TRACE("size mismatch between s dimension"
491  "and feature dimension");
493  "size mismatch between s dimension"
494  "and feature dimension"));
495  }
496  s_vector = this->s ;
497 }
498 
499 
513 void
514 vpGenericFeature::set_s(const double s0, const double s1, const double s2)
515 {
516 
517  if (3 != dim_s)
518  {
519  vpERROR_TRACE("size mismatch between number of parameters"
520  "and feature dimension");
522  "size mismatch between number of parameters"
523  "and feature dimension"));
524 
525  }
526  s[0] = s0 ; s[1] = s1 ; s[2] = s2 ;
527 }
528 
529 
543 void
544 vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
545 {
546 
547  if (3 != dim_s)
548  {
549  vpERROR_TRACE("size mismatch between number of parameters"
550  "and feature dimension");
552  "size mismatch between number of parameters"
553  "and feature dimension"));
554 
555  }
556  s0 = s[0] ; s1 = s[1] ; s2 = s[2] ;
557 }
558 
559 
571 void
572 vpGenericFeature::set_s(const double s0, const double s1)
573 {
574 
575  if (2 != 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  s[0] = s0 ; s[1] = s1 ;
585 }
586 
587 
599 void
600 vpGenericFeature::get_s(double &s0, double &s1) const
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  s0 = s[0] ; s1 = s[1] ;
613 }
614 
615 
625 void
626 vpGenericFeature::set_s(const double s0)
627 {
628 
629  if (1 != dim_s)
630  {
631  vpERROR_TRACE("size mismatch between number of parameters"
632  "and feature dimension");
634  "size mismatch between number of parameters"
635  "and feature dimension"));
636 
637  }
638  s[0] = s0 ;
639 }
640 
641 
651 void
652 vpGenericFeature::get_s(double &s0) const
653 {
654 
655  if (1 != dim_s)
656  {
657  vpERROR_TRACE("size mismatch between number of parameters"
658  "and feature dimension");
660  "size mismatch between number of parameters"
661  "and feature dimension"));
662 
663  }
664  s0 = s[0];
665 }
666 
667 
689 void
690 vpGenericFeature::print(const unsigned int select) const
691 {
692 
693  std::cout <<"Generic Feature: " ;
694  for (unsigned int i=0 ; i < dim_s ; i++)
695  if (FEATURE_LINE[i] & select )
696  {
697  std::cout << " s["<<i << "]=" << s[i] ;
698  }
699 
700  std::cout <<std::endl ;
701 }
702 
704 {
705  vpGenericFeature *feature= new vpGenericFeature(dim_s) ;
706 
707  vpTRACE("dims = %d",dim_s) ;
708  return feature ;
709 }
710 
714 void
716  const vpImage<unsigned char> &/* I */,
717  const vpColor &/* color */,
718  unsigned int /* thickness */) const
719 {
720  static int firsttime =0 ;
721 
722  if (firsttime==0)
723  {
724  firsttime=1 ;
725  vpERROR_TRACE("not implemented") ;
726  // Do not throw and error since it is not subject
727  // to produce a failure
728  }
729 }
733 void
735  const vpImage<vpRGBa> &/* I */,
736  const vpColor &/* color */,
737  unsigned int /* thickness */) const
738 {
739  static int firsttime =0 ;
740 
741  if (firsttime==0)
742  {
743  firsttime=1 ;
744  vpERROR_TRACE("not implemented") ;
745  // Do not throw and error since it is not subject
746  // to produce a failure
747  }
748 }
749 
750 /*
751  * Local variables:
752  * c-basic-offset: 2
753  * End:
754  */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void set_s(const vpColVector &s)
set the value of all the features.
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
#define vpERROR_TRACE
Definition: vpDebug.h:391
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.
feature list or desired feature list is empty
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
#define vpTRACE
Definition: vpDebug.h:414
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.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
static const unsigned int FEATURE_LINE[32]
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void print(const unsigned int select=FEATURE_ALL) const
vpGenericFeature * duplicate() const
#define vpDEBUG_TRACE
Definition: vpDebug.h:478
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
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:225
void setError(const vpColVector &error_vector)