Visual Servoing Platform  version 3.6.1 under development (2023-10-03)
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 
52 
53 void vpGenericFeature::init() { s = 0; }
54 
65 vpGenericFeature::vpGenericFeature() : L(), err(), errorStatus(errorNotInitalized)
66 {
67  /*
68  vpERROR_TRACE("You are not allow to use this constructor ") ;
69  vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
70  "constructor") ;
71  vpERROR_TRACE("And provide the dimension of the visual feature ") ;
72  throw(vpException(vpException::cannotUseConstructorError,
73  "You are not allow to use this constructor ")) ;
74  */
75 }
76 
84 vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s) : L(), err(), errorStatus(errorNotInitalized)
85 {
86  this->dim_s = dimension_gen_s;
87  s.resize(dimension_gen_s);
88 }
89 
99 void vpGenericFeature::setError(const vpColVector &error_vector)
100 {
101  if (error_vector.getRows() != dim_s) {
102  vpERROR_TRACE("size mismatch between error dimension"
103  "and feature dimension");
104  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between error dimension"
105  "and feature dimension"));
106  }
107  errorStatus = errorInitialized;
108  err = error_vector;
109 }
110 
166 vpColVector vpGenericFeature::error(const vpBasicFeature &s_star, unsigned int select)
167 {
168  if (s_star.get_s().getRows() != dim_s) {
169  vpERROR_TRACE("size mismatch between s* dimension "
170  "and feature dimension");
171  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s* dimension "
172  "and feature dimension"));
173  }
174 
175  vpColVector e(0);
176 
177  try {
178  if (errorStatus == errorHasToBeUpdated) {
179  vpERROR_TRACE("Error has no been updated since last iteration"
180  "you should have used vpGenericFeature::setError"
181  "in you visual servoing loop");
183  "Error has no been updated since last iteration"));
184  } else if (errorStatus == errorInitialized) {
185  vpDEBUG_TRACE(25, "Error init: e=e.");
186  errorStatus = errorHasToBeUpdated;
187  for (unsigned int i = 0; i < dim_s; i++)
188  if (FEATURE_LINE[i] & select) {
189  vpColVector ex(1);
190  ex[i] = err[i];
191 
192  e = vpColVector::stack(e, ex);
193  }
194  } else {
195  vpDEBUG_TRACE(25, "Error not init: e=s-s*.");
196 
197  for (unsigned int i = 0; i < dim_s; i++)
198  if (FEATURE_LINE[i] & select) {
199  vpColVector ex(1);
200  ex[0] = s[i] - s_star[i];
201 
202  e = vpColVector::stack(e, ex);
203  }
204  }
205  } catch (...) {
206  throw;
207  }
208  return e;
209 }
210 
252 {
253  vpColVector e(0);
254 
255  try {
256  if (errorStatus == errorHasToBeUpdated) {
257  vpERROR_TRACE("Error has no been updated since last iteration"
258  "you should have used vpGenericFeature::setError"
259  "in you visual servoing loop");
261  "Error has no been updated since last iteration"));
262  } else if (errorStatus == errorInitialized) {
263  errorStatus = errorHasToBeUpdated;
264  for (unsigned int i = 0; i < dim_s; i++)
265  if (FEATURE_LINE[i] & select) {
266  vpColVector ex(1);
267  ex[i] = err[i];
268 
269  e = vpColVector::stack(e, ex);
270  }
271  } else {
272 
273  for (unsigned int i = 0; i < dim_s; i++)
274  if (FEATURE_LINE[i] & select) {
275  vpColVector ex(1);
276  ex[i] = s[i];
277 
278  e = vpColVector::stack(e, ex);
279  }
280  }
281  } catch (...) {
282  throw;
283  }
284 
285  return e;
286 }
287 
339 {
340  if (L.getRows() == 0) {
341  std::cout << "interaction matrix " << L << std::endl;
342  vpERROR_TRACE("Interaction has not been initialized");
343  std::cout << "A possible reason (may be) is that you have set" << std::endl;
344  std::cout << "the interaction matrix for s and compute a control " << std::endl;
345  std::cout << "with Ls=s* (default) or vice versa" << std::endl;
346 
347  throw(vpFeatureException(vpFeatureException::notInitializedError, "size mismatch between s* dimension "
348  "and feature dimension"));
349  }
350 
351  vpMatrix Ls;
352 
353  Ls.resize(0, 6);
354 
355  for (unsigned int i = 0; i < dim_s; i++)
356  if (FEATURE_LINE[i] & select) {
357  vpMatrix Lx(1, 6);
358  Lx = 0;
359 
360  for (int j = 0; j < 6; j++)
361  Lx[0][j] = L[i][j];
362 
363  Ls = vpMatrix::stack(Ls, Lx);
364  }
365 
366  return Ls;
367 }
368 
379 {
380  if (L_.getRows() != dim_s) {
381  std::cout << L_.getRows() << " " << dim_s << std::endl;
382  vpERROR_TRACE("size mismatch between interaction matrix size "
383  "and feature dimension");
384  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between interaction matrix size "
385  "and feature dimension"));
386  }
387 
388  this->L = L_;
389 }
390 
402 {
403 
404  if (s_vector.getRows() != dim_s) {
405  vpERROR_TRACE("size mismatch between s dimension"
406  "and feature dimension");
407  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
408  "and feature dimension"));
409  }
410  this->s = s_vector;
411 }
412 
424 {
425  if (s_vector.getRows() != dim_s) {
426  vpERROR_TRACE("size mismatch between s dimension"
427  "and feature dimension");
428  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
429  "and feature dimension"));
430  }
431  s_vector = this->s;
432 }
433 
448 void vpGenericFeature::set_s(double s0, double s1, double s2)
449 {
450 
451  if (3 != dim_s) {
452  vpERROR_TRACE("size mismatch between number of parameters"
453  "and feature dimension");
454  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
455  "and feature dimension"));
456  }
457  s[0] = s0;
458  s[1] = s1;
459  s[2] = s2;
460 }
461 
476 void vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
477 {
478 
479  if (3 != dim_s) {
480  vpERROR_TRACE("size mismatch between number of parameters"
481  "and feature dimension");
482  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
483  "and feature dimension"));
484  }
485  s0 = s[0];
486  s1 = s[1];
487  s2 = s[2];
488 }
489 
501 void vpGenericFeature::set_s(double s0, double s1)
502 {
503 
504  if (2 != dim_s) {
505  vpERROR_TRACE("size mismatch between number of parameters"
506  "and feature dimension");
507  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
508  "and feature dimension"));
509  }
510  s[0] = s0;
511  s[1] = s1;
512 }
513 
525 void vpGenericFeature::get_s(double &s0, double &s1) const
526 {
527 
528  if (2 != dim_s) {
529  vpERROR_TRACE("size mismatch between number of parameters"
530  "and feature dimension");
531  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
532  "and feature dimension"));
533  }
534  s0 = s[0];
535  s1 = s[1];
536 }
537 
547 void vpGenericFeature::set_s(double s0)
548 {
549 
550  if (1 != dim_s) {
551  vpERROR_TRACE("size mismatch between number of parameters"
552  "and feature dimension");
553  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
554  "and feature dimension"));
555  }
556  s[0] = s0;
557 }
558 
568 void vpGenericFeature::get_s(double &s0) const
569 {
570 
571  if (1 != dim_s) {
572  vpERROR_TRACE("size mismatch between number of parameters"
573  "and feature dimension");
574  throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
575  "and feature dimension"));
576  }
577  s0 = s[0];
578 }
579 
601 void vpGenericFeature::print(unsigned int select) const
602 {
603 
604  std::cout << "Generic Feature: ";
605  for (unsigned int i = 0; i < dim_s; i++)
606  if (FEATURE_LINE[i] & select) {
607  std::cout << " s[" << i << "]=" << s[i];
608  }
609 
610  std::cout << std::endl;
611 }
612 
614 {
615  vpGenericFeature *feature = new vpGenericFeature(dim_s);
616 
617  vpTRACE("dims = %d", dim_s);
618  return feature;
619 }
620 
625  const vpColor & /* color */, unsigned int /* thickness */) const
626 {
627  static int firsttime = 0;
628 
629  if (firsttime == 0) {
630  firsttime = 1;
631  vpERROR_TRACE("not implemented");
632  // Do not throw and error since it is not subject
633  // to produce a failure
634  }
635 }
639 void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
640  const vpColor & /* color */, unsigned int /* thickness */) const
641 {
642  static int firsttime = 0;
643 
644  if (firsttime == 0) {
645  firsttime = 1;
646  vpERROR_TRACE("not implemented");
647  // Do not throw and error since it is not subject
648  // to produce a failure
649  }
650 }
651 
652 /*
653  * Local variables:
654  * c-basic-offset: 2
655  * End:
656  */
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
unsigned int getRows() const
Definition: vpArray2D.h:290
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:167
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1094
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 setInteractionMatrix(const vpMatrix &L)
set the value of the interaction matrix.
void print(unsigned int select=FEATURE_ALL) const
virtual ~vpGenericFeature()
vpMatrix interaction(unsigned int select=FEATURE_ALL)
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void get_s(vpColVector &s) const
get the value of all the features.
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void set_s(const vpColVector &s)
set the value of all the features.
vpGenericFeature * duplicate() const
void setError(const vpColVector &error_vector)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5884
#define vpTRACE
Definition: vpDebug.h:411
#define vpDEBUG_TRACE
Definition: vpDebug.h:482
#define vpERROR_TRACE
Definition: vpDebug.h:388