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