Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpPoseFeatures.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
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 https://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#include <visp3/vision/vpPoseFeatures.h>
34
35#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
36
41 : maxSize(0), totalSize(0), vvsIterMax(200), lambda(1.0), verbose(false), computeCovariance(false),
42 covarianceMatrix(), featurePoint_Point_list(), featurePoint3D_Point_list(), featureVanishingPoint_Point_list(),
43 featureVanishingPoint_DuoLine_list(), featureEllipse_Sphere_list(), featureEllipse_Circle_list(),
44 featureLine_Line_list(), featureLine_DuoLineInt_List(), featureSegment_DuoPoints_list()
45{
46}
47
52
57{
58 for (int i = (int)featurePoint_Point_list.size() - 1; i >= 0; i--)
59 delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
60 featurePoint_Point_list.clear();
61
62 for (int i = (int)featurePoint3D_Point_list.size() - 1; i >= 0; i--)
63 delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
64 featurePoint3D_Point_list.clear();
65
66 for (int i = (int)featureVanishingPoint_Point_list.size() - 1; i >= 0; i--)
67 delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
68 featureVanishingPoint_Point_list.clear();
69
70 for (int i = (int)featureVanishingPoint_DuoLine_list.size() - 1; i >= 0; i--)
71 delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
72 featureVanishingPoint_DuoLine_list.clear();
73
74 for (int i = (int)featureEllipse_Sphere_list.size() - 1; i >= 0; i--)
75 delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
76 featureEllipse_Sphere_list.clear();
77
78 for (int i = (int)featureEllipse_Circle_list.size() - 1; i >= 0; i--)
79 delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
80 featureEllipse_Circle_list.clear();
81
82 for (int i = (int)featureLine_Line_list.size() - 1; i >= 0; i--)
83 delete featureLine_Line_list[(unsigned int)i].desiredFeature;
84 featureLine_Line_list.clear();
85
86 for (int i = (int)featureLine_DuoLineInt_List.size() - 1; i >= 0; i--)
87 delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
88 featureLine_DuoLineInt_List.clear();
89
90 for (int i = (int)featureSegment_DuoPoints_list.size() - 1; i >= 0; i--)
91 delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
92 featureSegment_DuoPoints_list.clear();
93
94#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
95 for (int i = (int)featureSpecific_list.size() - 1; i >= 0; i--)
96 delete featureSpecific_list[(unsigned int)i];
97 featureSpecific_list.clear();
98#endif
99
100 maxSize = 0;
101 totalSize = 0;
102}
103
111{
112 featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint, vpPoint>());
113 featurePoint_Point_list.back().firstParam = p;
114 featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
115 vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature, p);
116
117 totalSize++;
118 if (featurePoint_Point_list.size() > maxSize)
119 maxSize = (unsigned int)featurePoint_Point_list.size();
120}
121
129{
130 featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D, vpPoint>());
131 featurePoint3D_Point_list.back().firstParam = p;
132 featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
133 vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature, p);
134
135 totalSize++;
136 if (featurePoint3D_Point_list.size() > maxSize)
137 maxSize = (unsigned int)featurePoint3D_Point_list.size();
138}
139
147{
148 featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint, vpPoint>());
149 featureVanishingPoint_Point_list.back().firstParam = p;
150 featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
151 vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature, p);
152
153 totalSize++;
154 if (featureVanishingPoint_Point_list.size() > maxSize)
155 maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
156}
157
166{
167 featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint, vpLine, vpLine>());
168 featureVanishingPoint_DuoLine_list.back().firstParam = l1;
169 featureVanishingPoint_DuoLine_list.back().secondParam = l2;
170 featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
171 vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature, l1, l2);
172
173 totalSize++;
174 if (featureVanishingPoint_DuoLine_list.size() > maxSize)
175 maxSize = (unsigned int)featureVanishingPoint_DuoLine_list.size();
176}
177
185{
186 featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse, vpSphere>());
187 featureEllipse_Sphere_list.back().firstParam = s;
188 featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
189 vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature, s);
190
191 totalSize++;
192 if (featureEllipse_Sphere_list.size() > maxSize)
193 maxSize = (unsigned int)featureEllipse_Sphere_list.size();
194}
195
203{
204 featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse, vpCircle>());
205 featureEllipse_Circle_list.back().firstParam = c;
206 featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
207 vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature, c);
208
209 totalSize++;
210 if (featureEllipse_Circle_list.size() > maxSize)
211 maxSize = (unsigned int)featureEllipse_Circle_list.size();
212}
213
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
240void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
241{
242 featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine, vpCylinder, int>());
243 featureLine_DuoLineInt_List.back().firstParam = c;
244 featureLine_DuoLineInt_List.back().secondParam = line;
245 featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
246 vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature, c, line);
247
248 totalSize++;
249 if (featureLine_DuoLineInt_List.size() > maxSize)
250 maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
251}
252
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
280void 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 //--------------vpFeaturePoint--------------
287 // From vpPoint
288 if (i < featurePoint_Point_list.size()) {
290 vpPoint p(featurePoint_Point_list[i].firstParam);
291 p.track(cMo);
293 err.stack(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
294 L.stack(fp.interaction());
295 }
296
297 //--------------vpFeaturePoint3D--------------
298 // From vpPoint
299 if (i < featurePoint3D_Point_list.size()) {
300 vpFeaturePoint3D fp3D;
301 vpPoint p(featurePoint3D_Point_list[i].firstParam);
302 p.track(cMo);
304 err.stack(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
305 L.stack(fp3D.interaction());
306 }
307
308 //--------------vpFeatureVanishingPoint--------------
309 // From vpPoint
310 if (i < featureVanishingPoint_Point_list.size()) {
312 vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
313 p.track(cMo);
315 err.stack(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
316 L.stack(fvp.interaction());
317 }
318 // From Duo of vpLines
319 if (i < featureVanishingPoint_DuoLine_list.size()) {
321 vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
322 vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
323 l1.track(cMo);
324 l2.track(cMo);
325 vpFeatureBuilder::create(fvp, l1, l2);
326 err.stack(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
327 L.stack(fvp.interaction());
328 }
329
330 //--------------vpFeatureEllipse--------------
331 // From vpSphere
332 if (i < featureEllipse_Sphere_list.size()) {
334 vpSphere s(featureEllipse_Sphere_list[i].firstParam);
335 s.track(cMo);
337 err.stack(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
338 L.stack(fe.interaction());
339 }
340 // From vpCircle
341 if (i < featureEllipse_Circle_list.size()) {
343 vpCircle c(featureEllipse_Circle_list[i].firstParam);
344 c.track(cMo);
346 err.stack(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
347 L.stack(fe.interaction());
348 }
349
350 //--------------vpFeatureLine--------------
351 // From vpLine
352 if (i < featureLine_Line_list.size()) {
353 vpFeatureLine fl;
354 vpLine l(featureLine_Line_list[i].firstParam);
355 l.track(cMo);
357 err.stack(fl.error(*(featureLine_Line_list[i].desiredFeature)));
358 L.stack(fl.interaction());
359 }
360 // From Duo of vpCylinder / Integer
361 if (i < featureLine_DuoLineInt_List.size()) {
362 vpFeatureLine fl;
363 vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
364 c.track(cMo);
365 vpFeatureBuilder::create(fl, c, featureLine_DuoLineInt_List[i].secondParam);
366 err.stack(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
367 L.stack(fl.interaction());
368 }
369
370 //--------------vpFeatureSegment--------------
371 // From Duo of vpPoints
372 if (i < featureSegment_DuoPoints_list.size()) {
374 vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
375 vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
376 p1.track(cMo);
377 p2.track(cMo);
378 vpFeatureBuilder::create(fs, p1, p2);
379 err.stack(fs.error(*(featureSegment_DuoPoints_list[i].desiredFeature)));
380 L.stack(fs.interaction());
381 }
382
383#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
384 //--------------Specific Feature--------------
385 if (i < featureSpecific_list.size()) {
386 featureSpecific_list[i]->createCurrent(cMo);
387 err.stack(featureSpecific_list[i]->error());
388 L.stack(featureSpecific_list[i]->currentInteraction());
389 }
390#endif
391 }
392}
393
409{
410 switch (type) {
411 case VIRTUAL_VS:
412 computePoseVVS(cMo);
413 break;
415 computePoseRobustVVS(cMo);
416 break;
417 default:
418 break;
419 }
420}
421
434void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix &cMo)
435{
436 try {
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 unsigned int rank_max = 0;
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 residu_1 = r;
450
451 // Compute the interaction matrix and the error
452 error_and_interaction(cMo, err, L);
453
454 // compute the residual
455 r = err.sumSquare();
456
457 // compute the pseudo inverse of the interaction matrix
458 vpMatrix Lp;
459 unsigned int rank = L.pseudoInverse(Lp, 1e-6); // modif FC 1e-16
460
461 if (rank_max < rank)
462 rank_max = rank;
463
464 // compute the VVS control law
465 v = -lambda * Lp * err;
466
467 cMo = vpExponentialMap::direct(v).inverse() * cMo;
468 if (iter++ > vvsIterMax) {
469 vpTRACE("Max iteration reached");
470 break;
471 }
472 }
473 if (rank_max < 6) {
474 if (verbose) {
475 vpTRACE("Only %d pose parameters can be estimated.", rank_max);
476 }
477 }
478
479 if (computeCovariance)
480 covarianceMatrix = vpMatrix::computeCovarianceMatrix(L, v, -lambda * err);
481
482 } catch (...) {
483 vpERROR_TRACE("vpPoseFeatures::computePoseVVS");
484 throw;
485 }
486}
487
494void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo)
495{
496 try {
497 double residu_1 = 1e8;
498 double r = 1e8 - 1;
499
500 // we stop the minimization when the error is bellow 1e-8
501 vpMatrix L, W;
502 vpColVector w, res;
503 vpColVector v;
504 vpColVector error; // error vector
505
506 vpRobust robust;
507 robust.setMinMedianAbsoluteDeviation(0.00001);
508
509 unsigned int rank_max = 0;
510 unsigned int iter = 0;
511
512 // while((int)((residu_1 - r)*1e12) !=0)
513 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
514 residu_1 = r;
515
516 // Compute the interaction matrix and the error
517 error_and_interaction(cMo, error, L);
518
519 // compute the residual
520 r = error.sumSquare();
521
522 if (iter == 0) {
523 res.resize(error.getRows() / 2);
524 w.resize(error.getRows() / 2);
525 W.resize(error.getRows(), error.getRows());
526 w = 1;
527 }
528
529 for (unsigned int k = 0; k < error.getRows() / 2; k++) {
530 res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
531 }
532 robust.MEstimator(vpRobust::TUKEY, res, w);
533
534 // compute the pseudo inverse of the interaction matrix
535 for (unsigned int k = 0; k < error.getRows() / 2; k++) {
536 W[2 * k][2 * k] = w[k];
537 W[2 * k + 1][2 * k + 1] = w[k];
538 }
539 // compute the pseudo inverse of the interaction matrix
540 vpMatrix Lp;
541 vpMatrix LRank;
542 (W * L).pseudoInverse(Lp, 1e-6);
543 unsigned int rank = L.pseudoInverse(LRank, 1e-6);
544
545 if (rank_max < rank)
546 rank_max = rank;
547
548 // compute the VVS control law
549 v = -lambda * Lp * W * error;
550
551 cMo = vpExponentialMap::direct(v).inverse() * cMo;
552 if (iter++ > vvsIterMax) {
553 vpTRACE("Max iteration reached");
554 break;
555 }
556 }
557
558 if (rank_max < 6) {
559 if (verbose) {
560 vpTRACE("Only %d pose parameters can be estimated.", rank_max);
561 }
562 }
563
564 if (computeCovariance)
565 covarianceMatrix =
566 vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
567 // matrix is diagonale, but using W*W
568 // is more efficient.
569 } catch (...) {
570 vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS");
571 throw;
572 }
573}
574
575#endif //#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition vpCircle.h:87
Implementation of column vector and the associated operations.
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition vpCylinder.h:98
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:100
static double sqr(double x)
Definition vpMath.h:124
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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:77
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:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
Class that defines a 3D sphere in the object frame and allows forward projection of a 3D sphere in th...
Definition vpSphere.h:78
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388