ViSP  2.9.0
vpPoseFeatures.cpp
1 /****************************************************************************
2  *
3  * $Id: vpPoseFeatures.cpp 4632 2014-02-03 17:06:40Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Pose computation from any features.
36  *
37  * Authors:
38  * Aurelien Yol
39  *
40  *****************************************************************************/
41 #include <visp/vpPoseFeatures.h>
42 
47  : maxSize(0), totalSize(0), vvsIterMax(200), lambda(1.0), verbose(false), computeCovariance(false),
48  covarianceMatrix(), featurePoint_Point_list(), featurePoint3D_Point_list(), featureVanishingPoint_Point_list(),
49  featureVanishingPoint_DuoLine_list(), featureEllipse_Sphere_list(), featureEllipse_Circle_list(),
50  featureLine_Line_list(), featureLine_DuoLineInt_List(), featureSegment_DuoPoints_list()
51 {
52 }
53 
58 {
59  clear();
60 }
61 
66 {
67  for(int i = (int)featurePoint_Point_list.size()-1 ; i >= 0 ; i--)
68  delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
69  featurePoint_Point_list.clear();
70 
71  for(int i = (int)featurePoint3D_Point_list.size()-1 ; i >= 0 ; i--)
72  delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
73  featurePoint3D_Point_list.clear();
74 
75  for(int i = (int)featureVanishingPoint_Point_list.size()-1 ; i >= 0 ; i--)
76  delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
77  featureVanishingPoint_Point_list.clear();
78 
79  for(int i = (int)featureVanishingPoint_DuoLine_list.size()-1 ; i >= 0 ; i--)
80  delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
81  featureVanishingPoint_DuoLine_list.clear();
82 
83  for(int i = (int)featureEllipse_Sphere_list.size()-1 ; i >= 0 ; i--)
84  delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
85  featureEllipse_Sphere_list.clear();
86 
87  for(int i = (int)featureEllipse_Circle_list.size()-1 ; i >= 0 ; i--)
88  delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
89  featureEllipse_Circle_list.clear();
90 
91  for(int i = (int)featureLine_Line_list.size()-1 ; i >= 0 ; i--)
92  delete featureLine_Line_list[(unsigned int)i].desiredFeature;
93  featureLine_Line_list.clear();
94 
95  for(int i = (int)featureLine_DuoLineInt_List.size()-1 ; i >= 0 ; i--)
96  delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
97  featureLine_DuoLineInt_List.clear();
98 
99  for(int i = (int)featureSegment_DuoPoints_list.size()-1 ; i >= 0 ; i--)
100  delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
101  featureSegment_DuoPoints_list.clear();
102 
103 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
104  for(int i = (int)featureSpecific_list.size()-1 ; i >= 0 ; i--)
105  delete featureSpecific_list[(unsigned int)i];
106  featureSpecific_list.clear();
107 #endif
108 
109  maxSize = 0;
110  totalSize = 0;
111 }
112 
119 {
120  featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint,vpPoint>());
121  featurePoint_Point_list.back().firstParam = p;
122  featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
123  vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature,p);
124 
125  totalSize++;
126  if(featurePoint_Point_list.size() > maxSize)
127  maxSize = (unsigned int)featurePoint_Point_list.size();
128 }
129 
136 {
137  featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D,vpPoint>());
138  featurePoint3D_Point_list.back().firstParam = p;
139  featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
140  vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature,p);
141 
142  totalSize++;
143  if(featurePoint3D_Point_list.size() > maxSize)
144  maxSize = (unsigned int)featurePoint3D_Point_list.size();
145 }
146 
153 {
154  featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint,vpPoint>());
155  featureVanishingPoint_Point_list.back().firstParam = p;
156  featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
157  vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature,p);
158 
159  totalSize++;
160  if(featureVanishingPoint_Point_list.size() > maxSize)
161  maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
162 }
163 
171 {
172  featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint,vpLine,vpLine>());
173  featureVanishingPoint_DuoLine_list.back().firstParam = l1;
174  featureVanishingPoint_DuoLine_list.back().secondParam = l2;
175  featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
176  vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature,l1,l2);
177 
178  totalSize++;
179  if(featureVanishingPoint_DuoLine_list.size() > maxSize)
180  maxSize = (unsigned int)featureVanishingPoint_DuoLine_list.size();
181 }
182 
189 {
190  featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse,vpSphere>());
191  featureEllipse_Sphere_list.back().firstParam = s;
192  featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
193  vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature,s);
194 
195  totalSize++;
196  if(featureEllipse_Sphere_list.size() > maxSize)
197  maxSize = (unsigned int)featureEllipse_Sphere_list.size();
198 }
199 
206 {
207  featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse,vpCircle>());
208  featureEllipse_Circle_list.back().firstParam = c;
209  featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
210  vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature,c);
211 
212  totalSize++;
213  if(featureEllipse_Circle_list.size() > maxSize)
214  maxSize = (unsigned int)featureEllipse_Circle_list.size();
215 }
216 
223 {
224  featureLine_Line_list.push_back(vpDuo<vpFeatureLine,vpLine>());
225  featureLine_Line_list.back().firstParam = l;
226  featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
227  vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature,l);
228 
229  totalSize++;
230  if(featureLine_Line_list.size() > maxSize)
231  maxSize = (unsigned int)featureLine_Line_list.size();
232 }
233 
241 void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
242 {
243  featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine,vpCylinder,int>());
244  featureLine_DuoLineInt_List.back().firstParam = c;
245  featureLine_DuoLineInt_List.back().secondParam = line;
246  featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
247  vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature,c,line);
248 
249  totalSize++;
250  if(featureLine_DuoLineInt_List.size() > maxSize)
251  maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
252 }
253 
261 {
262  featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment,vpPoint,vpPoint>());
263  featureSegment_DuoPoints_list.back().firstParam = P1;
264  featureSegment_DuoPoints_list.back().secondParam = P2;
265  featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
266  vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature,P1,P2);
267 
268  totalSize++;
269  if(featureSegment_DuoPoints_list.size() > maxSize)
270  maxSize = (unsigned int)featureSegment_DuoPoints_list.size();
271 }
272 
280 void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L)
281 {
282  err = vpColVector();
283  L = vpMatrix();
284 
285  for(unsigned int i = 0 ; i < maxSize ; i++)
286  {
287  //--------------vpFeaturePoint--------------
288  //From vpPoint
289  if( i < featurePoint_Point_list.size() ){
290  vpFeaturePoint fp;
291  vpPoint p(featurePoint_Point_list[i].firstParam);
292  p.track(cMo);
294  err.stackMatrices(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
295  L.stackMatrices(fp.interaction());
296  }
297 
298  //--------------vpFeaturePoint3D--------------
299  //From vpPoint
300  if( i < featurePoint3D_Point_list.size() ){
301  vpFeaturePoint3D fp3D;
302  vpPoint p(featurePoint3D_Point_list[i].firstParam);
303  p.track(cMo);
304  vpFeatureBuilder::create(fp3D, p);
305  err.stackMatrices(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
306  L.stackMatrices(fp3D.interaction());
307  }
308 
309  //--------------vpFeatureVanishingPoint--------------
310  //From vpPoint
311  if( i < featureVanishingPoint_Point_list.size() ){
313  vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
314  p.track(cMo);
315  vpFeatureBuilder::create(fvp, p);
316  err.stackMatrices(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
317  L.stackMatrices(fvp.interaction());
318  }
319  //From Duo of vpLines
320  if( i < featureVanishingPoint_DuoLine_list.size() ){
322  vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
323  vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
324  l1.track(cMo);
325  l2.track(cMo);
326  vpFeatureBuilder::create(fvp, l1, l2);
327  err.stackMatrices(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
328  L.stackMatrices(fvp.interaction());
329  }
330 
331  //--------------vpFeatureEllipse--------------
332  //From vpSphere
333  if( i < featureEllipse_Sphere_list.size() ){
334  vpFeatureEllipse fe;
335  vpSphere s(featureEllipse_Sphere_list[i].firstParam);
336  s.track(cMo);
338  err.stackMatrices(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
339  L.stackMatrices(fe.interaction());
340  }
341  //From vpCircle
342  if( i < featureEllipse_Circle_list.size() ){
343  vpFeatureEllipse fe;
344  vpCircle c(featureEllipse_Circle_list[i].firstParam);
345  c.track(cMo);
347  err.stackMatrices(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
348  L.stackMatrices(fe.interaction());
349  }
350 
351  //--------------vpFeatureLine--------------
352  //From vpLine
353  if( i < featureLine_Line_list.size() ){
354  vpFeatureLine fl;
355  vpLine l(featureLine_Line_list[i].firstParam);
356  l.track(cMo);
358  err.stackMatrices(fl.error(*(featureLine_Line_list[i].desiredFeature)));
359  L.stackMatrices(fl.interaction());
360  }
361  //From Duo of vpCylinder / Integer
362  if( i < featureLine_DuoLineInt_List.size() ){
363  vpFeatureLine fl;
364  vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
365  c.track(cMo);
366  vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam);
367  err.stackMatrices(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
368  L.stackMatrices(fl.interaction());
369  }
370 
371  //--------------vpFeatureSegment--------------
372  //From Duo of vpPoints
373  if( i < featureSegment_DuoPoints_list.size() ){
374  vpFeatureSegment fs;
375  vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
376  vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
377  p1.track(cMo);
378  p2.track(cMo);
379  vpFeatureBuilder::create(fs, p1, p2);
380  err.stackMatrices(fs.error(*(featureSegment_DuoPoints_list[i].desiredFeature)));
381  L.stackMatrices(fs.interaction());
382  }
383 
384 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
385  //--------------Specific Feature--------------
386  if( i < featureSpecific_list.size() ){
387  featureSpecific_list[i]->createCurrent(cMo);
388  err.stackMatrices(featureSpecific_list[i]->error());
389  L.stackMatrices(featureSpecific_list[i]->currentInteraction());
390  }
391 #endif
392  }
393 }
394 
395 
409 {
410  switch(type)
411  {
412  case VIRTUAL_VS:
413  computePoseVVS(cMo);
414  break;
415  case ROBUST_VIRTUAL_VS:
416  computePoseRobustVVS(cMo);
417  break;
418  default:
419  break;
420  }
421 }
422 
435 void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix & cMo)
436 {
437  try
438  {
439  double residu_1 = 1e8 ;
440  double r =1e8-1;
441  // we stop the minimization when the error is bellow 1e-8
442 
443  vpMatrix L;
444  vpColVector err;
445  vpColVector v ;
446 
447  unsigned int iter = 0;
448 
449  while((int)((residu_1 - r)*1e12) != 0 )
450  {
451  residu_1 = r ;
452 
453  // Compute the interaction matrix and the error
454  error_and_interaction(cMo,err,L);
455 
456  // compute the residual
457  r = err.sumSquare() ;
458 
459  // compute the pseudo inverse of the interaction matrix
460  vpMatrix Lp ;
461  unsigned int rank = L.pseudoInverse(Lp,1e-16) ;
462 
463  if(rank < 6){
464  if(verbose)
465  vpTRACE("Rank must be at least 6 ! cMo not computed.");
466 
467  break;
468  }
469 
470  // compute the VVS control law
471  v = -lambda*Lp*err ;
472 
473  cMo = vpExponentialMap::direct(v).inverse()*cMo ;
474  if (iter++>vvsIterMax){
475  vpTRACE("Max iteration reached") ;
476  break ;
477  }
478  }
479 
480  if(computeCovariance)
481  covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
482 
483  }
484  catch(...)
485  {
486  vpERROR_TRACE("vpPoseFeatures::computePoseVVS") ;
487  throw ;
488  }
489 }
490 
491 
497 void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix & cMo)
498 {
499  try{
500  double residu_1 = 1e8 ;
501  double r =1e8-1;
502 
503  // we stop the minimization when the error is bellow 1e-8
504  vpMatrix L, W;
505  vpColVector w, res;
506  vpColVector v ;
507  vpColVector error ; // error vector
508 
509  vpRobust robust(2*totalSize) ;
510  robust.setThreshold(0.0000) ;
511 
512  unsigned int iter = 0 ;
513 
514  while((int)((residu_1 - r)*1e12) !=0)
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 = vpColVector(error.getRows()/2);
526  W = vpMatrix(error.getRows(),error.getRows());
527  w = vpColVector(error.getRows()/2);
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 
578 
579 
580 
581 
582 
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
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)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
Class that defines 2D vanishing point visual feature (Z coordinate in 3D space is infinity)...
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:64
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
double sumSquare() const
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:809
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:65
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:124
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
static double sqr(double x)
Definition: vpMath.h:106
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)
void stackMatrices(const vpMatrix &A)
Definition: vpMatrix.cpp:3003
Class that defines what is a cylinder.
Definition: vpCylinder.h:97
Class that provides a data structure for the column vectors as well as a set of operations on these v...
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:63
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1861
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)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
vpMatrix interaction(const unsigned int select=FEATURE_ALL)
Class that defines what is a circle.
Definition: vpCircle.h:61
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