Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
vpGenericFeature.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/visual_features/vpGenericFeature.h>
37 
38 // Exception
39 #include <visp3/core/vpException.h>
40 #include <visp3/visual_features/vpFeatureException.h>
41 
42 // Debug trace
43 #include <visp3/core/vpDebug.h>
44 
45 BEGIN_VISP_NAMESPACE
46 void vpGenericFeature::init() { s = 0; }
47 
58 vpGenericFeature::vpGenericFeature() : L(), err(), errorStatus(errorNotInitialized)
59 {
60  /*
61  vpERROR_TRACE("You are not allow to use this constructor ") ;
62  vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
63  "constructor") ;
64  vpERROR_TRACE("And provide the dimension of the visual feature ") ;
65  throw(vpException(vpException::cannotUseConstructorError,
66  "You are not allow to use this constructor ")) ;
67  */
68 }
69 
77 vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s) : L(), err(), errorStatus(errorNotInitialized)
78 {
79  this->dim_s = dimension_gen_s;
80  s.resize(dimension_gen_s);
81 }
82 
92 void vpGenericFeature::setError(const vpColVector &error_vector)
93 {
94  if (error_vector.getRows() != dim_s) {
95  vpERROR_TRACE("size mismatch between error dimension"
96  "and feature dimension");
97  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between error dimension"
98  "and feature dimension"));
99  }
100  errorStatus = errorInitialized;
101  err = error_vector;
102 }
103 
159 vpColVector vpGenericFeature::error(const vpBasicFeature &s_star, unsigned int select)
160 {
161  if (s_star.get_s().getRows() != dim_s) {
162  vpERROR_TRACE("size mismatch between s* dimension "
163  "and feature dimension");
164  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s* dimension "
165  "and feature dimension"));
166  }
167 
168  vpColVector e(0);
169 
170  try {
171  if (errorStatus == errorHasToBeUpdated) {
172  vpERROR_TRACE("Error has no been updated since last iteration"
173  "you should have used vpGenericFeature::setError"
174  "in you visual servoing loop");
176  "Error has no been updated since last iteration"));
177  }
178  else if (errorStatus == errorInitialized) {
179  vpDEBUG_TRACE(25, "Error init: e=e.");
180  errorStatus = errorHasToBeUpdated;
181  for (unsigned int i = 0; i < dim_s; i++)
182  if (FEATURE_LINE[i] & select) {
183  vpColVector ex(1);
184  ex[i] = err[i];
185 
186  e = vpColVector::stack(e, ex);
187  }
188  }
189  else {
190  vpDEBUG_TRACE(25, "Error not init: e=s-s*.");
191 
192  for (unsigned int i = 0; i < dim_s; i++)
193  if (FEATURE_LINE[i] & select) {
194  vpColVector ex(1);
195  ex[0] = s[i] - s_star[i];
196 
197  e = vpColVector::stack(e, ex);
198  }
199  }
200  }
201  catch (...) {
202  throw;
203  }
204  return e;
205 }
206 
248 {
249  vpColVector e(0);
250 
251  try {
252  if (errorStatus == errorHasToBeUpdated) {
253  vpERROR_TRACE("Error has no been updated since last iteration"
254  "you should have used vpGenericFeature::setError"
255  "in you visual servoing loop");
257  "Error has no been updated since last iteration"));
258  }
259  else if (errorStatus == errorInitialized) {
260  errorStatus = errorHasToBeUpdated;
261  for (unsigned int i = 0; i < dim_s; i++)
262  if (FEATURE_LINE[i] & select) {
263  vpColVector ex(1);
264  ex[i] = err[i];
265 
266  e = vpColVector::stack(e, ex);
267  }
268  }
269  else {
270 
271  for (unsigned int i = 0; i < dim_s; i++)
272  if (FEATURE_LINE[i] & select) {
273  vpColVector ex(1);
274  ex[i] = s[i];
275 
276  e = vpColVector::stack(e, ex);
277  }
278  }
279  }
280  catch (...) {
281  throw;
282  }
283 
284  return e;
285 }
286 
338 {
339  if (L.getRows() == 0) {
340  std::cout << "interaction matrix " << L << std::endl;
341  vpERROR_TRACE("Interaction has not been initialized");
342  std::cout << "A possible reason (may be) is that you have set" << std::endl;
343  std::cout << "the interaction matrix for s and compute a control " << std::endl;
344  std::cout << "with Ls=s* (default) or vice versa" << std::endl;
345 
346  throw(vpFeatureException(vpFeatureException::notInitializedError, "size mismatch between s* dimension "
347  "and feature dimension"));
348  }
349 
350  vpMatrix Ls;
351 
352  Ls.resize(0, 6);
353 
354  for (unsigned int i = 0; i < dim_s; i++)
355  if (FEATURE_LINE[i] & select) {
356  vpMatrix Lx(1, 6);
357  Lx = 0;
358 
359  for (int j = 0; j < 6; j++)
360  Lx[0][j] = L[i][j];
361 
362  Ls = vpMatrix::stack(Ls, Lx);
363  }
364 
365  return Ls;
366 }
367 
378 {
379  if (L_.getRows() != dim_s) {
380  std::cout << L_.getRows() << " " << dim_s << std::endl;
381  vpERROR_TRACE("size mismatch between interaction matrix size "
382  "and feature dimension");
383  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between interaction matrix size "
384  "and feature dimension"));
385  }
386 
387  this->L = L_;
388 }
389 
401 {
402 
403  if (s_vector.getRows() != dim_s) {
404  vpERROR_TRACE("size mismatch between s dimension"
405  "and feature dimension");
406  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
407  "and feature dimension"));
408  }
409  this->s = s_vector;
410 }
411 
423 {
424  if (s_vector.getRows() != dim_s) {
425  vpERROR_TRACE("size mismatch between s dimension"
426  "and feature dimension");
427  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
428  "and feature dimension"));
429  }
430  s_vector = this->s;
431 }
432 
447 void vpGenericFeature::set_s(double s0, double s1, double s2)
448 {
449 
450  if (3 != dim_s) {
451  vpERROR_TRACE("size mismatch between number of parameters"
452  "and feature dimension");
453  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
454  "and feature dimension"));
455  }
456  s[0] = s0;
457  s[1] = s1;
458  s[2] = s2;
459 }
460 
475 void vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
476 {
477 
478  if (3 != dim_s) {
479  vpERROR_TRACE("size mismatch between number of parameters"
480  "and feature dimension");
481  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
482  "and feature dimension"));
483  }
484  s0 = s[0];
485  s1 = s[1];
486  s2 = s[2];
487 }
488 
500 void vpGenericFeature::set_s(double s0, double s1)
501 {
502 
503  if (2 != dim_s) {
504  vpERROR_TRACE("size mismatch between number of parameters"
505  "and feature dimension");
506  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
507  "and feature dimension"));
508  }
509  s[0] = s0;
510  s[1] = s1;
511 }
512 
524 void vpGenericFeature::get_s(double &s0, double &s1) const
525 {
526 
527  if (2 != dim_s) {
528  vpERROR_TRACE("size mismatch between number of parameters"
529  "and feature dimension");
530  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
531  "and feature dimension"));
532  }
533  s0 = s[0];
534  s1 = s[1];
535 }
536 
546 void vpGenericFeature::set_s(double s0)
547 {
548 
549  if (1 != dim_s) {
550  vpERROR_TRACE("size mismatch between number of parameters"
551  "and feature dimension");
552  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
553  "and feature dimension"));
554  }
555  s[0] = s0;
556 }
557 
567 void vpGenericFeature::get_s(double &s0) const
568 {
569 
570  if (1 != dim_s) {
571  vpERROR_TRACE("size mismatch between number of parameters"
572  "and feature dimension");
573  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
574  "and feature dimension"));
575  }
576  s0 = s[0];
577 }
578 
600 void vpGenericFeature::print(unsigned int select) const
601 {
602 
603  std::cout << "Generic Feature: ";
604  for (unsigned int i = 0; i < dim_s; i++)
605  if (FEATURE_LINE[i] & select) {
606  std::cout << " s[" << i << "]=" << s[i];
607  }
608 
609  std::cout << std::endl;
610 }
611 
613 {
614  vpGenericFeature *feature = new vpGenericFeature(dim_s);
615 
616  vpTRACE("dims = %d", dim_s);
617  return feature;
618 }
619 
624  const vpColor & /* color */, unsigned int /* thickness */) const
625 {
626  static int firsttime = 0;
627 
628  if (firsttime == 0) {
629  firsttime = 1;
630  vpERROR_TRACE("not implemented");
631  // Do not throw and error since it is not subject
632  // to produce a failure
633  }
634 }
638 void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
639  const vpColor & /* color */, unsigned int /* thickness */) const
640 {
641  static int firsttime = 0;
642 
643  if (firsttime == 0) {
644  firsttime = 1;
645  vpERROR_TRACE("not implemented");
646  // Do not throw and error since it is not subject
647  // to produce a failure
648  }
649 }
650 END_VISP_NAMESPACE
651 /*
652  * Local variables:
653  * c-basic-offset: 2
654  * End:
655  */
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
class that defines what is a visual feature
vpColVector s
State of the visual feature.
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int dim_s
Dimension of the visual feature.
static const unsigned int FEATURE_LINE[32]
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
Error that can be emitted by the vpBasicFeature class and its derivates.
@ notInitializedError
Feature not initialized.
@ badErrorVectorError
Feature list or desired feature list is empty.
@ sizeMismatchError
Size mismatch error.
Class that enables to define a feature or a set of features which are not implemented in ViSP as a sp...
vpGenericFeature * duplicate() const VP_OVERRIDE
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) VP_OVERRIDE
void setInteractionMatrix(const vpMatrix &L)
set the value of the interaction matrix.
void init() VP_OVERRIDE
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const VP_OVERRIDE
void get_s(vpColVector &s) const
get the value of all the features.
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
void print(unsigned int select=FEATURE_ALL) const VP_OVERRIDE
void set_s(const vpColVector &s)
set the value of all the features.
void setError(const vpColVector &error_vector)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void stack(const vpMatrix &A)