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