Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpGenericFeature.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Generic feature (used to create new feature not implemented in ViSP).
33  *
34  * Authors:
35  * Eric Marchand
36  *
37  *****************************************************************************/
38 
39 #include <visp3/visual_features/vpGenericFeature.h>
40 
41 // Exception
42 #include <visp3/core/vpException.h>
43 #include <visp3/visual_features/vpFeatureException.h>
44 
45 // Debug trace
46 #include <visp3/core/vpDebug.h>
47 
55 
56 void vpGenericFeature::init() { s = 0; }
57 
68 vpGenericFeature::vpGenericFeature() : L(), err(), errorStatus(errorNotInitalized)
69 {
70  /*
71  vpERROR_TRACE("You are not allow to use this constructor ") ;
72  vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
73  "constructor") ;
74  vpERROR_TRACE("And provide the dimension of the visual feature ") ;
75  throw(vpException(vpException::cannotUseConstructorError,
76  "You are not allow to use this constructor ")) ;
77  */
78 }
79 
87 vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s) : L(), err(), errorStatus(errorNotInitalized)
88 {
89  this->dim_s = dimension_gen_s;
90  s.resize(dimension_gen_s);
91 }
92 
102 void vpGenericFeature::setError(const vpColVector &error_vector)
103 {
104  if (error_vector.getRows() != dim_s) {
105  vpERROR_TRACE("size mismatch between error dimension"
106  "and feature dimension");
107  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between error dimension"
108  "and feature dimension"));
109  }
110  errorStatus = errorInitialized;
111  err = error_vector;
112 }
113 
169 vpColVector vpGenericFeature::error(const vpBasicFeature &s_star, const unsigned int select)
170 {
171  if (s_star.get_s().getRows() != dim_s) {
172  vpERROR_TRACE("size mismatch between s* dimension "
173  "and feature dimension");
174  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s* dimension "
175  "and feature dimension"));
176  }
177 
178  vpColVector e(0);
179 
180  try {
181  if (errorStatus == errorHasToBeUpdated) {
182  vpERROR_TRACE("Error has no been updated since last iteration"
183  "you should have used vpGenericFeature::setError"
184  "in you visual servoing loop");
186  "Error has no been updated since last iteration"));
187  } else if (errorStatus == errorInitialized) {
188  vpDEBUG_TRACE(25, "Error init: e=e.");
189  errorStatus = errorHasToBeUpdated;
190  for (unsigned int i = 0; i < dim_s; i++)
191  if (FEATURE_LINE[i] & select) {
192  vpColVector ex(1);
193  ex[i] = err[i];
194 
195  e = vpColVector::stack(e, ex);
196  }
197  } else {
198  vpDEBUG_TRACE(25, "Error not init: e=s-s*.");
199 
200  for (unsigned int i = 0; i < dim_s; i++)
201  if (FEATURE_LINE[i] & select) {
202  vpColVector ex(1);
203  ex[0] = s[i] - s_star[i];
204 
205  e = vpColVector::stack(e, ex);
206  }
207  }
208  } catch (...) {
209  throw;
210  }
211  return e;
212 }
213 
254 vpColVector vpGenericFeature::error(const unsigned int select)
255 {
256  vpColVector e(0);
257 
258  try {
259  if (errorStatus == errorHasToBeUpdated) {
260  vpERROR_TRACE("Error has no been updated since last iteration"
261  "you should have used vpGenericFeature::setError"
262  "in you visual servoing loop");
264  "Error has no been updated since last iteration"));
265  } else if (errorStatus == errorInitialized) {
266  errorStatus = errorHasToBeUpdated;
267  for (unsigned int i = 0; i < dim_s; i++)
268  if (FEATURE_LINE[i] & select) {
269  vpColVector ex(1);
270  ex[i] = err[i];
271 
272  e = vpColVector::stack(e, ex);
273  }
274  } else {
275 
276  for (unsigned int i = 0; i < dim_s; i++)
277  if (FEATURE_LINE[i] & select) {
278  vpColVector ex(1);
279  ex[i] = s[i];
280 
281  e = vpColVector::stack(e, ex);
282  }
283  }
284  } catch (...) {
285  throw;
286  }
287 
288  return e;
289 }
290 
341 vpMatrix vpGenericFeature::interaction(const unsigned int select)
342 {
343  if (L.getRows() == 0) {
344  std::cout << "interaction matrix " << L << std::endl;
345  vpERROR_TRACE("Interaction has not been initialized");
346  std::cout << "A possible reason (may be) is that you have set" << std::endl;
347  std::cout << "the interaction matrix for s and compute a control " << std::endl;
348  std::cout << "with Ls=s* (default) or vice versa" << std::endl;
349 
350  throw(vpFeatureException(vpFeatureException::notInitializedError, "size mismatch between s* dimension "
351  "and feature dimension"));
352  }
353 
354  vpMatrix Ls;
355 
356  Ls.resize(0, 6);
357 
358  for (unsigned int i = 0; i < dim_s; i++)
359  if (FEATURE_LINE[i] & select) {
360  vpMatrix Lx(1, 6);
361  Lx = 0;
362 
363  for (int j = 0; j < 6; j++)
364  Lx[0][j] = L[i][j];
365 
366  Ls = vpMatrix::stack(Ls, Lx);
367  }
368 
369  return Ls;
370 }
371 
382 {
383  if (L_.getRows() != dim_s) {
384  std::cout << L_.getRows() << " " << dim_s << std::endl;
385  ;
386  vpERROR_TRACE("size mismatch between interaction matrix size "
387  "and feature dimension");
388  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between interaction matrix size "
389  "and feature dimension"));
390  }
391 
392  this->L = L_;
393 }
394 
406 {
407 
408  if (s_vector.getRows() != dim_s) {
409  vpERROR_TRACE("size mismatch between s dimension"
410  "and feature dimension");
411  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
412  "and feature dimension"));
413  }
414  this->s = s_vector;
415 }
416 
428 {
429  if (s_vector.getRows() != dim_s) {
430  vpERROR_TRACE("size mismatch between s dimension"
431  "and feature dimension");
432  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
433  "and feature dimension"));
434  }
435  s_vector = this->s;
436 }
437 
452 void vpGenericFeature::set_s(const double s0, const double s1, const double s2)
453 {
454 
455  if (3 != dim_s) {
456  vpERROR_TRACE("size mismatch between number of parameters"
457  "and feature dimension");
458  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
459  "and feature dimension"));
460  }
461  s[0] = s0;
462  s[1] = s1;
463  s[2] = s2;
464 }
465 
480 void vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
481 {
482 
483  if (3 != dim_s) {
484  vpERROR_TRACE("size mismatch between number of parameters"
485  "and feature dimension");
486  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
487  "and feature dimension"));
488  }
489  s0 = s[0];
490  s1 = s[1];
491  s2 = s[2];
492 }
493 
505 void vpGenericFeature::set_s(const double s0, const double s1)
506 {
507 
508  if (2 != dim_s) {
509  vpERROR_TRACE("size mismatch between number of parameters"
510  "and feature dimension");
511  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
512  "and feature dimension"));
513  }
514  s[0] = s0;
515  s[1] = s1;
516 }
517 
529 void vpGenericFeature::get_s(double &s0, double &s1) const
530 {
531 
532  if (2 != dim_s) {
533  vpERROR_TRACE("size mismatch between number of parameters"
534  "and feature dimension");
535  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
536  "and feature dimension"));
537  }
538  s0 = s[0];
539  s1 = s[1];
540 }
541 
551 void vpGenericFeature::set_s(const double s0)
552 {
553 
554  if (1 != dim_s) {
555  vpERROR_TRACE("size mismatch between number of parameters"
556  "and feature dimension");
557  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
558  "and feature dimension"));
559  }
560  s[0] = s0;
561 }
562 
572 void vpGenericFeature::get_s(double &s0) const
573 {
574 
575  if (1 != dim_s) {
576  vpERROR_TRACE("size mismatch between number of parameters"
577  "and feature dimension");
578  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
579  "and feature dimension"));
580  }
581  s0 = s[0];
582 }
583 
605 void vpGenericFeature::print(const unsigned int select) const
606 {
607 
608  std::cout << "Generic Feature: ";
609  for (unsigned int i = 0; i < dim_s; i++)
610  if (FEATURE_LINE[i] & select) {
611  std::cout << " s[" << i << "]=" << s[i];
612  }
613 
614  std::cout << std::endl;
615 }
616 
618 {
619  vpGenericFeature *feature = new vpGenericFeature(dim_s);
620 
621  vpTRACE("dims = %d", dim_s);
622  return feature;
623 }
624 
629  const vpColor & /* color */, unsigned int /* thickness */) const
630 {
631  static int firsttime = 0;
632 
633  if (firsttime == 0) {
634  firsttime = 1;
635  vpERROR_TRACE("not implemented");
636  // Do not throw and error since it is not subject
637  // to produce a failure
638  }
639 }
643 void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
644  const vpColor & /* color */, unsigned int /* thickness */) const
645 {
646  static int firsttime = 0;
647 
648  if (firsttime == 0) {
649  firsttime = 1;
650  vpERROR_TRACE("not implemented");
651  // Do not throw and error since it is not subject
652  // to produce a failure
653  }
654 }
655 
656 /*
657  * Local variables:
658  * c-basic-offset: 2
659  * End:
660  */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void set_s(const vpColVector &s)
set the value of all the features.
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:393
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:171
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4462
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:416
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
Definition: vpArray2D.h:156
static const unsigned int FEATURE_LINE[32]
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void stack(double d)
void print(const unsigned int select=FEATURE_ALL) const
vpGenericFeature * duplicate() const
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
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:244
void setError(const vpColVector &error_vector)