Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
54
65vpGenericFeature::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
84vpGenericFeature::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
99void 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
166vpColVector 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
448void 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
476void 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
501void 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
525void 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
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
568void 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
601void 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{
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}
639void 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.
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
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
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)
Definition of the vpImage class member functions.
Definition vpImage.h:135
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void stack(const vpMatrix &A)
#define vpTRACE
Definition vpDebug.h:411
#define vpDEBUG_TRACE
Definition vpDebug.h:482
#define vpERROR_TRACE
Definition vpDebug.h:388