Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
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 #if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
36 
38  : m_maxSize(0), m_totalSize(0), m_vvsIterMax(200), m_lambda(1.0), m_verbose(false), m_computeCovariance(false),
39  m_covarianceMatrix(), m_featurePoint_Point_list(), m_featurePoint3D_Point_list(), m_featureVanishingPoint_Point_list(),
40  m_featureVanishingPoint_DuoLine_list(), m_featureEllipse_Sphere_list(), m_featureEllipse_Circle_list(),
41  m_featureLine_Line_list(), m_featureLine_DuoLineInt_List(), m_featureSegment_DuoPoints_list()
42 { }
43 
45 
47 {
48  for (int i = (int)m_featurePoint_Point_list.size() - 1; i >= 0; i--)
49  delete m_featurePoint_Point_list[(unsigned int)i].desiredFeature;
50  m_featurePoint_Point_list.clear();
51 
52  for (int i = (int)m_featurePoint3D_Point_list.size() - 1; i >= 0; i--)
53  delete m_featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
54  m_featurePoint3D_Point_list.clear();
55 
56  for (int i = (int)m_featureVanishingPoint_Point_list.size() - 1; i >= 0; i--)
57  delete m_featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
58  m_featureVanishingPoint_Point_list.clear();
59 
60  for (int i = (int)m_featureVanishingPoint_DuoLine_list.size() - 1; i >= 0; i--)
61  delete m_featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
62  m_featureVanishingPoint_DuoLine_list.clear();
63 
64  for (int i = (int)m_featureEllipse_Sphere_list.size() - 1; i >= 0; i--)
65  delete m_featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
66  m_featureEllipse_Sphere_list.clear();
67 
68  for (int i = (int)m_featureEllipse_Circle_list.size() - 1; i >= 0; i--)
69  delete m_featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
70  m_featureEllipse_Circle_list.clear();
71 
72  for (int i = (int)m_featureLine_Line_list.size() - 1; i >= 0; i--)
73  delete m_featureLine_Line_list[(unsigned int)i].desiredFeature;
74  m_featureLine_Line_list.clear();
75 
76  for (int i = (int)m_featureLine_DuoLineInt_List.size() - 1; i >= 0; i--)
77  delete m_featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
78  m_featureLine_DuoLineInt_List.clear();
79 
80  for (int i = (int)m_featureSegment_DuoPoints_list.size() - 1; i >= 0; i--)
81  delete m_featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
82  m_featureSegment_DuoPoints_list.clear();
83 
84  for (int i = (int)m_featureSpecific_list.size() - 1; i >= 0; i--)
85  delete m_featureSpecific_list[(unsigned int)i];
86  m_featureSpecific_list.clear();
87 
88  m_maxSize = 0;
89  m_totalSize = 0;
90 }
91 
93 {
94  m_featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint, vpPoint>());
95  m_featurePoint_Point_list.back().firstParam = p;
96  m_featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
97  vpFeatureBuilder::create(*m_featurePoint_Point_list.back().desiredFeature, p);
98 
99  m_totalSize++;
100  if (m_featurePoint_Point_list.size() > m_maxSize)
101  m_maxSize = (unsigned int)m_featurePoint_Point_list.size();
102 }
103 
105 {
106  m_featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D, vpPoint>());
107  m_featurePoint3D_Point_list.back().firstParam = p;
108  m_featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
109  vpFeatureBuilder::create(*m_featurePoint3D_Point_list.back().desiredFeature, p);
110 
111  m_totalSize++;
112  if (m_featurePoint3D_Point_list.size() > m_maxSize)
113  m_maxSize = (unsigned int)m_featurePoint3D_Point_list.size();
114 }
115 
117 {
118  m_featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint, vpPoint>());
119  m_featureVanishingPoint_Point_list.back().firstParam = p;
120  m_featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
121  vpFeatureBuilder::create(*m_featureVanishingPoint_Point_list.back().desiredFeature, p);
122 
123  m_totalSize++;
124  if (m_featureVanishingPoint_Point_list.size() > m_maxSize)
125  m_maxSize = (unsigned int)m_featureVanishingPoint_Point_list.size();
126 }
127 
129 {
130  m_featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint, vpLine, vpLine>());
131  m_featureVanishingPoint_DuoLine_list.back().firstParam = l1;
132  m_featureVanishingPoint_DuoLine_list.back().secondParam = l2;
133  m_featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
134  vpFeatureBuilder::create(*m_featureVanishingPoint_DuoLine_list.back().desiredFeature, l1, l2);
135 
136  m_totalSize++;
137  if (m_featureVanishingPoint_DuoLine_list.size() > m_maxSize)
138  m_maxSize = (unsigned int)m_featureVanishingPoint_DuoLine_list.size();
139 }
140 
142 {
143  m_featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse, vpSphere>());
144  m_featureEllipse_Sphere_list.back().firstParam = s;
145  m_featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
146  vpFeatureBuilder::create(*m_featureEllipse_Sphere_list.back().desiredFeature, s);
147 
148  m_totalSize++;
149  if (m_featureEllipse_Sphere_list.size() > m_maxSize)
150  m_maxSize = (unsigned int)m_featureEllipse_Sphere_list.size();
151 }
152 
154 {
155  m_featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse, vpCircle>());
156  m_featureEllipse_Circle_list.back().firstParam = c;
157  m_featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
158  vpFeatureBuilder::create(*m_featureEllipse_Circle_list.back().desiredFeature, c);
159 
160  m_totalSize++;
161  if (m_featureEllipse_Circle_list.size() > m_maxSize)
162  m_maxSize = (unsigned int)m_featureEllipse_Circle_list.size();
163 }
164 
166 {
167  m_featureLine_Line_list.push_back(vpDuo<vpFeatureLine, vpLine>());
168  m_featureLine_Line_list.back().firstParam = l;
169  m_featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
170  vpFeatureBuilder::create(*m_featureLine_Line_list.back().desiredFeature, l);
171 
172  m_totalSize++;
173  if (m_featureLine_Line_list.size() > m_maxSize)
174  m_maxSize = (unsigned int)m_featureLine_Line_list.size();
175 }
176 
177 void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
178 {
179  m_featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine, vpCylinder, int>());
180  m_featureLine_DuoLineInt_List.back().firstParam = c;
181  m_featureLine_DuoLineInt_List.back().secondParam = line;
182  m_featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
183  vpFeatureBuilder::create(*m_featureLine_DuoLineInt_List.back().desiredFeature, c, line);
184 
185  m_totalSize++;
186  if (m_featureLine_DuoLineInt_List.size() > m_maxSize)
187  m_maxSize = (unsigned int)m_featureLine_DuoLineInt_List.size();
188 }
189 
191 {
192  m_featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment, vpPoint, vpPoint>());
193  m_featureSegment_DuoPoints_list.back().firstParam = P1;
194  m_featureSegment_DuoPoints_list.back().secondParam = P2;
195  m_featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
196  vpFeatureBuilder::create(*m_featureSegment_DuoPoints_list.back().desiredFeature, P1, P2);
197 
198  m_totalSize++;
199  if (m_featureSegment_DuoPoints_list.size() > m_maxSize)
200  m_maxSize = (unsigned int)m_featureSegment_DuoPoints_list.size();
201 }
202 
203 void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L)
204 {
205  err = vpColVector();
206  L = vpMatrix();
207 
208  for (unsigned int i = 0; i < m_maxSize; i++) {
209  //--------------vpFeaturePoint--------------
210  // From vpPoint
211  if (i < m_featurePoint_Point_list.size()) {
212  vpFeaturePoint fp;
213  vpPoint p(m_featurePoint_Point_list[i].firstParam);
214  p.track(cMo);
216  err.stack(fp.error(*(m_featurePoint_Point_list[i].desiredFeature)));
217  L.stack(fp.interaction());
218  }
219 
220  //--------------vpFeaturePoint3D--------------
221  // From vpPoint
222  if (i < m_featurePoint3D_Point_list.size()) {
223  vpFeaturePoint3D fp3D;
224  vpPoint p(m_featurePoint3D_Point_list[i].firstParam);
225  p.track(cMo);
226  vpFeatureBuilder::create(fp3D, p);
227  err.stack(fp3D.error(*(m_featurePoint3D_Point_list[i].desiredFeature)));
228  L.stack(fp3D.interaction());
229  }
230 
231  //--------------vpFeatureVanishingPoint--------------
232  // From vpPoint
233  if (i < m_featureVanishingPoint_Point_list.size()) {
235  vpPoint p(m_featureVanishingPoint_Point_list[i].firstParam);
236  p.track(cMo);
237  vpFeatureBuilder::create(fvp, p);
238  err.stack(fvp.error(*(m_featureVanishingPoint_Point_list[i].desiredFeature)));
239  L.stack(fvp.interaction());
240  }
241  // From Duo of vpLines
242  if (i < m_featureVanishingPoint_DuoLine_list.size()) {
244  vpLine l1(m_featureVanishingPoint_DuoLine_list[i].firstParam);
245  vpLine l2(m_featureVanishingPoint_DuoLine_list[i].secondParam);
246  l1.track(cMo);
247  l2.track(cMo);
248  vpFeatureBuilder::create(fvp, l1, l2);
249  err.stack(fvp.error(*(m_featureVanishingPoint_DuoLine_list[i].desiredFeature)));
250  L.stack(fvp.interaction());
251  }
252 
253  //--------------vpFeatureEllipse--------------
254  // From vpSphere
255  if (i < m_featureEllipse_Sphere_list.size()) {
256  vpFeatureEllipse fe;
257  vpSphere s(m_featureEllipse_Sphere_list[i].firstParam);
258  s.track(cMo);
260  err.stack(fe.error(*(m_featureEllipse_Sphere_list[i].desiredFeature)));
261  L.stack(fe.interaction());
262  }
263  // From vpCircle
264  if (i < m_featureEllipse_Circle_list.size()) {
265  vpFeatureEllipse fe;
266  vpCircle c(m_featureEllipse_Circle_list[i].firstParam);
267  c.track(cMo);
269  err.stack(fe.error(*(m_featureEllipse_Circle_list[i].desiredFeature)));
270  L.stack(fe.interaction());
271  }
272 
273  //--------------vpFeatureLine--------------
274  // From vpLine
275  if (i < m_featureLine_Line_list.size()) {
276  vpFeatureLine fl;
277  vpLine l(m_featureLine_Line_list[i].firstParam);
278  l.track(cMo);
280  err.stack(fl.error(*(m_featureLine_Line_list[i].desiredFeature)));
281  L.stack(fl.interaction());
282  }
283  // From Duo of vpCylinder / Integer
284  if (i < m_featureLine_DuoLineInt_List.size()) {
285  vpFeatureLine fl;
286  vpCylinder c(m_featureLine_DuoLineInt_List[i].firstParam);
287  c.track(cMo);
288  vpFeatureBuilder::create(fl, c, m_featureLine_DuoLineInt_List[i].secondParam);
289  err.stack(fl.error(*(m_featureLine_DuoLineInt_List[i].desiredFeature)));
290  L.stack(fl.interaction());
291  }
292 
293  //--------------vpFeatureSegment--------------
294  // From Duo of vpPoints
295  if (i < m_featureSegment_DuoPoints_list.size()) {
296  vpFeatureSegment fs;
297  vpPoint p1(m_featureSegment_DuoPoints_list[i].firstParam);
298  vpPoint p2(m_featureSegment_DuoPoints_list[i].secondParam);
299  p1.track(cMo);
300  p2.track(cMo);
301  vpFeatureBuilder::create(fs, p1, p2);
302  err.stack(fs.error(*(m_featureSegment_DuoPoints_list[i].desiredFeature)));
303  L.stack(fs.interaction());
304  }
305 
306  //--------------Specific Feature--------------
307  if (i < m_featureSpecific_list.size()) {
308  m_featureSpecific_list[i]->createCurrent(cMo);
309  err.stack(m_featureSpecific_list[i]->error());
310  L.stack(m_featureSpecific_list[i]->currentInteraction());
311  }
312  }
313 }
314 
316 {
317  switch (type) {
318  case VIRTUAL_VS:
319  computePoseVVS(cMo);
320  break;
321  case ROBUST_VIRTUAL_VS:
322  computePoseRobustVVS(cMo);
323  break;
324  default:
325  break;
326  }
327 }
328 
329 void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix &cMo)
330 {
331  try {
332  double residu_1 = 1e8;
333  double r = 1e8 - 1;
334  // we stop the minimization when the error is bellow 1e-8
335 
336  vpMatrix L;
337  vpColVector err;
338  vpColVector v;
339  unsigned int rank_max = 0;
340  unsigned int iter = 0;
341 
342  // while((int)((residu_1 - r)*1e12) != 0 )
343  while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
344  residu_1 = r;
345 
346  // Compute the interaction matrix and the error
347  error_and_interaction(cMo, err, L);
348 
349  // compute the residual
350  r = err.sumSquare();
351 
352  // compute the pseudo inverse of the interaction matrix
353  vpMatrix Lp;
354  unsigned int rank = L.pseudoInverse(Lp, 1e-6); // modif FC 1e-16
355 
356  if (rank_max < rank)
357  rank_max = rank;
358 
359  // compute the VVS control law
360  v = -m_lambda * Lp * err;
361 
362  cMo = vpExponentialMap::direct(v).inverse() * cMo;
363  if (iter++ > m_vvsIterMax) {
364  vpTRACE("Max iteration reached");
365  break;
366  }
367  }
368  if (rank_max < 6) {
369  if (m_verbose) {
370  vpTRACE("Only %d pose parameters can be estimated.", rank_max);
371  }
372  }
373 
374  if (m_computeCovariance)
375  m_covarianceMatrix = vpMatrix::computeCovarianceMatrix(L, v, -m_lambda * err);
376 
377  }
378  catch (...) {
379  vpERROR_TRACE("vpPoseFeatures::computePoseVVS");
380  throw;
381  }
382 }
383 
384 void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo)
385 {
386  try {
387  double residu_1 = 1e8;
388  double r = 1e8 - 1;
389 
390  // we stop the minimization when the error is bellow 1e-8
391  vpMatrix L, W;
392  vpColVector w, res;
393  vpColVector v;
394  vpColVector error; // error vector
395 
396  vpRobust robust;
397  robust.setMinMedianAbsoluteDeviation(0.00001);
398 
399  unsigned int rank_max = 0;
400  unsigned int iter = 0;
401 
402  // while((int)((residu_1 - r)*1e12) !=0)
403  while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
404  residu_1 = r;
405 
406  // Compute the interaction matrix and the error
407  error_and_interaction(cMo, error, L);
408 
409  // compute the residual
410  r = error.sumSquare();
411 
412  if (iter == 0) {
413  res.resize(error.getRows() / 2);
414  w.resize(error.getRows() / 2);
415  W.resize(error.getRows(), error.getRows());
416  w = 1;
417  }
418 
419  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
420  res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
421  }
422  robust.MEstimator(vpRobust::TUKEY, res, w);
423 
424  // compute the pseudo inverse of the interaction matrix
425  for (unsigned int k = 0; k < error.getRows() / 2; k++) {
426  W[2 * k][2 * k] = w[k];
427  W[2 * k + 1][2 * k + 1] = w[k];
428  }
429  // compute the pseudo inverse of the interaction matrix
430  vpMatrix Lp;
431  vpMatrix LRank;
432  (W * L).pseudoInverse(Lp, 1e-6);
433  unsigned int rank = L.pseudoInverse(LRank, 1e-6);
434 
435  if (rank_max < rank)
436  rank_max = rank;
437 
438  // compute the VVS control law
439  v = -m_lambda * Lp * W * error;
440 
441  cMo = vpExponentialMap::direct(v).inverse() * cMo;
442  if (iter++ > m_vvsIterMax) {
443  vpTRACE("Max iteration reached");
444  break;
445  }
446  }
447 
448  if (rank_max < 6) {
449  if (m_verbose) {
450  vpTRACE("Only %d pose parameters can be estimated.", rank_max);
451  }
452  }
453 
454  if (m_computeCovariance)
455  m_covarianceMatrix =
456  vpMatrix::computeCovarianceMatrix(L, v, -m_lambda * error, W * W); // Remark: W*W = W*W.t() since the
457  // matrix is diagonal, but using W*W
458  // is more efficient.
459  }
460  catch (...) {
461  vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS");
462  throw;
463  }
464 }
465 #elif !defined(VISP_BUILD_SHARED_LIBS)
466 // Work around to avoid warning: libvisp_vision.a(vpPoseFeatures.cpp.o) has no symbols
467 void dummy_vpPoseFeatures() { };
468 #endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int getRows() const
Definition: vpArray2D.h:284
Class that defines a 3D circle in the object frame and allows forward projection of a 3D circle in th...
Definition: vpCircle.h:86
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
void stack(double d)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Definition: vpCylinder.h:99
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) vp_override
compute the interaction matrix from a subset a the possible features
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
Class that defines the 3D point visual feature.
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
Class that defines a 2D segment visual features. This class allow to consider two sets of visual feat...
vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL) vp_override
vpMatrix interaction(unsigned int select=FEATURE_ALL) vp_override
vpMatrix interaction(unsigned int select=(vpFeatureVanishingPoint::selectX()|vpFeatureVanishingPoint::selectY())) vp_override
vpColVector error(const vpBasicFeature &s_star, unsigned int select=(vpFeatureVanishingPoint::selectX()|vpFeatureVanishingPoint::selectY())) vp_override
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:101
static double sqr(double x)
Definition: vpMath.h:201
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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 addFeaturePoint3D(const vpPoint &p)
void addFeaturePoint(const vpPoint &p)
void addFeatureLine(const vpLine &)
virtual ~vpPoseFeatures()
void addFeatureEllipse(const vpCircle &)
void addFeatureVanishingPoint(const vpPoint &p)
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:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:136
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:156
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:405
#define vpERROR_TRACE
Definition: vpDebug.h:382