Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpPoseFeatures.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  * Pose computation from any features.
32  *
33  * Authors:
34  * Aurelien Yol
35  *
36  *****************************************************************************/
37 #include <visp3/vision/vpPoseFeatures.h>
38 
39 #ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
40 
45  : maxSize(0), totalSize(0), vvsIterMax(200), lambda(1.0), verbose(false), computeCovariance(false),
46  covarianceMatrix(), featurePoint_Point_list(), featurePoint3D_Point_list(), featureVanishingPoint_Point_list(),
47  featureVanishingPoint_DuoLine_list(), featureEllipse_Sphere_list(), featureEllipse_Circle_list(),
48  featureLine_Line_list(), featureLine_DuoLineInt_List(), featureSegment_DuoPoints_list()
49 {
50 }
51 
56 {
57  clear();
58 }
59 
64 {
65  for(int i = (int)featurePoint_Point_list.size()-1 ; i >= 0 ; i--)
66  delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
67  featurePoint_Point_list.clear();
68 
69  for(int i = (int)featurePoint3D_Point_list.size()-1 ; i >= 0 ; i--)
70  delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
71  featurePoint3D_Point_list.clear();
72 
73  for(int i = (int)featureVanishingPoint_Point_list.size()-1 ; i >= 0 ; i--)
74  delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
75  featureVanishingPoint_Point_list.clear();
76 
77  for(int i = (int)featureVanishingPoint_DuoLine_list.size()-1 ; i >= 0 ; i--)
78  delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
79  featureVanishingPoint_DuoLine_list.clear();
80 
81  for(int i = (int)featureEllipse_Sphere_list.size()-1 ; i >= 0 ; i--)
82  delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
83  featureEllipse_Sphere_list.clear();
84 
85  for(int i = (int)featureEllipse_Circle_list.size()-1 ; i >= 0 ; i--)
86  delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
87  featureEllipse_Circle_list.clear();
88 
89  for(int i = (int)featureLine_Line_list.size()-1 ; i >= 0 ; i--)
90  delete featureLine_Line_list[(unsigned int)i].desiredFeature;
91  featureLine_Line_list.clear();
92 
93  for(int i = (int)featureLine_DuoLineInt_List.size()-1 ; i >= 0 ; i--)
94  delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
95  featureLine_DuoLineInt_List.clear();
96 
97  for(int i = (int)featureSegment_DuoPoints_list.size()-1 ; i >= 0 ; i--)
98  delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
99  featureSegment_DuoPoints_list.clear();
100 
101 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
102  for(int i = (int)featureSpecific_list.size()-1 ; i >= 0 ; i--)
103  delete featureSpecific_list[(unsigned int)i];
104  featureSpecific_list.clear();
105 #endif
106 
107  maxSize = 0;
108  totalSize = 0;
109 }
110 
117 {
118  featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint,vpPoint>());
119  featurePoint_Point_list.back().firstParam = p;
120  featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
121  vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature,p);
122 
123  totalSize++;
124  if(featurePoint_Point_list.size() > maxSize)
125  maxSize = (unsigned int)featurePoint_Point_list.size();
126 }
127 
134 {
135  featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D,vpPoint>());
136  featurePoint3D_Point_list.back().firstParam = p;
137  featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
138  vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature,p);
139 
140  totalSize++;
141  if(featurePoint3D_Point_list.size() > maxSize)
142  maxSize = (unsigned int)featurePoint3D_Point_list.size();
143 }
144 
151 {
152  featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint,vpPoint>());
153  featureVanishingPoint_Point_list.back().firstParam = p;
154  featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
155  vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature,p);
156 
157  totalSize++;
158  if(featureVanishingPoint_Point_list.size() > maxSize)
159  maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
160 }
161 
169 {
170  featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint,vpLine,vpLine>());
171  featureVanishingPoint_DuoLine_list.back().firstParam = l1;
172  featureVanishingPoint_DuoLine_list.back().secondParam = l2;
173  featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
174  vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature,l1,l2);
175 
176  totalSize++;
177  if(featureVanishingPoint_DuoLine_list.size() > maxSize)
178  maxSize = (unsigned int)featureVanishingPoint_DuoLine_list.size();
179 }
180 
187 {
188  featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse,vpSphere>());
189  featureEllipse_Sphere_list.back().firstParam = s;
190  featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
191  vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature,s);
192 
193  totalSize++;
194  if(featureEllipse_Sphere_list.size() > maxSize)
195  maxSize = (unsigned int)featureEllipse_Sphere_list.size();
196 }
197 
204 {
205  featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse,vpCircle>());
206  featureEllipse_Circle_list.back().firstParam = c;
207  featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
208  vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature,c);
209 
210  totalSize++;
211  if(featureEllipse_Circle_list.size() > maxSize)
212  maxSize = (unsigned int)featureEllipse_Circle_list.size();
213 }
214 
221 {
222  featureLine_Line_list.push_back(vpDuo<vpFeatureLine,vpLine>());
223  featureLine_Line_list.back().firstParam = l;
224  featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
225  vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature,l);
226 
227  totalSize++;
228  if(featureLine_Line_list.size() > maxSize)
229  maxSize = (unsigned int)featureLine_Line_list.size();
230 }
231 
239 void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
240 {
241  featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine,vpCylinder,int>());
242  featureLine_DuoLineInt_List.back().firstParam = c;
243  featureLine_DuoLineInt_List.back().secondParam = line;
244  featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
245  vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature,c,line);
246 
247  totalSize++;
248  if(featureLine_DuoLineInt_List.size() > maxSize)
249  maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
250 }
251 
259 {
260  featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment,vpPoint,vpPoint>());
261  featureSegment_DuoPoints_list.back().firstParam = P1;
262  featureSegment_DuoPoints_list.back().secondParam = P2;
263  featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
264  vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature,P1,P2);
265 
266  totalSize++;
267  if(featureSegment_DuoPoints_list.size() > maxSize)
268  maxSize = (unsigned int)featureSegment_DuoPoints_list.size();
269 }
270 
278 void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L)
279 {
280  err = vpColVector();
281  L = vpMatrix();
282 
283  for(unsigned int i = 0 ; i < maxSize ; i++)
284  {
285  //--------------vpFeaturePoint--------------
286  //From vpPoint
287  if( i < featurePoint_Point_list.size() ){
288  vpFeaturePoint fp;
289  vpPoint p(featurePoint_Point_list[i].firstParam);
290  p.track(cMo);
292  err.stack(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
293  L.stack(fp.interaction());
294  }
295 
296  //--------------vpFeaturePoint3D--------------
297  //From vpPoint
298  if( i < featurePoint3D_Point_list.size() ){
299  vpFeaturePoint3D fp3D;
300  vpPoint p(featurePoint3D_Point_list[i].firstParam);
301  p.track(cMo);
302  vpFeatureBuilder::create(fp3D, p);
303  err.stack(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
304  L.stack(fp3D.interaction());
305  }
306 
307  //--------------vpFeatureVanishingPoint--------------
308  //From vpPoint
309  if( i < featureVanishingPoint_Point_list.size() ){
311  vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
312  p.track(cMo);
313  vpFeatureBuilder::create(fvp, p);
314  err.stack(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
315  L.stack(fvp.interaction());
316  }
317  //From Duo of vpLines
318  if( i < featureVanishingPoint_DuoLine_list.size() ){
320  vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
321  vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
322  l1.track(cMo);
323  l2.track(cMo);
324  vpFeatureBuilder::create(fvp, l1, l2);
325  err.stack(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
326  L.stack(fvp.interaction());
327  }
328 
329  //--------------vpFeatureEllipse--------------
330  //From vpSphere
331  if( i < featureEllipse_Sphere_list.size() ){
332  vpFeatureEllipse fe;
333  vpSphere s(featureEllipse_Sphere_list[i].firstParam);
334  s.track(cMo);
336  err.stack(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
337  L.stack(fe.interaction());
338  }
339  //From vpCircle
340  if( i < featureEllipse_Circle_list.size() ){
341  vpFeatureEllipse fe;
342  vpCircle c(featureEllipse_Circle_list[i].firstParam);
343  c.track(cMo);
345  err.stack(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
346  L.stack(fe.interaction());
347  }
348 
349  //--------------vpFeatureLine--------------
350  //From vpLine
351  if( i < featureLine_Line_list.size() ){
352  vpFeatureLine fl;
353  vpLine l(featureLine_Line_list[i].firstParam);
354  l.track(cMo);
356  err.stack(fl.error(*(featureLine_Line_list[i].desiredFeature)));
357  L.stack(fl.interaction());
358  }
359  //From Duo of vpCylinder / Integer
360  if( i < featureLine_DuoLineInt_List.size() ){
361  vpFeatureLine fl;
362  vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
363  c.track(cMo);
364  vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam);
365  err.stack(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
366  L.stack(fl.interaction());
367  }
368 
369  //--------------vpFeatureSegment--------------
370  //From Duo of vpPoints
371  if( i < featureSegment_DuoPoints_list.size() ){
372  vpFeatureSegment fs;
373  vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
374  vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
375  p1.track(cMo);
376  p2.track(cMo);
377  vpFeatureBuilder::create(fs, p1, p2);
378  err.stack(fs.error(*(featureSegment_DuoPoints_list[i].desiredFeature)));
379  L.stack(fs.interaction());
380  }
381 
382 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
383  //--------------Specific Feature--------------
384  if( i < featureSpecific_list.size() ){
385  featureSpecific_list[i]->createCurrent(cMo);
386  err.stack(featureSpecific_list[i]->error());
387  L.stack(featureSpecific_list[i]->currentInteraction());
388  }
389 #endif
390  }
391 }
392 
393 
407 {
408  switch(type)
409  {
410  case VIRTUAL_VS:
411  computePoseVVS(cMo);
412  break;
413  case ROBUST_VIRTUAL_VS:
414  computePoseRobustVVS(cMo);
415  break;
416  default:
417  break;
418  }
419 }
420 
433 void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix & cMo)
434 {
435  try
436  {
437  double residu_1 = 1e8 ;
438  double r =1e8-1;
439  // we stop the minimization when the error is bellow 1e-8
440 
441  vpMatrix L;
442  vpColVector err;
443  vpColVector v ;
444 
445  unsigned int iter = 0;
446 
447  //while((int)((residu_1 - r)*1e12) != 0 )
448  while(std::fabs((residu_1 - r)*1e12) > std::numeric_limits<double>::epsilon() )
449  {
450  residu_1 = r ;
451 
452  // Compute the interaction matrix and the error
453  error_and_interaction(cMo,err,L);
454 
455  // compute the residual
456  r = err.sumSquare() ;
457 
458  // compute the pseudo inverse of the interaction matrix
459  vpMatrix Lp ;
460  unsigned int rank = L.pseudoInverse(Lp,1e-16) ;
461 
462  if(rank < 6){
463  if(verbose)
464  vpTRACE("Rank must be at least 6 ! cMo not computed.");
465 
466  break;
467  }
468 
469  // compute the VVS control law
470  v = -lambda*Lp*err ;
471 
472  cMo = vpExponentialMap::direct(v).inverse()*cMo ;
473  if (iter++>vvsIterMax){
474  vpTRACE("Max iteration reached") ;
475  break ;
476  }
477  }
478 
479  if(computeCovariance)
480  covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
481 
482  }
483  catch(...)
484  {
485  vpERROR_TRACE("vpPoseFeatures::computePoseVVS") ;
486  throw ;
487  }
488 }
489 
490 
496 void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix & cMo)
497 {
498  try{
499  double residu_1 = 1e8 ;
500  double r =1e8-1;
501 
502  // we stop the minimization when the error is bellow 1e-8
503  vpMatrix L, W;
504  vpColVector w, res;
505  vpColVector v ;
506  vpColVector error ; // error vector
507 
508  vpRobust robust(2*totalSize) ;
509  robust.setThreshold(0.0000) ;
510 
511  unsigned int iter = 0 ;
512 
513  //while((int)((residu_1 - r)*1e12) !=0)
514  while(std::fabs((residu_1 - r)*1e12) > std::numeric_limits<double>::epsilon())
515  {
516  residu_1 = r ;
517 
518  // Compute the interaction matrix and the error
519  error_and_interaction(cMo,error,L);
520 
521  // compute the residual
522  r = error.sumSquare() ;
523 
524  if(iter == 0){
525  res.resize(error.getRows()/2);
526  w.resize(error.getRows()/2);
527  W.resize(error.getRows(),error.getRows());
528  w = 1;
529  }
530 
531  for(unsigned int k=0 ; k < error.getRows()/2 ; k++)
532  {
533  res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
534  }
535  robust.setIteration(0);
536  robust.MEstimator(vpRobust::TUKEY, res, w);
537 
538  // compute the pseudo inverse of the interaction matrix
539  for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
540  {
541  W[2*k][2*k] = w[k] ;
542  W[2*k+1][2*k+1] = w[k] ;
543  }
544  // compute the pseudo inverse of the interaction matrix
545  vpMatrix Lp ;
546  vpMatrix LRank;
547  (W*L).pseudoInverse(Lp,1e-6) ;
548  unsigned int rank = L.pseudoInverse(LRank,1e-6) ;
549 
550  if(rank < 6){
551  if(verbose)
552  vpTRACE("Rank must be at least 6 ! cMo not computed.");
553 
554  break;
555  }
556 
557  // compute the VVS control law
558  v = -lambda*Lp*W*error ;
559 
560  cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
561  if (iter++>vvsIterMax){
562  vpTRACE("Max iteration reached") ;
563  break ;
564  }
565  }
566 
567  if(computeCovariance)
568  covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
569  }
570  catch(...)
571  {
572  vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS") ;
573  throw ;
574  }
575 }
576 
577 #endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
void addFeatureSegment(vpPoint &, vpPoint &)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
void stack(const double &d)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true)
Definition: vpArray2D.h:167
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
#define vpERROR_TRACE
Definition: vpDebug.h:391
Class that defines 2D vanishing point visual feature (Z coordinate in 3D space is infinity)...
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:2981
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void addFeatureVanishingPoint(const vpPoint &)
Class that defines what is a sphere.
Definition: vpSphere.h:60
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
void addFeaturePoint3D(const vpPoint &)
void addFeaturePoint(const vpPoint &)
Class that defines what is a point.
Definition: vpPoint.h:59
void addFeatureEllipse(const vpCircle &)
virtual ~vpPoseFeatures()
void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type=VIRTUAL_VS)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
Definition: vpLine.h:105
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
#define vpTRACE
Definition: vpDebug.h:414
static double sqr(double x)
Definition: vpMath.h:110
Class that defines the 3D point visual feature.
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
double sumSquare() const
Class that defines what is a cylinder.
Definition: vpCylinder.h:93
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix direct(const vpColVector &v)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:60
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1741
Class that defines 2D ellipse visual feature.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
Class that defines what is a circle.
Definition: vpCircle.h:57
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
void addFeatureLine(const vpLine &)
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225