Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpPoseFeatures.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  * Pose computation from any features.
33  *
34  * Authors:
35  * Aurelien Yol
36  *
37  *****************************************************************************/
38 #include <visp3/vision/vpPoseFeatures.h>
39 
40 #ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
41 
46  : maxSize(0), totalSize(0), vvsIterMax(200), lambda(1.0), verbose(false), computeCovariance(false),
47  covarianceMatrix(), featurePoint_Point_list(), featurePoint3D_Point_list(), featureVanishingPoint_Point_list(),
48  featureVanishingPoint_DuoLine_list(), featureEllipse_Sphere_list(), featureEllipse_Circle_list(),
49  featureLine_Line_list(), featureLine_DuoLineInt_List(), featureSegment_DuoPoints_list()
50 {
51 }
52 
57 
62 {
63  for (int i = (int)featurePoint_Point_list.size() - 1; i >= 0; i--)
64  delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
65  featurePoint_Point_list.clear();
66 
67  for (int i = (int)featurePoint3D_Point_list.size() - 1; i >= 0; i--)
68  delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
69  featurePoint3D_Point_list.clear();
70 
71  for (int i = (int)featureVanishingPoint_Point_list.size() - 1; i >= 0; i--)
72  delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
73  featureVanishingPoint_Point_list.clear();
74 
75  for (int i = (int)featureVanishingPoint_DuoLine_list.size() - 1; i >= 0; i--)
76  delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
77  featureVanishingPoint_DuoLine_list.clear();
78 
79  for (int i = (int)featureEllipse_Sphere_list.size() - 1; i >= 0; i--)
80  delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
81  featureEllipse_Sphere_list.clear();
82 
83  for (int i = (int)featureEllipse_Circle_list.size() - 1; i >= 0; i--)
84  delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
85  featureEllipse_Circle_list.clear();
86 
87  for (int i = (int)featureLine_Line_list.size() - 1; i >= 0; i--)
88  delete featureLine_Line_list[(unsigned int)i].desiredFeature;
89  featureLine_Line_list.clear();
90 
91  for (int i = (int)featureLine_DuoLineInt_List.size() - 1; i >= 0; i--)
92  delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
93  featureLine_DuoLineInt_List.clear();
94 
95  for (int i = (int)featureSegment_DuoPoints_list.size() - 1; i >= 0; i--)
96  delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
97  featureSegment_DuoPoints_list.clear();
98 
99 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
100  for (int i = (int)featureSpecific_list.size() - 1; i >= 0; i--)
101  delete featureSpecific_list[(unsigned int)i];
102  featureSpecific_list.clear();
103 #endif
104 
105  maxSize = 0;
106  totalSize = 0;
107 }
108 
116 {
117  featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint, vpPoint>());
118  featurePoint_Point_list.back().firstParam = p;
119  featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
120  vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature, p);
121 
122  totalSize++;
123  if (featurePoint_Point_list.size() > maxSize)
124  maxSize = (unsigned int)featurePoint_Point_list.size();
125 }
126 
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 
152 {
153  featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint, vpPoint>());
154  featureVanishingPoint_Point_list.back().firstParam = p;
155  featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
156  vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature, p);
157 
158  totalSize++;
159  if (featureVanishingPoint_Point_list.size() > maxSize)
160  maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
161 }
162 
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 
190 {
191  featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse, vpSphere>());
192  featureEllipse_Sphere_list.back().firstParam = s;
193  featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
194  vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature, s);
195 
196  totalSize++;
197  if (featureEllipse_Sphere_list.size() > maxSize)
198  maxSize = (unsigned int)featureEllipse_Sphere_list.size();
199 }
200 
208 {
209  featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse, vpCircle>());
210  featureEllipse_Circle_list.back().firstParam = c;
211  featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
212  vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature, c);
213 
214  totalSize++;
215  if (featureEllipse_Circle_list.size() > maxSize)
216  maxSize = (unsigned int)featureEllipse_Circle_list.size();
217 }
218 
226 {
227  featureLine_Line_list.push_back(vpDuo<vpFeatureLine, vpLine>());
228  featureLine_Line_list.back().firstParam = l;
229  featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
230  vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature, l);
231 
232  totalSize++;
233  if (featureLine_Line_list.size() > maxSize)
234  maxSize = (unsigned int)featureLine_Line_list.size();
235 }
236 
245 void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
246 {
247  featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine, vpCylinder, int>());
248  featureLine_DuoLineInt_List.back().firstParam = c;
249  featureLine_DuoLineInt_List.back().secondParam = line;
250  featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
251  vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature, c, line);
252 
253  totalSize++;
254  if (featureLine_DuoLineInt_List.size() > maxSize)
255  maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
256 }
257 
266 {
267  featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment, vpPoint, vpPoint>());
268  featureSegment_DuoPoints_list.back().firstParam = P1;
269  featureSegment_DuoPoints_list.back().secondParam = P2;
270  featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
271  vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature, P1, P2);
272 
273  totalSize++;
274  if (featureSegment_DuoPoints_list.size() > maxSize)
275  maxSize = (unsigned int)featureSegment_DuoPoints_list.size();
276 }
277 
285 void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L)
286 {
287  err = vpColVector();
288  L = vpMatrix();
289 
290  for (unsigned int i = 0; i < maxSize; i++) {
291  //--------------vpFeaturePoint--------------
292  // From vpPoint
293  if (i < featurePoint_Point_list.size()) {
294  vpFeaturePoint fp;
295  vpPoint p(featurePoint_Point_list[i].firstParam);
296  p.track(cMo);
298  err.stack(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
299  L.stack(fp.interaction());
300  }
301 
302  //--------------vpFeaturePoint3D--------------
303  // From vpPoint
304  if (i < featurePoint3D_Point_list.size()) {
305  vpFeaturePoint3D fp3D;
306  vpPoint p(featurePoint3D_Point_list[i].firstParam);
307  p.track(cMo);
308  vpFeatureBuilder::create(fp3D, p);
309  err.stack(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
310  L.stack(fp3D.interaction());
311  }
312 
313  //--------------vpFeatureVanishingPoint--------------
314  // From vpPoint
315  if (i < featureVanishingPoint_Point_list.size()) {
317  vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
318  p.track(cMo);
319  vpFeatureBuilder::create(fvp, p);
320  err.stack(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
321  L.stack(fvp.interaction());
322  }
323  // From Duo of vpLines
324  if (i < featureVanishingPoint_DuoLine_list.size()) {
326  vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
327  vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
328  l1.track(cMo);
329  l2.track(cMo);
330  vpFeatureBuilder::create(fvp, l1, l2);
331  err.stack(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
332  L.stack(fvp.interaction());
333  }
334 
335  //--------------vpFeatureEllipse--------------
336  // From vpSphere
337  if (i < featureEllipse_Sphere_list.size()) {
338  vpFeatureEllipse fe;
339  vpSphere s(featureEllipse_Sphere_list[i].firstParam);
340  s.track(cMo);
342  err.stack(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
343  L.stack(fe.interaction());
344  }
345  // From vpCircle
346  if (i < featureEllipse_Circle_list.size()) {
347  vpFeatureEllipse fe;
348  vpCircle c(featureEllipse_Circle_list[i].firstParam);
349  c.track(cMo);
351  err.stack(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
352  L.stack(fe.interaction());
353  }
354 
355  //--------------vpFeatureLine--------------
356  // From vpLine
357  if (i < featureLine_Line_list.size()) {
358  vpFeatureLine fl;
359  vpLine l(featureLine_Line_list[i].firstParam);
360  l.track(cMo);
362  err.stack(fl.error(*(featureLine_Line_list[i].desiredFeature)));
363  L.stack(fl.interaction());
364  }
365  // From Duo of vpCylinder / Integer
366  if (i < featureLine_DuoLineInt_List.size()) {
367  vpFeatureLine fl;
368  vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
369  c.track(cMo);
370  vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam);
371  err.stack(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
372  L.stack(fl.interaction());
373  }
374 
375  //--------------vpFeatureSegment--------------
376  // From Duo of vpPoints
377  if (i < featureSegment_DuoPoints_list.size()) {
378  vpFeatureSegment fs;
379  vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
380  vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
381  p1.track(cMo);
382  p2.track(cMo);
383  vpFeatureBuilder::create(fs, p1, p2);
384  err.stack(fs.error(*(featureSegment_DuoPoints_list[i].desiredFeature)));
385  L.stack(fs.interaction());
386  }
387 
388 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
389  //--------------Specific Feature--------------
390  if (i < featureSpecific_list.size()) {
391  featureSpecific_list[i]->createCurrent(cMo);
392  err.stack(featureSpecific_list[i]->error());
393  L.stack(featureSpecific_list[i]->currentInteraction());
394  }
395 #endif
396  }
397 }
398 
414 {
415  switch (type) {
416  case VIRTUAL_VS:
417  computePoseVVS(cMo);
418  break;
419  case ROBUST_VIRTUAL_VS:
420  computePoseRobustVVS(cMo);
421  break;
422  default:
423  break;
424  }
425 }
426 
439 void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix &cMo)
440 {
441  try {
442  double residu_1 = 1e8;
443  double r = 1e8 - 1;
444  // we stop the minimization when the error is bellow 1e-8
445 
446  vpMatrix L;
447  vpColVector err;
448  vpColVector v;
449 
450  unsigned int iter = 0;
451 
452  // while((int)((residu_1 - r)*1e12) != 0 )
453  while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
454  residu_1 = r;
455 
456  // Compute the interaction matrix and the error
457  error_and_interaction(cMo, err, L);
458 
459  // compute the residual
460  r = err.sumSquare();
461 
462  // compute the pseudo inverse of the interaction matrix
463  vpMatrix Lp;
464  unsigned int rank = L.pseudoInverse(Lp, 1e-16);
465 
466  if (rank < 6) {
467  if (verbose)
468  vpTRACE("Rank must be at least 6 ! cMo not computed.");
469 
470  break;
471  }
472 
473  // compute the VVS control law
474  v = -lambda * Lp * err;
475 
476  cMo = vpExponentialMap::direct(v).inverse() * cMo;
477  if (iter++ > vvsIterMax) {
478  vpTRACE("Max iteration reached");
479  break;
480  }
481  }
482 
483  if (computeCovariance)
484  covarianceMatrix = vpMatrix::computeCovarianceMatrix(L, v, -lambda * err);
485 
486  } catch (...) {
487  vpERROR_TRACE("vpPoseFeatures::computePoseVVS");
488  throw;
489  }
490 }
491 
498 void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo)
499 {
500  try {
501  double residu_1 = 1e8;
502  double r = 1e8 - 1;
503 
504  // we stop the minimization when the error is bellow 1e-8
505  vpMatrix L, W;
506  vpColVector w, res;
507  vpColVector v;
508  vpColVector error; // error vector
509 
510  vpRobust robust(2 * totalSize);
511  robust.setThreshold(0.0000);
512 
513  unsigned int iter = 0;
514 
515  // while((int)((residu_1 - r)*1e12) !=0)
516  while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
517  residu_1 = r;
518 
519  // Compute the interaction matrix and the error
520  error_and_interaction(cMo, error, L);
521 
522  // compute the residual
523  r = error.sumSquare();
524 
525  if (iter == 0) {
526  res.resize(error.getRows() / 2);
527  w.resize(error.getRows() / 2);
528  W.resize(error.getRows(), error.getRows());
529  w = 1;
530  }
531 
532  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
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  W[2 * k][2 * k] = w[k];
541  W[2 * k + 1][2 * k + 1] = w[k];
542  }
543  // compute the pseudo inverse of the interaction matrix
544  vpMatrix Lp;
545  vpMatrix LRank;
546  (W * L).pseudoInverse(Lp, 1e-6);
547  unsigned int rank = L.pseudoInverse(LRank, 1e-6);
548 
549  if (rank < 6) {
550  if (verbose)
551  vpTRACE("Rank must be at least 6 ! cMo not computed.");
552 
553  break;
554  }
555 
556  // compute the VVS control law
557  v = -lambda * Lp * W * error;
558 
559  cMo = vpExponentialMap::direct(v).inverse() * cMo;
560  ;
561  if (iter++ > vvsIterMax) {
562  vpTRACE("Max iteration reached");
563  break;
564  }
565  }
566 
567  if (computeCovariance)
568  covarianceMatrix =
569  vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
570  // matrix is diagonale, but using W*W
571  // is more efficient.
572  } catch (...) {
573  vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS");
574  throw;
575  }
576 }
577 
578 #endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:305
vpMatrix interaction(unsigned int select=FEATURE_ALL)
void addFeatureSegment(vpPoint &, vpPoint &)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
#define vpERROR_TRACE
Definition: vpDebug.h:393
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4800
vpMatrix interaction(unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
unsigned int getRows() const
Definition: vpArray2D.h:289
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void addFeatureVanishingPoint(const vpPoint &)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=(selectX()|selectY()))
vpMatrix interaction(unsigned int select=(selectX()|selectY()))
Class that defines what is a sphere.
Definition: vpSphere.h:60
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:58
void addFeatureEllipse(const vpCircle &)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
virtual ~vpPoseFeatures()
void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type=VIRTUAL_VS)
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
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:114
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 ...
void setThreshold(double noise_threshold)
Definition: vpRobust.h:115
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Class that defines what is a cylinder.
Definition: vpCylinder.h:96
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void stack(double d)
double sumSquare() const
static vpHomogeneousMatrix direct(const vpColVector &v)
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
Class that defines 2D ellipse visual feature.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Class that defines what is a circle.
Definition: vpCircle.h:58
void addFeatureLine(const vpLine &)
void setIteration(unsigned int iter)
Set iteration.
Definition: vpRobust.h:109