Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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, 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 
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 
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  vpERROR_TRACE("size mismatch between interaction matrix size "
386  "and feature dimension");
387  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between interaction matrix size "
388  "and feature dimension"));
389  }
390 
391  this->L = L_;
392 }
393 
405 {
406 
407  if (s_vector.getRows() != dim_s) {
408  vpERROR_TRACE("size mismatch between s dimension"
409  "and feature dimension");
410  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
411  "and feature dimension"));
412  }
413  this->s = s_vector;
414 }
415 
427 {
428  if (s_vector.getRows() != dim_s) {
429  vpERROR_TRACE("size mismatch between s dimension"
430  "and feature dimension");
431  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
432  "and feature dimension"));
433  }
434  s_vector = this->s;
435 }
436 
451 void vpGenericFeature::set_s(double s0, double s1, double s2)
452 {
453 
454  if (3 != dim_s) {
455  vpERROR_TRACE("size mismatch between number of parameters"
456  "and feature dimension");
457  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
458  "and feature dimension"));
459  }
460  s[0] = s0;
461  s[1] = s1;
462  s[2] = s2;
463 }
464 
479 void vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
480 {
481 
482  if (3 != dim_s) {
483  vpERROR_TRACE("size mismatch between number of parameters"
484  "and feature dimension");
485  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
486  "and feature dimension"));
487  }
488  s0 = s[0];
489  s1 = s[1];
490  s2 = s[2];
491 }
492 
504 void vpGenericFeature::set_s(double s0, double s1)
505 {
506 
507  if (2 != dim_s) {
508  vpERROR_TRACE("size mismatch between number of parameters"
509  "and feature dimension");
510  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
511  "and feature dimension"));
512  }
513  s[0] = s0;
514  s[1] = s1;
515 }
516 
528 void vpGenericFeature::get_s(double &s0, double &s1) const
529 {
530 
531  if (2 != dim_s) {
532  vpERROR_TRACE("size mismatch between number of parameters"
533  "and feature dimension");
534  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
535  "and feature dimension"));
536  }
537  s0 = s[0];
538  s1 = s[1];
539 }
540 
550 void vpGenericFeature::set_s(double s0)
551 {
552 
553  if (1 != dim_s) {
554  vpERROR_TRACE("size mismatch between number of parameters"
555  "and feature dimension");
556  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
557  "and feature dimension"));
558  }
559  s[0] = s0;
560 }
561 
571 void vpGenericFeature::get_s(double &s0) const
572 {
573 
574  if (1 != dim_s) {
575  vpERROR_TRACE("size mismatch between number of parameters"
576  "and feature dimension");
577  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
578  "and feature dimension"));
579  }
580  s0 = s[0];
581 }
582 
604 void vpGenericFeature::print(unsigned int select) const
605 {
606 
607  std::cout << "Generic Feature: ";
608  for (unsigned int i = 0; i < dim_s; i++)
609  if (FEATURE_LINE[i] & select) {
610  std::cout << " s[" << i << "]=" << s[i];
611  }
612 
613  std::cout << std::endl;
614 }
615 
617 {
618  vpGenericFeature *feature = new vpGenericFeature(dim_s);
619 
620  vpTRACE("dims = %d", dim_s);
621  return feature;
622 }
623 
628  const vpColor & /* color */, unsigned int /* thickness */) const
629 {
630  static int firsttime = 0;
631 
632  if (firsttime == 0) {
633  firsttime = 1;
634  vpERROR_TRACE("not implemented");
635  // Do not throw and error since it is not subject
636  // to produce a failure
637  }
638 }
642 void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
643  const vpColor & /* color */, unsigned int /* thickness */) const
644 {
645  static int firsttime = 0;
646 
647  if (firsttime == 0) {
648  firsttime = 1;
649  vpERROR_TRACE("not implemented");
650  // Do not throw and error since it is not subject
651  // to produce a failure
652  }
653 }
654 
655 /*
656  * Local variables:
657  * c-basic-offset: 2
658  * End:
659  */
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
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
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void get_s(vpColVector &s) const
get the value of all the features.
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
unsigned int dim_s
Dimension of the visual feature.
unsigned int getRows() const
Definition: vpArray2D.h:289
feature list or desired feature list is empty
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
void print(unsigned int select=FEATURE_ALL) const
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.
static const unsigned int FEATURE_LINE[32]
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
#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...
vpGenericFeature * duplicate() const
vpColVector s
State of the visual feature.
void setError(const vpColVector &error_vector)