Visual Servoing Platform version 3.5.0
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
245void 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
285void 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()) {
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);
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);
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()) {
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()) {
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()) {
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;
420 computePoseRobustVVS(cMo);
421 break;
422 default:
423 break;
424 }
425}
426
439void 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 unsigned int rank_max = 0;
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-6); // modif FC 1e-16
465
466 if (rank_max < rank)
467 rank_max = rank;
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 if (rank_max < 6) {
479 if (verbose) {
480 vpTRACE("Only %d pose parameters can be estimated.", rank_max);
481 }
482 }
483
484 if (computeCovariance)
485 covarianceMatrix = vpMatrix::computeCovarianceMatrix(L, v, -lambda * err);
486
487 } catch (...) {
488 vpERROR_TRACE("vpPoseFeatures::computePoseVVS");
489 throw;
490 }
491}
492
499void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo)
500{
501 try {
502 double residu_1 = 1e8;
503 double r = 1e8 - 1;
504
505 // we stop the minimization when the error is bellow 1e-8
506 vpMatrix L, W;
507 vpColVector w, res;
508 vpColVector v;
509 vpColVector error; // error vector
510
511 vpRobust robust;
512 robust.setMinMedianAbsoluteDeviation(0.00001);
513
514 unsigned int rank_max = 0;
515 unsigned int iter = 0;
516
517 // while((int)((residu_1 - r)*1e12) !=0)
518 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
519 residu_1 = r;
520
521 // Compute the interaction matrix and the error
522 error_and_interaction(cMo, error, L);
523
524 // compute the residual
525 r = error.sumSquare();
526
527 if (iter == 0) {
528 res.resize(error.getRows() / 2);
529 w.resize(error.getRows() / 2);
530 W.resize(error.getRows(), error.getRows());
531 w = 1;
532 }
533
534 for (unsigned int k = 0; k < error.getRows() / 2; k++) {
535 res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
536 }
537 robust.MEstimator(vpRobust::TUKEY, res, w);
538
539 // compute the pseudo inverse of the interaction matrix
540 for (unsigned int k = 0; k < error.getRows() / 2; k++) {
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_max < rank)
551 rank_max = rank;
552
553 // compute the VVS control law
554 v = -lambda * Lp * W * error;
555
556 cMo = vpExponentialMap::direct(v).inverse() * cMo;
557 if (iter++ > vvsIterMax) {
558 vpTRACE("Max iteration reached");
559 break;
560 }
561 }
562
563 if (rank_max < 6) {
564 if (verbose) {
565 vpTRACE("Only %d pose parameters can be estimated.", rank_max);
566 }
567 }
568
569 if (computeCovariance)
570 covarianceMatrix =
571 vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
572 // matrix is diagonale, but using W*W
573 // is more efficient.
574 } catch (...) {
575 vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS");
576 throw;
577 }
578}
579
580#endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:92
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:103
static vpHomogeneousMatrix direct(const vpColVector &v)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines 2D ellipse visual feature.
vpMatrix interaction(unsigned int select=FEATURE_ALL)
compute the interaction matrix from a subset a the possible features
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines the 3D point visual feature.
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
vpMatrix interaction(unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
vpColVector error(const vpBasicFeature &s_star, unsigned int select=(selectX()|selectY()))
vpMatrix interaction(unsigned int select=(selectX()|selectY()))
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
Definition: vpLine.h:105
static double sqr(double x)
Definition: vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void addFeatureVanishingPoint(const vpPoint &)
void addFeaturePoint3D(const vpPoint &)
void addFeatureLine(const vpLine &)
void addFeaturePoint(const vpPoint &)
virtual ~vpPoseFeatures()
void addFeatureEllipse(const vpCircle &)
void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type=VIRTUAL_VS)
void addFeatureSegment(vpPoint &, vpPoint &)
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition: vpSphere.h:83
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393