Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
vpFeatureTranslation.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  * 3D translation visual feature.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/visual_features/vpBasicFeature.h>
37 #include <visp3/visual_features/vpFeatureTranslation.h>
38 
39 #include <visp3/core/vpMath.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 
52 /*
53 
54 attributes and members directly related to the vpBasicFeature needs
55 other functionalities are useful but not mandatory
56 
57 */
58 
65 {
66  // feature dimension
67  dim_s = 3;
68  nbParameters = 1;
69 
70  // memory allocation
71  s.resize(dim_s);
72  if (flags == nullptr)
73  flags = new bool[nbParameters];
74  for (unsigned int i = 0; i < nbParameters; i++)
75  flags[i] = false;
76 }
77 
85 
94 {
95  init();
96 }
97 
111  : f2Mf1(), translation(r)
112 {
113  init();
114 
115  build(f2Mf1_);
116 }
117 
118 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
130 {
131  build(f2Mf1_);
132 }
133 #endif
134 
145 {
146  this->f2Mf1 = f2Mf1_;
147  s[0] = f2Mf1[0][3];
148  s[1] = f2Mf1[1][3];
149  s[2] = f2Mf1[2][3];
150 
151  flags[0] = true;
152  return *this;
153 }
154 
165 {
166  translation = r;
167 }
168 
178 void vpFeatureTranslation::set_Tx(double t_x) { s[0] = t_x; }
188 void vpFeatureTranslation::set_Ty(double t_y) { s[1] = t_y; }
198 void vpFeatureTranslation::set_Tz(double t_z) { s[2] = t_z; }
199 
210 {
211  return translation;
212 }
213 
219 double vpFeatureTranslation::get_Tx() const { return s[0]; }
220 
226 double vpFeatureTranslation::get_Ty() const { return s[1]; }
227 
233 double vpFeatureTranslation::get_Tz() const { return s[2]; }
234 
317 {
318 
319  vpMatrix L;
320  L.resize(0, 6);
321 
323  for (unsigned int i = 0; i < nbParameters; i++) {
324  if (flags[i] == false) {
325  switch (i) {
326  case 0:
327  vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 "
328  "was not set yet");
329  break;
330  default:
331  vpTRACE("Problem during the reading of the variable flags");
332  }
333  }
334  }
335  resetFlags();
336  }
337 
338  if (translation == cdMc) {
339  // This version is a simplification
340  if (vpFeatureTranslation::selectTx() & select) {
341  vpMatrix Lx(1, 6);
342 
343  for (int i = 0; i < 3; i++)
344  Lx[0][i] = f2Mf1[0][i];
345  Lx[0][3] = 0;
346  Lx[0][4] = 0;
347  Lx[0][5] = 0;
348 
349  L = vpMatrix::stack(L, Lx);
350  }
351 
352  if (vpFeatureTranslation::selectTy() & select) {
353  vpMatrix Ly(1, 6);
354 
355  for (int i = 0; i < 3; i++)
356  Ly[0][i] = f2Mf1[1][i];
357  Ly[0][3] = 0;
358  Ly[0][4] = 0;
359  Ly[0][5] = 0;
360 
361  L = vpMatrix::stack(L, Ly);
362  }
363 
364  if (vpFeatureTranslation::selectTz() & select) {
365  vpMatrix Lz(1, 6);
366 
367  for (int i = 0; i < 3; i++)
368  Lz[0][i] = f2Mf1[2][i];
369  Lz[0][3] = 0;
370  Lz[0][4] = 0;
371  Lz[0][5] = 0;
372 
373  L = vpMatrix::stack(L, Lz);
374  }
375  }
376  if (translation == cMcd) {
377  // This version is a simplification
378  if (vpFeatureTranslation::selectTx() & select) {
379  vpMatrix Lx(1, 6);
380  Lx[0][0] = -1;
381  Lx[0][1] = 0;
382  Lx[0][2] = 0;
383  Lx[0][3] = 0;
384  Lx[0][4] = -s[2];
385  Lx[0][5] = s[1];
386 
387  L = vpMatrix::stack(L, Lx);
388  }
389 
390  if (vpFeatureTranslation::selectTy() & select) {
391  vpMatrix Ly(1, 6);
392  Ly[0][0] = 0;
393  Ly[0][1] = -1;
394  Ly[0][2] = 0;
395  Ly[0][3] = s[2];
396  Ly[0][4] = 0;
397  Ly[0][5] = -s[0];
398 
399  L = vpMatrix::stack(L, Ly);
400  }
401 
402  if (vpFeatureTranslation::selectTz() & select) {
403  vpMatrix Lz(1, 6);
404  Lz[0][0] = 0;
405  Lz[0][1] = 0;
406  Lz[0][2] = -1;
407  Lz[0][3] = -s[1];
408  Lz[0][4] = s[0];
409  Lz[0][5] = 0;
410 
411  L = vpMatrix::stack(L, Lz);
412  }
413  }
414 
415  if (translation == cMo) {
416  // This version is a simplification
417  if (vpFeatureTranslation::selectTx() & select) {
418  vpMatrix Lx(1, 6);
419  Lx[0][0] = -1;
420  Lx[0][1] = 0;
421  Lx[0][2] = 0;
422  Lx[0][3] = 0;
423  Lx[0][4] = -s[2];
424  Lx[0][5] = s[1];
425 
426  L = vpMatrix::stack(L, Lx);
427  }
428 
429  if (vpFeatureTranslation::selectTy() & select) {
430  vpMatrix Ly(1, 6);
431  Ly[0][0] = 0;
432  Ly[0][1] = -1;
433  Ly[0][2] = 0;
434  Ly[0][3] = s[2];
435  Ly[0][4] = 0;
436  Ly[0][5] = -s[0];
437 
438  L = vpMatrix::stack(L, Ly);
439  }
440 
441  if (vpFeatureTranslation::selectTz() & select) {
442  vpMatrix Lz(1, 6);
443  Lz[0][0] = 0;
444  Lz[0][1] = 0;
445  Lz[0][2] = -1;
446  Lz[0][3] = -s[1];
447  Lz[0][4] = s[0];
448  Lz[0][5] = 0;
449 
450  L = vpMatrix::stack(L, Lz);
451  }
452  }
453 
454  return L;
455 }
456 
523 vpColVector vpFeatureTranslation::error(const vpBasicFeature &s_star, unsigned int select)
524 {
525  vpColVector e(0);
526 
527  if (translation == cdMc || translation == cMcd) {
528  if (s_star.get_s().sumSquare() > 1e-6) {
529  vpERROR_TRACE("s* should be zero ! ");
530  throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
531  }
532  }
533 
534  if (vpFeatureTranslation::selectTx() & select) {
535  vpColVector ex(1);
536  ex[0] = s[0] - s_star[0];
537  e = vpColVector::stack(e, ex);
538  }
539 
540  if (vpFeatureTranslation::selectTy() & select) {
541  vpColVector ey(1);
542  ey[0] = s[1] - s_star[1];
543  e = vpColVector::stack(e, ey);
544  }
545 
546  if (vpFeatureTranslation::selectTz() & select) {
547  vpColVector ez(1);
548  ez[0] = s[2] - s_star[2];
549  e = vpColVector::stack(e, ez);
550  }
551 
552  return e;
553 }
554 
578 void vpFeatureTranslation::print(unsigned int select) const
579 {
580  std::cout << "Translation 3D:";
581  if (vpFeatureTranslation::selectTx() & select) {
582  std::cout << " tx=" << s[0];
583  }
584  if (vpFeatureTranslation::selectTy() & select) {
585  std::cout << " ty=" << s[1];
586  }
587  if (vpFeatureTranslation::selectTz() & select) {
588  std::cout << " tz=" << s[2];
589  }
590  std::cout << std::endl;
591 }
592 
607 {
608  vpFeatureTranslation *feature = nullptr;
609  if (translation == cdMc)
610  feature = new vpFeatureTranslation(cdMc);
611  if (translation == cMo)
612  feature = new vpFeatureTranslation(cMo);
613  if (translation == cMcd)
614  feature = new vpFeatureTranslation(cMcd);
615  return feature;
616 }
617 
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 }
640 void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
641  const vpColor & /* color */, unsigned int /* thickness */) const
642 {
643  static int firsttime = 0;
644 
645  if (firsttime == 0) {
646  firsttime = 1;
647  vpERROR_TRACE("not implemented");
648  // Do not throw and error since it is not subject
649  // to produce a failure
650  }
651 }
652 
697 unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; }
698 
742 unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1]; }
743 
787 unsigned int vpFeatureTranslation::selectTz() { return FEATURE_LINE[2]; }
class that defines what is a visual feature
vpColVector s
State of the visual feature.
static const unsigned int FEATURE_LINE[32]
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int dim_s
Dimension of the visual feature.
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
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.
@ badInitializationError
Wrong feature initialization.
Class that defines the translation visual feature .
static unsigned int selectTz()
void buildFrom(const vpHomogeneousMatrix &f2Mf1)
void setFeatureTranslationType(const vpFeatureTranslationRepresentationType r)
static unsigned int selectTx()
vpFeatureTranslation * duplicate() const vp_override
Feature duplication.
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
vpFeatureTranslationRepresentationType getFeatureTranslationType() const
vpFeatureTranslation & build(const vpHomogeneousMatrix &f2Mf1)
static unsigned int selectTy()
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
void print(unsigned int select=FEATURE_ALL) const vp_override
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const vp_override
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5669
#define vpTRACE
Definition: vpDebug.h:407
#define vpERROR_TRACE
Definition: vpDebug.h:384