Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpFeatureTranslation.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * 3D translation visual feature.
32  *
33  * Authors:
34  * Eric Marchand
35  * Fabien Spindler
36  *
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 
52 
57 /*
58 
59 attributes and members directly related to the vpBasicFeature needs
60 other functionalities are useful but not mandatory
61 
62 */
63 
69 void
71 {
72  //feature dimension
73  dim_s = 3 ;
74  nbParameters = 1;
75 
76  // memory allocation
77  s.resize(dim_s) ;
78  if (flags == NULL)
79  flags = new bool[nbParameters];
80  for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
81 
82 }
83 
91  : f2Mf1(), translation(vpFeatureTranslation::cdMc)
92 {
93  init() ;
94 }
95 
96 
104  : f2Mf1(), translation(r)
105 {
106  init() ;
107 }
108 
109 
121  : f2Mf1(), translation(r)
122 {
123  init() ;
124 
125  buildFrom(f2Mf1_) ;
126 }
127 
135 void
137 {
138  this->f2Mf1 = f2Mf1_ ;
139  s[0] = f2Mf1[0][3] ;
140  s[1] = f2Mf1[1][3] ;
141  s[2] = f2Mf1[2][3] ;
142 
143  flags[0] = true;
144 }
145 
156 {
157  translation = r;
158 }
159 
169 void vpFeatureTranslation::set_Tx(const double t_x)
170 {
171  s[0] = t_x ;
172 }
182 void vpFeatureTranslation::set_Ty(const double t_y)
183 {
184  s[1] = t_y ;
185 }
195 void
197 {
198  s[2] = t_z ;
199 }
200 
201 
212 {
213  return translation ;
214 }
215 
222 {
223  return s[0] ;
224 }
225 
226 
233 {
234  return s[1] ;
235 }
236 
237 
243 double
245 {
246  return s[2] ;
247 }
248 
249 
331 vpMatrix
332 vpFeatureTranslation::interaction(const unsigned int select)
333 {
334 
335  vpMatrix L ;
336  L.resize(0,6) ;
337 
339  for (unsigned int i = 0; i < nbParameters; i++) {
340  if (flags[i] == false) {
341  switch(i){
342  case 0:
343  vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 was not set yet");
344  break;
345  default:
346  vpTRACE("Problem during the reading of the variable flags");
347  }
348  }
349  }
350  resetFlags();
351  }
352 
353  if (translation == cdMc) {
354  //This version is a simplification
355  if (vpFeatureTranslation::selectTx() & select ) {
356  vpMatrix Lx(1,6) ;
357 
358  for (int i=0 ; i < 3 ; i++)
359  Lx[0][i] = f2Mf1[0][i] ;
360  Lx[0][3] = 0 ; Lx[0][4] = 0 ; Lx[0][5] = 0 ;
361 
362  L = vpMatrix::stack(L,Lx) ;
363  }
364 
365  if (vpFeatureTranslation::selectTy() & select ) {
366  vpMatrix Ly(1,6) ;
367 
368  for (int i=0 ; i < 3 ; i++)
369  Ly[0][i] = f2Mf1[1][i] ;
370  Ly[0][3] = 0 ; Ly[0][4] = 0 ; Ly[0][5] = 0 ;
371 
372  L = vpMatrix::stack(L,Ly) ;
373  }
374 
375  if (vpFeatureTranslation::selectTz() & select ) {
376  vpMatrix Lz(1,6) ;
377 
378  for (int i=0 ; i < 3 ; i++)
379  Lz[0][i] = f2Mf1[2][i] ;
380  Lz[0][3] = 0 ; Lz[0][4] = 0 ; Lz[0][5] = 0 ;
381 
382  L = vpMatrix::stack(L,Lz) ;
383  }
384  }
385  if (translation == cMcd) {
386  //This version is a simplification
387  if (vpFeatureTranslation::selectTx() & select ) {
388  vpMatrix Lx(1,6) ;
389  Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
390  Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
391 
392  L = vpMatrix::stack(L,Lx) ;
393  }
394 
395  if (vpFeatureTranslation::selectTy() & select ) {
396  vpMatrix Ly(1,6) ;
397  Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
398  Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
399 
400  L = vpMatrix::stack(L,Ly) ;
401  }
402 
403  if (vpFeatureTranslation::selectTz() & select ) {
404  vpMatrix Lz(1,6) ;
405  Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
406  Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
407 
408  L = vpMatrix::stack(L,Lz) ;
409  }
410  }
411 
412  if (translation == cMo) {
413  //This version is a simplification
414  if (vpFeatureTranslation::selectTx() & select ) {
415  vpMatrix Lx(1,6) ;
416  Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
417  Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
418 
419  L = vpMatrix::stack(L,Lx) ;
420  }
421 
422  if (vpFeatureTranslation::selectTy() & select ) {
423  vpMatrix Ly(1,6) ;
424  Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
425  Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
426 
427  L = vpMatrix::stack(L,Ly) ;
428  }
429 
430  if (vpFeatureTranslation::selectTz() & select ) {
431  vpMatrix Lz(1,6) ;
432  Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
433  Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
434 
435  L = vpMatrix::stack(L,Lz) ;
436  }
437  }
438 
439  return L ;
440 }
441 
505  const unsigned int select)
506 {
507  vpColVector e(0) ;
508 
509  if(translation == cdMc || translation == cMcd)
510  {
511  if (s_star.get_s().sumSquare() > 1e-6)
512  {
513  vpERROR_TRACE("s* should be zero ! ") ;
515  "s* should be zero !")) ;
516  }
517  }
518 
519 
520  if (vpFeatureTranslation::selectTx() & select )
521  {
522  vpColVector ex(1) ;
523  ex[0] = s[0]-s_star[0] ;
524  e = vpColVector::stack(e,ex) ;
525  }
526 
527  if (vpFeatureTranslation::selectTy() & select )
528  {
529  vpColVector ey(1) ;
530  ey[0] = s[1]-s_star[1] ;
531  e = vpColVector::stack(e,ey) ;
532  }
533 
534  if (vpFeatureTranslation::selectTz() & select )
535  {
536  vpColVector ez(1) ;
537  ez[0] = s[2]-s_star[2] ;
538  e = vpColVector::stack(e,ez) ;
539  }
540 
541  return e ;
542 }
543 
566 void
567 vpFeatureTranslation::print(const unsigned int select) const
568 {
569  std::cout <<"Translation 3D: ";
570  if (vpFeatureTranslation::selectTx() & select ) {
571  std::cout << s[0] << " ";
572  }
573  if (vpFeatureTranslation::selectTy() & select ) {
574  std::cout << s[1] << " ";
575  }
576  if (vpFeatureTranslation::selectTz() & select ) {
577  std::cout << s[2] << " ";
578  }
579  std::cout << std::endl;
580 }
581 
582 
595 {
596  vpFeatureTranslation *feature = NULL;
597  if (translation == cdMc)
598  feature = new vpFeatureTranslation(cdMc) ;
599  if (translation == cMo)
600  feature = new vpFeatureTranslation(cMo) ;
601  if (translation == cMcd)
602  feature = new vpFeatureTranslation(cMcd) ;
603  return feature ;
604 }
605 
606 
612 void
614  const vpImage<unsigned char> &/* I */,
615  const vpColor &/* color */,
616  unsigned int /* thickness */) const
617 {
618  static int firsttime =0 ;
619 
620  if (firsttime==0)
621  {
622  firsttime=1 ;
623  vpERROR_TRACE("not implemented") ;
624  // Do not throw and error since it is not subject
625  // to produce a failure
626  }
627 }
633 void
635  const vpImage<vpRGBa> &/* I */,
636  const vpColor &/* color */,
637  unsigned int /* thickness */) const
638 {
639  static int firsttime =0 ;
640 
641  if (firsttime==0)
642  {
643  firsttime=1 ;
644  vpERROR_TRACE("not implemented") ;
645  // Do not throw and error since it is not subject
646  // to produce a failure
647  }
648 }
649 
694 unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0] ; }
695 
739 unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1] ; }
740 
784 unsigned int vpFeatureTranslation::selectTz() { return FEATURE_LINE[2] ; }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
Class that defines the translation visual feature .
bool * flags
Ensure that all the parameters needed to compute the iteraction matrix are set.
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
static unsigned int selectTx()
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class to define colors available for display functionnalities.
Definition: vpColor.h:121
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
unsigned int dim_s
Dimension of the visual feature.
void display(const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1) const
void set_Ty(const double t_y)
void set_Tx(const double t_x)
static unsigned int selectTy()
vpFeatureTranslationRepresentationType getFeatureTranslationType() const
vpFeatureTranslation * duplicate() const
Feature duplication.
class that defines what is a visual feature
#define vpTRACE
Definition: vpDebug.h:414
void setFeatureTranslationType(const vpFeatureTranslationRepresentationType r)
Error that can be emited by the vpBasicFeature class and its derivates.
Generic class defining intrinsic camera parameters.
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
static const unsigned int FEATURE_LINE[32]
double sumSquare() const
static unsigned int selectTz()
vpBasicFeatureDeallocatorType deallocate
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
void set_Tz(const double t_z)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void buildFrom(const vpHomogeneousMatrix &f2Mf1)
vpColVector get_s(unsigned int select=FEATURE_ALL) const
Get the feature vector .
unsigned int nbParameters
Number of parameters needed to compute the interaction matrix.
vpColVector s
State of the visual feature.
void print(const unsigned int select=FEATURE_ALL) const
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225