Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
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 
41 #include <visp3/visual_features/vpBasicFeature.h>
42 #include <visp3/visual_features/vpFeatureTranslation.h>
43 
44 #include <visp3/core/vpMath.h>
45 
46 // Exception
47 #include <visp3/core/vpException.h>
48 #include <visp3/visual_features/vpFeatureException.h>
49 
50 // Debug trace
51 #include <visp3/core/vpDebug.h>
52 
53 /*
54 
55 attributes and members directly related to the vpBasicFeature needs
56 other functionalities are useful but not mandatory
57 
58 */
59 
60 BEGIN_VISP_NAMESPACE
67 {
68  // feature dimension
69  dim_s = 3;
70  nbParameters = 1;
71 
72  // memory allocation
73  s.resize(dim_s);
74  if (flags == nullptr)
75  flags = new bool[nbParameters];
76  for (unsigned int i = 0; i < nbParameters; i++)
77  flags[i] = false;
78 }
79 
87 
96 {
97  init();
98 }
99 
113  : f2Mf1(), translation(r)
114 {
115  init();
116 
117  buildFrom(f2Mf1_);
118 }
119 
130 {
131  this->f2Mf1 = f2Mf1_;
132  s[0] = f2Mf1[0][3];
133  s[1] = f2Mf1[1][3];
134  s[2] = f2Mf1[2][3];
135 
136  flags[0] = true;
137  return *this;
138 }
139 
150 {
151  translation = r;
152 }
153 
163 void vpFeatureTranslation::set_Tx(double t_x) { s[0] = t_x; }
173 void vpFeatureTranslation::set_Ty(double t_y) { s[1] = t_y; }
183 void vpFeatureTranslation::set_Tz(double t_z) { s[2] = t_z; }
184 
195 {
196  return translation;
197 }
198 
204 double vpFeatureTranslation::get_Tx() const { return s[0]; }
205 
211 double vpFeatureTranslation::get_Ty() const { return s[1]; }
212 
218 double vpFeatureTranslation::get_Tz() const { return s[2]; }
219 
302 {
303 
304  vpMatrix L;
305  L.resize(0, 6);
306 
308  for (unsigned int i = 0; i < nbParameters; i++) {
309  if (flags[i] == false) {
310  switch (i) {
311  case 0:
312  vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 "
313  "was not set yet");
314  break;
315  default:
316  vpTRACE("Problem during the reading of the variable flags");
317  }
318  }
319  }
320  resetFlags();
321  }
322 
323  if (translation == cdMc) {
324  // This version is a simplification
325  if (vpFeatureTranslation::selectTx() & select) {
326  vpMatrix Lx(1, 6);
327 
328  for (int i = 0; i < 3; i++)
329  Lx[0][i] = f2Mf1[0][i];
330  Lx[0][3] = 0;
331  Lx[0][4] = 0;
332  Lx[0][5] = 0;
333 
334  L = vpMatrix::stack(L, Lx);
335  }
336 
337  if (vpFeatureTranslation::selectTy() & select) {
338  vpMatrix Ly(1, 6);
339 
340  for (int i = 0; i < 3; i++)
341  Ly[0][i] = f2Mf1[1][i];
342  Ly[0][3] = 0;
343  Ly[0][4] = 0;
344  Ly[0][5] = 0;
345 
346  L = vpMatrix::stack(L, Ly);
347  }
348 
349  if (vpFeatureTranslation::selectTz() & select) {
350  vpMatrix Lz(1, 6);
351 
352  for (int i = 0; i < 3; i++)
353  Lz[0][i] = f2Mf1[2][i];
354  Lz[0][3] = 0;
355  Lz[0][4] = 0;
356  Lz[0][5] = 0;
357 
358  L = vpMatrix::stack(L, Lz);
359  }
360  }
361  if (translation == cMcd) {
362  // This version is a simplification
363  if (vpFeatureTranslation::selectTx() & select) {
364  vpMatrix Lx(1, 6);
365  Lx[0][0] = -1;
366  Lx[0][1] = 0;
367  Lx[0][2] = 0;
368  Lx[0][3] = 0;
369  Lx[0][4] = -s[2];
370  Lx[0][5] = s[1];
371 
372  L = vpMatrix::stack(L, Lx);
373  }
374 
375  if (vpFeatureTranslation::selectTy() & select) {
376  vpMatrix Ly(1, 6);
377  Ly[0][0] = 0;
378  Ly[0][1] = -1;
379  Ly[0][2] = 0;
380  Ly[0][3] = s[2];
381  Ly[0][4] = 0;
382  Ly[0][5] = -s[0];
383 
384  L = vpMatrix::stack(L, Ly);
385  }
386 
387  if (vpFeatureTranslation::selectTz() & select) {
388  vpMatrix Lz(1, 6);
389  Lz[0][0] = 0;
390  Lz[0][1] = 0;
391  Lz[0][2] = -1;
392  Lz[0][3] = -s[1];
393  Lz[0][4] = s[0];
394  Lz[0][5] = 0;
395 
396  L = vpMatrix::stack(L, Lz);
397  }
398  }
399 
400  if (translation == cMo) {
401  // This version is a simplification
402  if (vpFeatureTranslation::selectTx() & select) {
403  vpMatrix Lx(1, 6);
404  Lx[0][0] = -1;
405  Lx[0][1] = 0;
406  Lx[0][2] = 0;
407  Lx[0][3] = 0;
408  Lx[0][4] = -s[2];
409  Lx[0][5] = s[1];
410 
411  L = vpMatrix::stack(L, Lx);
412  }
413 
414  if (vpFeatureTranslation::selectTy() & select) {
415  vpMatrix Ly(1, 6);
416  Ly[0][0] = 0;
417  Ly[0][1] = -1;
418  Ly[0][2] = 0;
419  Ly[0][3] = s[2];
420  Ly[0][4] = 0;
421  Ly[0][5] = -s[0];
422 
423  L = vpMatrix::stack(L, Ly);
424  }
425 
426  if (vpFeatureTranslation::selectTz() & select) {
427  vpMatrix Lz(1, 6);
428  Lz[0][0] = 0;
429  Lz[0][1] = 0;
430  Lz[0][2] = -1;
431  Lz[0][3] = -s[1];
432  Lz[0][4] = s[0];
433  Lz[0][5] = 0;
434 
435  L = vpMatrix::stack(L, Lz);
436  }
437  }
438 
439  return L;
440 }
441 
508 vpColVector vpFeatureTranslation::error(const vpBasicFeature &s_star, unsigned int select)
509 {
510  vpColVector e(0);
511 
512  if (translation == cdMc || translation == cMcd) {
513  if (s_star.get_s().sumSquare() > 1e-6) {
514  vpERROR_TRACE("s* should be zero ! ");
515  throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
516  }
517  }
518 
519  if (vpFeatureTranslation::selectTx() & select) {
520  vpColVector ex(1);
521  ex[0] = s[0] - s_star[0];
522  e = vpColVector::stack(e, ex);
523  }
524 
525  if (vpFeatureTranslation::selectTy() & select) {
526  vpColVector ey(1);
527  ey[0] = s[1] - s_star[1];
528  e = vpColVector::stack(e, ey);
529  }
530 
531  if (vpFeatureTranslation::selectTz() & select) {
532  vpColVector ez(1);
533  ez[0] = s[2] - s_star[2];
534  e = vpColVector::stack(e, ez);
535  }
536 
537  return e;
538 }
539 
563 void vpFeatureTranslation::print(unsigned int select) const
564 {
565  std::cout << "Translation 3D:";
566  if (vpFeatureTranslation::selectTx() & select) {
567  std::cout << " tx=" << s[0];
568  }
569  if (vpFeatureTranslation::selectTy() & select) {
570  std::cout << " ty=" << s[1];
571  }
572  if (vpFeatureTranslation::selectTz() & select) {
573  std::cout << " tz=" << s[2];
574  }
575  std::cout << std::endl;
576 }
577 
592 {
593  vpFeatureTranslation *feature = nullptr;
594  if (translation == cdMc)
595  feature = new vpFeatureTranslation(cdMc);
596  if (translation == cMo)
597  feature = new vpFeatureTranslation(cMo);
598  if (translation == cMcd)
599  feature = new vpFeatureTranslation(cMcd);
600  return feature;
601 }
602 
609  const vpColor & /* color */, unsigned int /* thickness */) const
610 {
611  static int firsttime = 0;
612 
613  if (firsttime == 0) {
614  firsttime = 1;
615  vpERROR_TRACE("not implemented");
616  // Do not throw and error since it is not subject
617  // to produce a failure
618  }
619 }
625 void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
626  const vpColor & /* color */, unsigned int /* thickness */) const
627 {
628  static int firsttime = 0;
629 
630  if (firsttime == 0) {
631  firsttime = 1;
632  vpERROR_TRACE("not implemented");
633  // Do not throw and error since it is not subject
634  // to produce a failure
635  }
636 }
637 
682 unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; }
683 
727 unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1]; }
728 
772 unsigned int vpFeatureTranslation::selectTz() { return FEATURE_LINE[2]; }
773 END_VISP_NAMESPACE
class that defines what is a visual feature
vpColVector s
State of the visual feature.
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.
static const unsigned int FEATURE_LINE[32]
vpBasicFeatureDeallocatorType deallocate
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
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 setFeatureTranslationType(const vpFeatureTranslationRepresentationType r)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) VP_OVERRIDE
static unsigned int selectTx()
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
vpFeatureTranslationRepresentationType getFeatureTranslationType() const
vpMatrix interaction(unsigned int select=FEATURE_ALL) VP_OVERRIDE
vpFeatureTranslation & buildFrom(const vpHomogeneousMatrix &f2Mf1)
vpFeatureTranslation * duplicate() const VP_OVERRIDE
Feature duplication.
static unsigned int selectTy()
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
void stack(const vpMatrix &A)