Visual Servoing Platform version 3.5.0
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
57
68vpGenericFeature::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
87vpGenericFeature::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
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
169vpColVector 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
451void 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
479void 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
504void 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
528void 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
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
571void 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
604void 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{
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}
642void 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 */
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
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:131
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
Error that can be emited by the vpBasicFeature class and its derivates.
@ badErrorVectorError
feature list or desired feature list is empty
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:154
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
#define vpTRACE
Definition: vpDebug.h:416
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393