Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  * Make the complete tracking of an object by using its CAD model
33  *
34 *****************************************************************************/
35 
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpExponentialMap.h>
44 #include <visp3/core/vpMath.h>
45 #include <visp3/core/vpMatrixException.h>
46 #include <visp3/core/vpPixelMeterConversion.h>
47 #include <visp3/core/vpPolygon3D.h>
48 #include <visp3/core/vpTrackingException.h>
49 #include <visp3/core/vpVelocityTwistMatrix.h>
50 #include <visp3/mbt/vpMbEdgeTracker.h>
51 #include <visp3/mbt/vpMbtDistanceLine.h>
52 #include <visp3/mbt/vpMbtXmlGenericParser.h>
53 #include <visp3/vision/vpPose.h>
54 
55 #include <float.h>
56 #include <limits>
57 #include <map>
58 #include <sstream>
59 #include <string>
60 
65  : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
66  percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
67  m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
68  m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
69  m_robust_edge(), m_featuresToBeDisplayedEdge()
70 {
71  scales[0] = true;
72 
73 #ifdef VISP_HAVE_OGRE
74  faces.getOgreContext()->setWindowName("MBT Edge");
75 #endif
76 }
77 
82 {
86 
87  for (unsigned int i = 0; i < scales.size(); i += 1) {
88  if (scales[i]) {
89  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
90  l = *it;
91  if (l != nullptr) {
92  delete l;
93  }
94  l = nullptr;
95  }
96 
97  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
98  ++it) {
99  cy = *it;
100  if (cy != nullptr) {
101  delete cy;
102  }
103  cy = nullptr;
104  }
105 
106  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
107  ci = *it;
108  if (ci != nullptr) {
109  delete ci;
110  }
111  ci = nullptr;
112  }
113 
114  lines[i].clear();
115  cylinders[i].clear();
116  circles[i].clear();
117  }
118  }
119 
121 }
122 
129 {
130  this->me = p_me;
131 
132  for (unsigned int i = 0; i < scales.size(); i += 1) {
133  if (scales[i]) {
134  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
135  vpMbtDistanceLine *l = *it;
136  l->setMovingEdge(&(this->me));
137  }
138 
139  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
140  ++it) {
141  vpMbtDistanceCylinder *cy = *it;
142  cy->setMovingEdge(&(this->me));
143  }
144 
145  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
146  vpMbtDistanceCircle *ci = *it;
147  ci->setMovingEdge(&(this->me));
148  }
149  }
150  }
151 }
152 
162 void vpMbEdgeTracker::computeVVS(const vpImage<unsigned char> &_I, unsigned int lvl)
163 {
164  double residu_1 = 1e3;
165  double r = 1e3 - 1;
166 
167  unsigned int iter = 0;
168 
169  computeVVSInit();
170  unsigned int nbrow = m_error_edge.getRows();
171 
172  bool reloop = true;
173 
174  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
175  if (isoJoIdentity)
176  oJo.eye();
177 
178  /*** First phase ***/
179 
180  while (reloop == true && iter < 10) {
181  double count = 0;
182 
183  computeVVSFirstPhase(_I, iter, count, lvl);
184 
185  count = count / (double)nbrow;
186  if (count >= 0.85) {
187  reloop = false;
188  }
189 
190  computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity);
191 
192  iter++;
193  }
194 
195  // std::cout << "\t First minimization in " << iter << " iteration give as
196  // initial cMo: \n" << cMo << std::endl;
197 
198  /*** Second phase ***/
199  vpHomogeneousMatrix cMoPrev;
200  vpColVector W_true(nbrow);
201  vpMatrix L_true;
202  vpMatrix LVJ_true;
203 
204  double mu = m_initialMu;
205  vpColVector m_error_prev;
206  vpColVector m_w_prev;
207 
208  // To avoid to create these matrices each iteration
209  vpMatrix LTL;
210  vpColVector LTR;
211  vpColVector v;
212 
213  iter = 0;
214  m_w_edge = 1;
215 
216  // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
217  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
219 
220  bool reStartFromLastIncrement = false;
221  computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
222  &m_w_edge, &m_w_prev);
223 
224  if (!reStartFromLastIncrement) {
226 
227  L_true = m_L_edge;
229 
230  if (computeCovariance) {
231  L_true = m_L_edge;
232  if (!isoJoIdentity) {
233  cVo.build(m_cMo);
234  LVJ_true = (m_L_edge * cVo * oJo);
235  }
236  }
237 
238  double wi = 0.0, eri = 0.0;
239  double num = 0.0, den = 0.0;
240  if ((iter == 0) || m_computeInteraction) {
241  for (unsigned int i = 0; i < nbrow; i++) {
242  wi = m_w_edge[i] * m_factor[i];
243  W_true[i] = wi;
244  eri = m_error_edge[i];
245  num += wi * vpMath::sqr(eri);
246  den += wi;
247 
248  m_weightedError_edge[i] = wi * eri;
249 
250  for (unsigned int j = 0; j < 6; j++) {
251  m_L_edge[i][j] = wi * m_L_edge[i][j];
252  }
253  }
254  }
255  else {
256  for (unsigned int i = 0; i < nbrow; i++) {
257  wi = m_w_edge[i] * m_factor[i];
258  W_true[i] = wi;
259  eri = m_error_edge[i];
260  num += wi * vpMath::sqr(eri);
261  den += wi;
262 
263  m_weightedError_edge[i] = wi * eri;
264  }
265  }
266 
267  residu_1 = r;
268  r = sqrt(num / den); // Le critere d'arret prend en compte le poids
269 
270  computeVVSPoseEstimation(isoJoIdentity, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev,
271  LTR, mu, v, &m_w_edge, &m_w_prev);
272 
273  cMoPrev = m_cMo;
275 
276  } // endif(!restartFromLast)
277 
278  iter++;
279  }
280 
281  computeCovarianceMatrixVVS(isoJoIdentity, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);
282 
284 }
285 
286 void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_I, unsigned int iter, double &count,
287  unsigned int lvl)
288 {
292 
293  double limite = 3; // Une limite de 3 pixels
294  limite = limite / m_cam.get_px(); // Transformation limite pixel en limite metre.
295 
296  unsigned int n = 0;
297 
298  // Parametre pour la premiere phase d'asservissement
299  double e_prev = 0, e_cur, e_next;
300 
301  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
302  if ((*it)->isTracked()) {
303  l = *it;
305 
306  double fac = 1;
307  if (iter == 0) {
308  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
309  ++itindex) {
310  int index = *itindex;
311  if (l->hiddenface->isAppearing((unsigned int)index)) {
312  fac = 0.2;
313  break;
314  }
315  if (l->closeToImageBorder(_I, 10)) {
316  fac = 0.1;
317  break;
318  }
319  }
320  }
321 
322  std::list<vpMeSite>::const_iterator itListLine;
323 
324  unsigned int indexFeature = 0;
325 
326  for (size_t a = 0; a < l->meline.size(); a++) {
327  if (iter == 0 && l->meline[a] != nullptr)
328  itListLine = l->meline[a]->getMeList().begin();
329 
330  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
331  for (unsigned int j = 0; j < 6; j++) {
332  m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale
333  }
334  m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur
335 
336  if (m_error_edge[n + i] <= limite)
337  count = count + 1.0; // Si erreur proche de 0 on incremente cur
338 
339  m_w_edge[n + i] = 0;
340 
341  if (iter == 0) {
342  m_factor[n + i] = fac;
343  vpMeSite site = *itListLine;
344  if (site.getState() != vpMeSite::NO_SUPPRESSION)
345  m_factor[n + i] = 0.2;
346  ++itListLine;
347  }
348 
349  // If pour la premiere extremite des moving edges
350  if (indexFeature == 0) {
351  e_cur = l->error[0];
352  if (l->nbFeature[a] > 1) {
353  e_next = l->error[1];
354  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
355  m_w_edge[n + i] = 1 /*0.5*/;
356  }
357  e_prev = e_cur;
358  }
359  else
360  m_w_edge[n + i] = 1;
361  }
362 
363  // If pour la derniere extremite des moving edges
364  else if (indexFeature == l->nbFeatureTotal - 1) {
365  e_cur = l->error[indexFeature];
366  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
367  m_w_edge[n + i] += 1 /*0.5*/;
368  }
369  }
370 
371  else {
372  e_cur = l->error[indexFeature];
373  e_next = l->error[indexFeature + 1];
374  if (fabs(e_cur - e_prev) < limite) {
375  m_w_edge[n + i] += 0.5;
376  }
377  if (fabs(e_cur - e_next) < limite) {
378  m_w_edge[n + i] += 0.5;
379  }
380  e_prev = e_cur;
381  }
382  indexFeature++;
383  }
384  n += l->nbFeature[a];
385  }
386  }
387  }
388 
389  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
390  ++it) {
391  if ((*it)->isTracked()) {
392  cy = *it;
394  double fac = 1.0;
395 
396  std::list<vpMeSite>::const_iterator itCyl1;
397  std::list<vpMeSite>::const_iterator itCyl2;
398  if (iter == 0 && (cy->meline1 != nullptr || cy->meline2 != nullptr)) {
399  itCyl1 = cy->meline1->getMeList().begin();
400  itCyl2 = cy->meline2->getMeList().begin();
401  }
402 
403  for (unsigned int i = 0; i < cy->nbFeature; i++) {
404  for (unsigned int j = 0; j < 6; j++) {
405  m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale
406  }
407  m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur
408 
409  if (m_error_edge[n + i] <= limite)
410  count = count + 1.0; // Si erreur proche de 0 on incremente cur
411 
412  m_w_edge[n + i] = 0;
413 
414  if (iter == 0) {
415  m_factor[n + i] = fac;
416  vpMeSite site;
417  if (i < cy->nbFeaturel1) {
418  site = *itCyl1;
419  ++itCyl1;
420  }
421  else {
422  site = *itCyl2;
423  ++itCyl2;
424  }
425  if (site.getState() != vpMeSite::NO_SUPPRESSION)
426  m_factor[n + i] = 0.2;
427  }
428 
429  // If pour la premiere extremite des moving edges
430  if (i == 0) {
431  e_cur = cy->error[0];
432  if (cy->nbFeature > 1) {
433  e_next = cy->error[1];
434  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
435  m_w_edge[n + i] = 1 /*0.5*/;
436  }
437  e_prev = e_cur;
438  }
439  else
440  m_w_edge[n + i] = 1;
441  }
442  if (i == cy->nbFeaturel1) {
443  e_cur = cy->error[i];
444  if (cy->nbFeaturel2 > 1) {
445  e_next = cy->error[i + 1];
446  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
447  m_w_edge[n + i] = 1 /*0.5*/;
448  }
449  e_prev = e_cur;
450  }
451  else
452  m_w_edge[n + i] = 1;
453  }
454 
455  // If pour la derniere extremite des moving edges
456  else if (i == cy->nbFeaturel1 - 1) {
457  e_cur = cy->error[i];
458  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
459  m_w_edge[n + i] += 1 /*0.5*/;
460  }
461  }
462  // If pour la derniere extremite des moving edges
463  else if (i == cy->nbFeature - 1) {
464  e_cur = cy->error[i];
465  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
466  m_w_edge[n + i] += 1 /*0.5*/;
467  }
468  }
469 
470  else {
471  e_cur = cy->error[i];
472  e_next = cy->error[i + 1];
473  if (fabs(e_cur - e_prev) < limite) {
474  m_w_edge[n + i] += 0.5;
475  }
476  if (fabs(e_cur - e_next) < limite) {
477  m_w_edge[n + i] += 0.5;
478  }
479  e_prev = e_cur;
480  }
481  }
482 
483  n += cy->nbFeature;
484  }
485  }
486 
487  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
488  if ((*it)->isTracked()) {
489  ci = *it;
491  double fac = 1.0;
492 
493  std::list<vpMeSite>::const_iterator itCir;
494  if (iter == 0 && (ci->meEllipse != nullptr)) {
495  itCir = ci->meEllipse->getMeList().begin();
496  }
497 
498  for (unsigned int i = 0; i < ci->nbFeature; i++) {
499  for (unsigned int j = 0; j < 6; j++) {
500  m_L_edge[n + i][j] = ci->L[i][j]; // On remplit la matrice d'interaction globale
501  }
502  m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur
503 
504  if (m_error_edge[n + i] <= limite)
505  count = count + 1.0; // Si erreur proche de 0 on incremente cur
506 
507  m_w_edge[n + i] = 0;
508 
509  if (iter == 0) {
510  m_factor[n + i] = fac;
511  vpMeSite site = *itCir;
512  if (site.getState() != vpMeSite::NO_SUPPRESSION)
513  m_factor[n + i] = 0.2;
514  ++itCir;
515  }
516 
517  // If pour la premiere extremite des moving edges
518  if (i == 0) {
519  e_cur = ci->error[0];
520  if (ci->nbFeature > 1) {
521  e_next = ci->error[1];
522  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
523  m_w_edge[n + i] = 1 /*0.5*/;
524  }
525  e_prev = e_cur;
526  }
527  else
528  m_w_edge[n + i] = 1;
529  }
530 
531  // If pour la derniere extremite des moving edges
532  else if (i == ci->nbFeature - 1) {
533  e_cur = ci->error[i];
534  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
535  m_w_edge[n + i] += 1 /*0.5*/;
536  }
537  }
538 
539  else {
540  e_cur = ci->error[i];
541  e_next = ci->error[i + 1];
542  if (fabs(e_cur - e_prev) < limite) {
543  m_w_edge[n + i] += 0.5;
544  }
545  if (fabs(e_cur - e_next) < limite) {
546  m_w_edge[n + i] += 0.5;
547  }
548  e_prev = e_cur;
549  }
550  }
551 
552  n += ci->nbFeature;
553  }
554  }
555 }
556 
558 {
562 
563  unsigned int n = 0;
564  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
565  if ((*it)->isTracked()) {
566  l = *it;
568 
569  double fac = 1;
570  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
571  ++itindex) {
572  int index = *itindex;
573  if (l->hiddenface->isAppearing((unsigned int)index)) {
574  fac = 0.2;
575  break;
576  }
577  if (l->closeToImageBorder(I, 10)) {
578  fac = 0.1;
579  break;
580  }
581  }
582 
583  for (size_t a = 0; a < l->meline.size(); a++) {
584  std::list<vpMeSite>::const_iterator itListLine;
585  if (l->meline[a] != nullptr) {
586  itListLine = l->meline[a]->getMeList().begin();
587 
588  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
589  m_factor[n + i] = fac;
590  vpMeSite site = *itListLine;
591  if (site.getState() != vpMeSite::NO_SUPPRESSION)
592  m_factor[n + i] = 0.2;
593  ++itListLine;
594  }
595  n += l->nbFeature[a];
596  }
597  }
598  }
599  }
600 
601  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
602  ++it) {
603  if ((*it)->isTracked()) {
604  cy = *it;
606 
607  std::list<vpMeSite>::const_iterator itCyl1;
608  std::list<vpMeSite>::const_iterator itCyl2;
609  if ((cy->meline1 != nullptr || cy->meline2 != nullptr)) {
610  itCyl1 = cy->meline1->getMeList().begin();
611  itCyl2 = cy->meline2->getMeList().begin();
612 
613  double fac = 1.0;
614  for (unsigned int i = 0; i < cy->nbFeature; i++) {
615  m_factor[n + i] = fac;
616  vpMeSite site;
617  if (i < cy->nbFeaturel1) {
618  site = *itCyl1;
619  ++itCyl1;
620  }
621  else {
622  site = *itCyl2;
623  ++itCyl2;
624  }
625  if (site.getState() != vpMeSite::NO_SUPPRESSION)
626  m_factor[n + i] = 0.2;
627  }
628  n += cy->nbFeature;
629  }
630  }
631  }
632 
633  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
634  if ((*it)->isTracked()) {
635  ci = *it;
637 
638  std::list<vpMeSite>::const_iterator itCir;
639  if (ci->meEllipse != nullptr) {
640  itCir = ci->meEllipse->getMeList().begin();
641  double fac = 1.0;
642 
643  for (unsigned int i = 0; i < ci->nbFeature; i++) {
644  m_factor[n + i] = fac;
645  vpMeSite site = *itCir;
646  if (site.getState() != vpMeSite::NO_SUPPRESSION)
647  m_factor[n + i] = 0.2;
648  ++itCir;
649  }
650  n += ci->nbFeature;
651  }
652  }
653  }
654 }
655 
656 void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
657 {
658  unsigned int nerror = m_weightedError_edge.getRows();
659 
660  double wi, eri;
661  if ((iter == 0) || m_computeInteraction) {
662  for (unsigned int i = 0; i < nerror; i++) {
663  wi = m_w_edge[i] * m_factor[i];
664  eri = m_error_edge[i];
665 
666  m_weightedError_edge[i] = wi * eri;
667 
668  for (unsigned int j = 0; j < 6; j++) {
669  m_L_edge[i][j] = wi * m_L_edge[i][j];
670  }
671  }
672  }
673  else {
674  for (unsigned int i = 0; i < nerror; i++) {
675  wi = m_w_edge[i] * m_factor[i];
676  eri = m_error_edge[i];
677 
678  m_weightedError_edge[i] = wi * eri;
679  }
680  }
681 
683 
684  // If all the 6 dof should be estimated, we check if the interaction matrix
685  // is full rank. If not we remove automatically the dof that cannot be
686  // estimated This is particularly useful when considering circles (rank 5) and
687  // cylinders (rank 4)
688  if (isoJoIdentity) {
689  cVo.build(m_cMo);
690 
691  vpMatrix K; // kernel
692  unsigned int rank = (m_L_edge * cVo).kernel(K);
693  if (rank == 0) {
694  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
695  }
696  if (rank != 6) {
697  vpMatrix I; // Identity
698  I.eye(6);
699  oJo = I - K.AtA();
700 
701  isoJoIdentity = false;
702  }
703  }
704 
705  vpColVector v;
706  vpMatrix LTL;
707  vpColVector LTR;
708 
709  if (isoJoIdentity) {
710  LTL = m_L_edge.AtA();
712  v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
713  }
714  else {
715  cVo.build(m_cMo);
716  vpMatrix LVJ = (m_L_edge * cVo * oJo);
717  vpMatrix LVJTLVJ = (LVJ).AtA();
718  vpColVector LVJTR;
719  computeJTR(LVJ, m_weightedError_edge, LVJTR);
720  v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
721  v = cVo * v;
722  }
723 
725 }
726 
728 {
729  // Nombre de moving edges
730  unsigned int nbrow = 0;
731  unsigned int nberrors_lines = 0;
732  unsigned int nberrors_cylinders = 0;
733  unsigned int nberrors_circles = 0;
734 
735  nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
736 
737  if (nbrow == 0) {
739  "No data found to compute the interaction matrix...");
740  }
741 
742  m_L_edge.resize(nbrow, 6, false, false);
743  m_error_edge.resize(nbrow, false);
744 
745  m_weightedError_edge.resize(nbrow, false);
746  m_w_edge.resize(nbrow, false);
747  m_w_edge = 1;
748  m_factor.resize(nbrow, false);
749  m_factor = 1;
750 
754 
755  m_wLines.resize(nberrors_lines, false);
756  m_wLines = 1;
757  m_wCylinders.resize(nberrors_cylinders, false);
758  m_wCylinders = 1;
759  m_wCircles.resize(nberrors_circles, false);
760  m_wCircles = 1;
761 
762  m_errorLines.resize(nberrors_lines, false);
763  m_errorCylinders.resize(nberrors_cylinders, false);
764  m_errorCircles.resize(nberrors_circles, false);
765 }
766 
768 {
769  throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
770  "computeVVSInteractionMatrixAndR"
771  "esidu() should not be called!");
772 }
773 
775 {
779 
780  unsigned int n = 0;
781  unsigned int nlines = 0;
782  unsigned int ncylinders = 0;
783  unsigned int ncircles = 0;
784 
785  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
786  ++it) {
787  if ((*it)->isTracked()) {
788  l = *it;
790  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
791  for (unsigned int j = 0; j < 6; j++) {
792  m_L_edge[n + i][j] = l->L[i][j];
793  m_error_edge[n + i] = l->error[i];
794  m_errorLines[nlines + i] = m_error_edge[n + i];
795  }
796  }
797  n += l->nbFeatureTotal;
798  nlines += l->nbFeatureTotal;
799  }
800  }
801 
802  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
803  it != cylinders[scaleLevel].end(); ++it) {
804  if ((*it)->isTracked()) {
805  cy = *it;
807  for (unsigned int i = 0; i < cy->nbFeature; i++) {
808  for (unsigned int j = 0; j < 6; j++) {
809  m_L_edge[n + i][j] = cy->L[i][j];
810  m_error_edge[n + i] = cy->error[i];
811  m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
812  }
813  }
814 
815  n += cy->nbFeature;
816  ncylinders += cy->nbFeature;
817  }
818  }
819 
820  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
821  it != circles[scaleLevel].end(); ++it) {
822  if ((*it)->isTracked()) {
823  ci = *it;
825  for (unsigned int i = 0; i < ci->nbFeature; i++) {
826  for (unsigned int j = 0; j < 6; j++) {
827  m_L_edge[n + i][j] = ci->L[i][j];
828  m_error_edge[n + i] = ci->error[i];
829  m_errorCircles[ncircles + i] = m_error_edge[n + i];
830  }
831  }
832 
833  n += ci->nbFeature;
834  ncircles += ci->nbFeature;
835  }
836  }
837 }
838 
840 {
841  unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
842  nberrors_circles = m_errorCircles.getRows();
843 
844  if (nberrors_lines > 0)
846  if (nberrors_cylinders > 0)
848  if (nberrors_circles > 0)
850 
854 }
855 
865 {
866  projectionError = 0.0;
867  unsigned int nbFeatures = 0;
868  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
869  ++it) {
870  vpMbtDistanceLine *l = *it;
871  if (l->isVisible() && l->isTracked()) {
872  for (size_t a = 0; a < l->meline.size(); a++) {
873  if (l->meline[a] != nullptr) {
874  double lineNormGradient;
875  unsigned int lineNbFeatures;
876  l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
879  projectionError += lineNormGradient;
880  nbFeatures += lineNbFeatures;
881  }
882  }
883  }
884  }
885 
886  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
887  it != cylinders[scaleLevel].end(); ++it) {
888  vpMbtDistanceCylinder *cy = *it;
889  if (cy->isVisible() && cy->isTracked()) {
890  if (cy->meline1 != nullptr) {
891  double cylinderNormGradient = 0;
892  unsigned int cylinderNbFeatures = 0;
893  cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
896  projectionError += cylinderNormGradient;
897  nbFeatures += cylinderNbFeatures;
898  }
899 
900  if (cy->meline2 != nullptr) {
901  double cylinderNormGradient = 0;
902  unsigned int cylinderNbFeatures = 0;
903  cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
906  projectionError += cylinderNormGradient;
907  nbFeatures += cylinderNbFeatures;
908  }
909  }
910  }
911 
912  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
913  it != circles[scaleLevel].end(); ++it) {
914  vpMbtDistanceCircle *c = *it;
915  if (c->isVisible() && c->isTracked() && c->meEllipse != nullptr) {
916  double circleNormGradient = 0;
917  unsigned int circleNbFeatures = 0;
918  c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
921  projectionError += circleNormGradient;
922  nbFeatures += circleNbFeatures;
923  }
924  }
925 
926  if (nbFeatures > 0) {
927  projectionError = vpMath::deg(projectionError / (double)nbFeatures);
928  }
929  else {
930  projectionError = 90.0;
931  }
932 
934  // std::cout << "Norm Gradient = " << errorGradient << std::endl;
935 }
936 
943 {
944  int nbExpectedPoint = 0;
945  int nbGoodPoint = 0;
946  int nbBadPoint = 0;
947 
948  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
949  ++it) {
950  vpMbtDistanceLine *l = *it;
951  if (l->isVisible() && l->isTracked()) {
952  for (size_t a = 0; a < l->meline.size(); a++) {
953  if (l->meline[a] != nullptr) {
954  nbExpectedPoint += (int)l->meline[a]->expecteddensity;
955  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
956  itme != l->meline[a]->getMeList().end(); ++itme) {
957  vpMeSite pix = *itme;
958  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
959  nbGoodPoint++;
960  else
961  nbBadPoint++;
962  }
963  }
964  }
965  }
966  }
967 
968  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
969  it != cylinders[scaleLevel].end(); ++it) {
970  vpMbtDistanceCylinder *cy = *it;
971  if ((cy->meline1 != nullptr && cy->meline2 != nullptr) && cy->isVisible() && cy->isTracked()) {
972  nbExpectedPoint += (int)cy->meline1->expecteddensity;
973  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
974  itme1 != cy->meline1->getMeList().end(); ++itme1) {
975  vpMeSite pix = *itme1;
976  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
977  nbGoodPoint++;
978  else
979  nbBadPoint++;
980  }
981  nbExpectedPoint += (int)cy->meline2->expecteddensity;
982  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
983  itme2 != cy->meline2->getMeList().end(); ++itme2) {
984  vpMeSite pix = *itme2;
985  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
986  nbGoodPoint++;
987  else
988  nbBadPoint++;
989  }
990  }
991  }
992 
993  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
994  it != circles[scaleLevel].end(); ++it) {
995  vpMbtDistanceCircle *ci = *it;
996  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != nullptr) {
997  nbExpectedPoint += ci->meEllipse->getExpectedDensity();
998  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
999  itme != ci->meEllipse->getMeList().end(); ++itme) {
1000  vpMeSite pix = *itme;
1001  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
1002  nbGoodPoint++;
1003  else
1004  nbBadPoint++;
1005  }
1006  }
1007  }
1008 
1009  // Compare the number of good points with the min between the number of
1010  // expected points and number of points that are tracked
1011  int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
1012  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1013  std::ostringstream oss;
1014  oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
1015  << ". Try to reduce the threshold=" << percentageGdPt
1016  << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1018  }
1019 }
1020 
1029 {
1030  initPyramid(I, Ipyramid);
1031 
1032  unsigned int lvl = (unsigned int)scales.size();
1033  do {
1034  lvl--;
1035 
1036  projectionError = 90.0;
1037 
1038  if (scales[lvl]) {
1039  vpHomogeneousMatrix cMo_1 = m_cMo;
1040  try {
1041  downScale(lvl);
1042 
1043  try {
1044  trackMovingEdge(*Ipyramid[lvl]);
1045  }
1046  catch (...) {
1047  vpTRACE("Error in moving edge tracking");
1048  throw;
1049  }
1050 
1051  // initialize the vector that contains the error and the matrix that
1052  // contains the interaction matrix AY: Useless as it is done in
1053  // coputeVVS()
1054  /*
1055  for(std::list<vpMbtDistanceLine*>::const_iterator
1056  it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
1057  (l->isVisible()){ l->initInteractionMatrixError();
1058  }
1059  }
1060 
1061  for(std::list<vpMbtDistanceCylinder*>::const_iterator
1062  it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
1063  if(cy->isVisible()) {
1064  cy->initInteractionMatrixError();
1065  }
1066  }
1067 
1068  for(std::list<vpMbtDistanceCircle*>::const_iterator
1069  it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
1070  (ci->isVisible()){ ci->initInteractionMatrixError();
1071  }
1072  }
1073  */
1074 
1075  try {
1076  computeVVS(*Ipyramid[lvl], lvl);
1077  }
1078  catch (...) {
1079  covarianceMatrix = -1;
1080  throw; // throw the original exception
1081  }
1082 
1083  testTracking();
1084 
1085  if (displayFeatures) {
1087  }
1088 
1089  // Looking for new visible face
1090  bool newvisibleface = false;
1091  visibleFace(I, m_cMo, newvisibleface);
1092 
1093  // cam.computeFov(I.getWidth(), I.getHeight());
1094  if (useScanLine) {
1097  }
1098 
1099  updateMovingEdge(I);
1100 
1101  initMovingEdge(I, m_cMo);
1102  // Reinit the moving edge for the lines which need it.
1103  reinitMovingEdge(I, m_cMo);
1104 
1105  if (computeProjError)
1107 
1108  upScale(lvl);
1109  }
1110  catch (const vpException &e) {
1111  if (lvl != 0) {
1112  m_cMo = cMo_1;
1113  reInitLevel(lvl);
1114  upScale(lvl);
1115  }
1116  else {
1117  upScale(lvl);
1118  throw(e);
1119  }
1120  }
1121  }
1122  } while (lvl != 0);
1123 
1125 }
1126 
1128 {
1130  track(m_I);
1131 }
1132 
1139 {
1140  if (!modelInitialised) {
1141  throw vpException(vpException::fatalError, "model not initialized");
1142  }
1143 
1144  bool a = false;
1145 
1146 #ifdef VISP_HAVE_OGRE
1147  if (useOgre) {
1148  if (!faces.isOgreInitialised()) {
1151  faces.initOgre(m_cam);
1152  // Turn off Ogre config dialog display for the next call to this
1153  // function since settings are saved in the ogre.cfg file and used
1154  // during the next call
1155  ogreShowConfigDialog = false;
1156  }
1157  }
1158 #endif
1159 
1160  if (clippingFlag > 2)
1161  m_cam.computeFov(I.getWidth(), I.getHeight());
1162 
1163  visibleFace(I, m_cMo, a);
1164  resetMovingEdge();
1165 
1166  if (useScanLine) {
1167  if (clippingFlag <= 2)
1168  m_cam.computeFov(I.getWidth(), I.getHeight());
1169 
1172  }
1173 
1174  initPyramid(I, Ipyramid);
1175  unsigned int i = (unsigned int)scales.size();
1176  do {
1177  i--;
1178  if (scales[i]) {
1179  downScale(i);
1181  upScale(i);
1182  }
1183  } while (i != 0);
1184 
1186 }
1187 
1196 {
1197  m_cMo = cdMo;
1198 
1199  init(I);
1200 }
1201 
1210 {
1211  m_cMo = cdMo;
1212 
1213  vpImageConvert::convert(I_color, m_I);
1214  init(m_I);
1215 }
1216 
1228 void vpMbEdgeTracker::loadConfigFile(const std::string &configFile, bool verbose)
1229 {
1230 #if defined(VISP_HAVE_PUGIXML)
1231 // Load projection error config
1232  vpMbTracker::loadConfigFile(configFile, verbose);
1233 
1235  xmlp.setVerbose(verbose);
1236  xmlp.setCameraParameters(m_cam);
1239  xmlp.setEdgeMe(me);
1240 
1241  try {
1242  if (verbose) {
1243  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1244  }
1245  xmlp.parse(configFile);
1246  }
1247  catch (...) {
1248  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
1249  }
1250 
1251  vpCameraParameters camera;
1252  vpMe meParser;
1253  xmlp.getCameraParameters(camera);
1254  xmlp.getEdgeMe(meParser);
1255 
1256  setCameraParameters(camera);
1257  setMovingEdge(meParser);
1260 
1261  if (xmlp.hasNearClippingDistance())
1263 
1264  if (xmlp.hasFarClippingDistance())
1266 
1267  if (xmlp.getFovClipping())
1269 
1270  useLodGeneral = xmlp.getLodState();
1273 
1274  applyLodSettingInConfig = false;
1275  if (this->getNbPolygon() > 0) {
1276  applyLodSettingInConfig = true;
1280  }
1281 #else
1282  (void)configFile;
1283  (void)verbose;
1284  throw(vpException(vpException::ioError, "vpMbEdgeTracker::loadConfigFile() needs pugixml built-in 3rdparty"));
1285 #endif
1286 }
1287 
1300  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1301  bool displayFullModel)
1302 {
1303  // Display first the Moving-Edges
1304  if (displayFeatures) {
1306  }
1307 
1308  std::vector<std::vector<double> > models =
1309  vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1310 
1311  for (size_t i = 0; i < models.size(); i++) {
1312  if (vpMath::equal(models[i][0], 0)) {
1313  vpImagePoint ip1(models[i][1], models[i][2]);
1314  vpImagePoint ip2(models[i][3], models[i][4]);
1315  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1316  }
1317  else if (vpMath::equal(models[i][0], 1)) {
1318  vpImagePoint center(models[i][1], models[i][2]);
1319  double n20 = models[i][3];
1320  double n11 = models[i][4];
1321  double n02 = models[i][5];
1322  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1323  }
1324  }
1325 
1326 #ifdef VISP_HAVE_OGRE
1327  if (useOgre)
1328  faces.displayOgre(cMo);
1329 #endif
1330 }
1331 
1344  const vpColor &col, unsigned int thickness, bool displayFullModel)
1345 {
1346  // Display first the Moving-Edges
1347  if (displayFeatures) {
1349  }
1350 
1351  std::vector<std::vector<double> > models =
1352  vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1353 
1354  for (size_t i = 0; i < models.size(); i++) {
1355  if (vpMath::equal(models[i][0], 0)) {
1356  vpImagePoint ip1(models[i][1], models[i][2]);
1357  vpImagePoint ip2(models[i][3], models[i][4]);
1358  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1359  }
1360  else if (vpMath::equal(models[i][0], 1)) {
1361  vpImagePoint center(models[i][1], models[i][2]);
1362  double n20 = models[i][3];
1363  double n11 = models[i][4];
1364  double n02 = models[i][5];
1365  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1366  }
1367  }
1368 
1369 #ifdef VISP_HAVE_OGRE
1370  if (useOgre)
1371  faces.displayOgre(cMo);
1372 #endif
1373 }
1374 
1375 std::vector<std::vector<double> > vpMbEdgeTracker::getFeaturesForDisplayEdge()
1376 {
1377  std::vector<std::vector<double> > features;
1378 
1379  const unsigned int lvl = 0;
1380  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1381  vpMbtDistanceLine *l = *it;
1382  if (l->isVisible() && l->isTracked()) {
1383  std::vector<std::vector<double> > currentFeatures = l->getFeaturesForDisplay();
1384  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1385  }
1386  }
1387 
1388  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1389  ++it) {
1390  vpMbtDistanceCylinder *cy = *it;
1391  if (cy->isVisible() && cy->isTracked()) {
1392  std::vector<std::vector<double> > currentFeatures = cy->getFeaturesForDisplay();
1393  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1394  }
1395  }
1396 
1397  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1398  vpMbtDistanceCircle *ci = *it;
1399  if (ci->isVisible() && ci->isTracked()) {
1400  std::vector<std::vector<double> > currentFeatures = ci->getFeaturesForDisplay();
1401  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1402  }
1403  }
1404 
1405  return features;
1406 }
1407 
1423 std::vector<std::vector<double> > vpMbEdgeTracker::getModelForDisplay(unsigned int width, unsigned int height,
1424  const vpHomogeneousMatrix &cMo,
1425  const vpCameraParameters &cam,
1426  bool displayFullModel)
1427 {
1428  std::vector<std::vector<double> > models;
1429 
1430  for (unsigned int i = 0; i < scales.size(); i += 1) {
1431  if (scales[i]) {
1432  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1433  ++it) {
1434  std::vector<std::vector<double> > currentModel =
1435  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1436  models.insert(models.end(), currentModel.begin(), currentModel.end());
1437  }
1438 
1439  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1440  it != cylinders[scaleLevel].end(); ++it) {
1441  std::vector<std::vector<double> > currentModel =
1442  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1443  models.insert(models.end(), currentModel.begin(), currentModel.end());
1444  }
1445 
1446  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1447  it != circles[scaleLevel].end(); ++it) {
1448  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1449  if (!paramsCircle.empty()) {
1450  models.push_back(paramsCircle);
1451  }
1452  }
1453  break; // displaying model on one scale only
1454  }
1455  }
1456 
1457  return models;
1458 }
1459 
1461 {
1462  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1465  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1466 
1467  switch (state) {
1470  break;
1471 
1472  case vpMeSite::CONTRAST:
1473  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1474  break;
1475 
1476  case vpMeSite::THRESHOLD:
1478  break;
1479 
1480  case vpMeSite::M_ESTIMATOR:
1481  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1482  break;
1483 
1484  case vpMeSite::TOO_NEAR:
1485  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1486  break;
1487 
1488  default:
1490  }
1491  }
1492  }
1493 }
1494 
1496 {
1497  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1500  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1501 
1502  switch (state) {
1505  break;
1506 
1507  case vpMeSite::CONTRAST:
1508  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1509  break;
1510 
1511  case vpMeSite::THRESHOLD:
1513  break;
1514 
1515  case vpMeSite::M_ESTIMATOR:
1516  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1517  break;
1518 
1519  case vpMeSite::TOO_NEAR:
1520  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1521  break;
1522 
1523  default:
1525  }
1526  }
1527  }
1528 }
1529 
1539 {
1540  const bool doNotTrack = false;
1541 
1542  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1543  ++it) {
1544  vpMbtDistanceLine *l = *it;
1545  bool isvisible = false;
1546 
1547  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1548  ++itindex) {
1549  int index = *itindex;
1550  if (index == -1)
1551  isvisible = true;
1552  else {
1553  if (l->hiddenface->isVisible((unsigned int)index))
1554  isvisible = true;
1555  }
1556  }
1557 
1558  // Si la ligne n'appartient a aucune face elle est tout le temps visible
1559  if (l->Lindex_polygon.empty())
1560  isvisible = true; // Not sure that this can occur
1561 
1562  if (isvisible) {
1563  l->setVisible(true);
1564  l->updateTracked();
1565  if (l->meline.empty() && l->isTracked())
1566  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1567  }
1568  else {
1569  l->setVisible(false);
1570  for (size_t a = 0; a < l->meline.size(); a++) {
1571  if (l->meline[a] != nullptr)
1572  delete l->meline[a];
1573  if (a < l->nbFeature.size())
1574  l->nbFeature[a] = 0;
1575  }
1576  l->nbFeatureTotal = 0;
1577  l->meline.clear();
1578  l->nbFeature.clear();
1579  }
1580  }
1581 
1582  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1583  it != cylinders[scaleLevel].end(); ++it) {
1584  vpMbtDistanceCylinder *cy = *it;
1585 
1586  bool isvisible = false;
1587 
1588  int index = cy->index_polygon;
1589  if (index == -1)
1590  isvisible = true;
1591  else {
1592  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1593  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1594  isvisible = true;
1595  }
1596  // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1597 
1598  if (isvisible) {
1599  cy->setVisible(true);
1600  if (cy->meline1 == nullptr || cy->meline2 == nullptr) {
1601  if (cy->isTracked())
1602  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1603  }
1604  }
1605  else {
1606  cy->setVisible(false);
1607  if (cy->meline1 != nullptr)
1608  delete cy->meline1;
1609  if (cy->meline2 != nullptr)
1610  delete cy->meline2;
1611  cy->meline1 = nullptr;
1612  cy->meline2 = nullptr;
1613  cy->nbFeature = 0;
1614  cy->nbFeaturel1 = 0;
1615  cy->nbFeaturel2 = 0;
1616  }
1617  }
1618 
1619  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1620  it != circles[scaleLevel].end(); ++it) {
1621  vpMbtDistanceCircle *ci = *it;
1622  bool isvisible = false;
1623 
1624  int index = ci->index_polygon;
1625  if (index == -1)
1626  isvisible = true;
1627  else {
1628  if (ci->hiddenface->isVisible((unsigned int)index))
1629  isvisible = true;
1630  }
1631 
1632  if (isvisible) {
1633  ci->setVisible(true);
1634  if (ci->meEllipse == nullptr) {
1635  if (ci->isTracked())
1636  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1637  }
1638  }
1639  else {
1640  ci->setVisible(false);
1641  if (ci->meEllipse != nullptr)
1642  delete ci->meEllipse;
1643  ci->meEllipse = nullptr;
1644  ci->nbFeature = 0;
1645  }
1646  }
1647 }
1648 
1655 {
1656  const bool doNotTrack = false;
1657 
1658  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1659  ++it) {
1660  vpMbtDistanceLine *l = *it;
1661  if (l->isVisible() && l->isTracked()) {
1662  if (l->meline.empty()) {
1663  l->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1664  }
1665  l->trackMovingEdge(I);
1666  }
1667  }
1668 
1669  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1670  it != cylinders[scaleLevel].end(); ++it) {
1671  vpMbtDistanceCylinder *cy = *it;
1672  if (cy->isVisible() && cy->isTracked()) {
1673  if (cy->meline1 == nullptr || cy->meline2 == nullptr) {
1674  cy->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1675  }
1676  cy->trackMovingEdge(I, m_cMo);
1677  }
1678  }
1679 
1680  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1681  it != circles[scaleLevel].end(); ++it) {
1682  vpMbtDistanceCircle *ci = *it;
1683  if (ci->isVisible() && ci->isTracked()) {
1684  if (ci->meEllipse == nullptr) {
1685  ci->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1686  }
1687  ci->trackMovingEdge(I, m_cMo);
1688  }
1689  }
1690 }
1691 
1698 {
1699  vpMbtDistanceLine *l;
1700  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1701  ++it) {
1702  if ((*it)->isTracked()) {
1703  l = *it;
1704  l->updateMovingEdge(I, m_cMo);
1705  if (l->nbFeatureTotal == 0 && l->isVisible()) {
1706  l->Reinit = true;
1707  }
1708  }
1709  }
1710 
1712  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1713  it != cylinders[scaleLevel].end(); ++it) {
1714  if ((*it)->isTracked()) {
1715  cy = *it;
1716  cy->updateMovingEdge(I, m_cMo);
1717  if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1718  cy->Reinit = true;
1719  }
1720  }
1721  }
1722 
1723  vpMbtDistanceCircle *ci;
1724  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1725  it != circles[scaleLevel].end(); ++it) {
1726  if ((*it)->isTracked()) {
1727  ci = *it;
1728  ci->updateMovingEdge(I, m_cMo);
1729  if (ci->nbFeature == 0 && ci->isVisible()) {
1730  ci->Reinit = true;
1731  }
1732  }
1733  }
1734 }
1735 
1737 {
1738  unsigned int n = 0;
1739 
1740  vpMbtDistanceLine *l;
1741  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1742  ++it) {
1743  if ((*it)->isTracked()) {
1744  l = *it;
1745  unsigned int indexLine = 0;
1746  double wmean = 0;
1747  for (size_t a = 0; a < l->meline.size(); a++) {
1748  if (l->nbFeature[a] > 0) {
1749  std::list<vpMeSite>::iterator itListLine;
1750  itListLine = l->meline[a]->getMeList().begin();
1751 
1752  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1753  wmean += m_w_edge[n + indexLine];
1754  vpMeSite p = *itListLine;
1755  if (m_w_edge[n + indexLine] < 0.5) {
1757 
1758  *itListLine = p;
1759  }
1760 
1761  ++itListLine;
1762  indexLine++;
1763  }
1764  }
1765  }
1766  n += l->nbFeatureTotal;
1767 
1768  if (l->nbFeatureTotal != 0)
1769  wmean /= l->nbFeatureTotal;
1770  else
1771  wmean = 1;
1772 
1773  l->setMeanWeight(wmean);
1774 
1775  if (wmean < 0.8)
1776  l->Reinit = true;
1777  }
1778  }
1779 
1780  // Same thing with cylinders as with lines
1782  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1783  it != cylinders[scaleLevel].end(); ++it) {
1784  if ((*it)->isTracked()) {
1785  cy = *it;
1786  double wmean = 0;
1787  std::list<vpMeSite>::iterator itListCyl1;
1788  std::list<vpMeSite>::iterator itListCyl2;
1789 
1790  if (cy->nbFeature > 0) {
1791  itListCyl1 = cy->meline1->getMeList().begin();
1792  itListCyl2 = cy->meline2->getMeList().begin();
1793 
1794  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1795  wmean += m_w_edge[n + i];
1796  vpMeSite p = *itListCyl1;
1797  if (m_w_edge[n + i] < 0.5) {
1799 
1800  *itListCyl1 = p;
1801  }
1802 
1803  ++itListCyl1;
1804  }
1805  }
1806 
1807  if (cy->nbFeaturel1 != 0)
1808  wmean /= cy->nbFeaturel1;
1809  else
1810  wmean = 1;
1811 
1812  cy->setMeanWeight1(wmean);
1813 
1814  if (wmean < 0.8) {
1815  cy->Reinit = true;
1816  }
1817 
1818  wmean = 0;
1819  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1820  wmean += m_w_edge[n + i];
1821  vpMeSite p = *itListCyl2;
1822  if (m_w_edge[n + i] < 0.5) {
1824 
1825  *itListCyl2 = p;
1826  }
1827 
1828  ++itListCyl2;
1829  }
1830 
1831  if (cy->nbFeaturel2 != 0)
1832  wmean /= cy->nbFeaturel2;
1833  else
1834  wmean = 1;
1835 
1836  cy->setMeanWeight2(wmean);
1837 
1838  if (wmean < 0.8) {
1839  cy->Reinit = true;
1840  }
1841 
1842  n += cy->nbFeature;
1843  }
1844  }
1845 
1846  // Same thing with circles as with lines
1847  vpMbtDistanceCircle *ci;
1848  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1849  it != circles[scaleLevel].end(); ++it) {
1850  if ((*it)->isTracked()) {
1851  ci = *it;
1852  double wmean = 0;
1853  std::list<vpMeSite>::iterator itListCir;
1854 
1855  if (ci->nbFeature > 0) {
1856  itListCir = ci->meEllipse->getMeList().begin();
1857  }
1858 
1859  wmean = 0;
1860  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1861  wmean += m_w_edge[n + i];
1862  vpMeSite p = *itListCir;
1863  if (m_w_edge[n + i] < 0.5) {
1865 
1866  *itListCir = p;
1867  }
1868 
1869  ++itListCir;
1870  }
1871 
1872  if (ci->nbFeature != 0)
1873  wmean /= ci->nbFeature;
1874  else
1875  wmean = 1;
1876 
1877  ci->setMeanWeight(wmean);
1878 
1879  if (wmean < 0.8) {
1880  ci->Reinit = true;
1881  }
1882 
1883  n += ci->nbFeature;
1884  }
1885  }
1886 }
1887 
1898 {
1899  vpMbtDistanceLine *l;
1900  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1901  ++it) {
1902  if ((*it)->isTracked()) {
1903  l = *it;
1904  if (l->Reinit && l->isVisible())
1905  l->reinitMovingEdge(I, _cMo, m_mask);
1906  }
1907  }
1908 
1910  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1911  it != cylinders[scaleLevel].end(); ++it) {
1912  if ((*it)->isTracked()) {
1913  cy = *it;
1914  if (cy->Reinit && cy->isVisible())
1915  cy->reinitMovingEdge(I, _cMo, m_mask);
1916  }
1917  }
1918 
1919  vpMbtDistanceCircle *ci;
1920  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1921  it != circles[scaleLevel].end(); ++it) {
1922  if ((*it)->isTracked()) {
1923  ci = *it;
1924  if (ci->Reinit && ci->isVisible())
1925  ci->reinitMovingEdge(I, _cMo, m_mask);
1926  }
1927  }
1928 }
1929 
1931 {
1932  // Clear ME to be displayed
1934 
1935  for (unsigned int i = 0; i < scales.size(); i += 1) {
1936  if (scales[i]) {
1937  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1938  for (size_t a = 0; a < (*it)->meline.size(); a++) {
1939  if ((*it)->meline[a] != nullptr) {
1940  delete (*it)->meline[a];
1941  (*it)->meline[a] = nullptr;
1942  }
1943  }
1944 
1945  (*it)->meline.clear();
1946  (*it)->nbFeature.clear();
1947  (*it)->nbFeatureTotal = 0;
1948  }
1949 
1950  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1951  ++it) {
1952  if ((*it)->meline1 != nullptr) {
1953  delete (*it)->meline1;
1954  (*it)->meline1 = nullptr;
1955  }
1956  if ((*it)->meline2 != nullptr) {
1957  delete (*it)->meline2;
1958  (*it)->meline2 = nullptr;
1959  }
1960 
1961  (*it)->nbFeature = 0;
1962  (*it)->nbFeaturel1 = 0;
1963  (*it)->nbFeaturel2 = 0;
1964  }
1965 
1966  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1967  if ((*it)->meEllipse != nullptr) {
1968  delete (*it)->meEllipse;
1969  (*it)->meEllipse = nullptr;
1970  }
1971  (*it)->nbFeature = 0;
1972  }
1973  }
1974  }
1975 }
1976 
1989 void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1990 {
1991  {
1992  // suppress line already in the model
1993  bool already_here = false;
1994  vpMbtDistanceLine *l;
1995 
1996  for (unsigned int i = 0; i < scales.size(); i += 1) {
1997  if (scales[i]) {
1998  downScale(i);
1999  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2000  l = *it;
2001  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
2002  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
2003  already_here = true;
2004  l->addPolygon(polygon);
2005  l->hiddenface = &faces;
2006  }
2007  }
2008 
2009  if (!already_here) {
2010  l = new vpMbtDistanceLine;
2011 
2013  l->buildFrom(P1, P2, m_rand);
2014  l->addPolygon(polygon);
2015  l->setMovingEdge(&me);
2016  l->hiddenface = &faces;
2017  l->useScanLine = useScanLine;
2018 
2019  l->setIndex(nline);
2020  l->setName(name);
2021 
2024 
2027 
2030 
2031  nline += 1;
2032  lines[i].push_back(l);
2033  }
2034  upScale(i);
2035  }
2036  }
2037  }
2038 }
2039 
2045 void vpMbEdgeTracker::removeLine(const std::string &name)
2046 {
2047  vpMbtDistanceLine *l;
2048 
2049  for (unsigned int i = 0; i < scales.size(); i++) {
2050  if (scales[i]) {
2051  for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2052  l = *it;
2053  if (name.compare(l->getName()) == 0) {
2054  lines[i].erase(it);
2055  break;
2056  }
2057  }
2058  }
2059  }
2060 }
2061 
2072 void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
2073  const std::string &name)
2074 {
2075  {
2076  bool already_here = false;
2077  vpMbtDistanceCircle *ci;
2078 
2079  for (unsigned int i = 0; i < scales.size(); i += 1) {
2080  if (scales[i]) {
2081  downScale(i);
2082  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2083  ci = *it;
2084  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
2085  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
2086  already_here =
2087  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
2088  }
2089  }
2090 
2091  if (!already_here) {
2092  ci = new vpMbtDistanceCircle;
2093 
2095  ci->buildFrom(P1, P2, P3, r);
2096  ci->setMovingEdge(&me);
2097  ci->setIndex(ncircle);
2098  ci->setName(name);
2099  ci->index_polygon = idFace;
2100  ci->hiddenface = &faces;
2101 
2102  // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
2103  // ci->getPolygon().setClipping(clippingFlag);
2104 
2105  // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
2106  // vpPolygon3D::NEAR_CLIPPING)
2107  // ci->getPolygon().setNearClippingDistance(distNearClip);
2108 
2109  // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
2110  // vpPolygon3D::FAR_CLIPPING)
2111  // ci->getPolygon().setFarClippingDistance(distFarClip);
2112 
2113  ncircle += 1;
2114  circles[i].push_back(ci);
2115  }
2116  upScale(i);
2117  }
2118  }
2119  }
2120 }
2121 
2131 void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace, const std::string &name)
2132 {
2133  {
2134  bool already_here = false;
2136 
2137  for (unsigned int i = 0; i < scales.size(); i += 1) {
2138  if (scales[i]) {
2139  downScale(i);
2140  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2141  ++it) {
2142  cy = *it;
2143  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
2144  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
2145  already_here =
2146  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
2147  }
2148  }
2149 
2150  if (!already_here) {
2151  cy = new vpMbtDistanceCylinder;
2152 
2154  cy->buildFrom(P1, P2, r);
2155  cy->setMovingEdge(&me);
2156  cy->setIndex(ncylinder);
2157  cy->setName(name);
2158  cy->index_polygon = idFace;
2159  cy->hiddenface = &faces;
2160  ncylinder += 1;
2161  cylinders[i].push_back(cy);
2162  }
2163  upScale(i);
2164  }
2165  }
2166  }
2167 }
2168 
2174 void vpMbEdgeTracker::removeCylinder(const std::string &name)
2175 {
2177 
2178  for (unsigned int i = 0; i < scales.size(); i++) {
2179  if (scales[i]) {
2180  for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
2181  cy = *it;
2182  if (name.compare(cy->getName()) == 0) {
2183  cylinders[i].erase(it);
2184  break;
2185  }
2186  }
2187  }
2188  }
2189 }
2190 
2196 void vpMbEdgeTracker::removeCircle(const std::string &name)
2197 {
2198  vpMbtDistanceCircle *ci;
2199 
2200  for (unsigned int i = 0; i < scales.size(); i++) {
2201  if (scales[i]) {
2202  for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2203  ci = *it;
2204  if (name.compare(ci->getName()) == 0) {
2205  circles[i].erase(it);
2206  break;
2207  }
2208  }
2209  }
2210  }
2211 }
2212 
2219 {
2220  unsigned int nbpt = p.getNbPoint();
2221  if (nbpt > 0) {
2222  for (unsigned int i = 0; i < nbpt - 1; i++)
2223  addLine(p.p[i], p.p[i + 1], p.getIndex());
2224  addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
2225  }
2226 }
2227 
2239 void vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, bool &newvisibleline)
2240 {
2241  unsigned int n;
2242  bool changed = false;
2243 
2244  if (!useOgre) {
2245  // n = faces.setVisible(_I.getWidth(), I.getHeight(), m_cam, cMo, vpMath::rad(89), vpMath::rad(89),
2246  // changed);
2247  n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2248  }
2249  else {
2250 #ifdef VISP_HAVE_OGRE
2251  n = faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2252 #else
2253  n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2254 #endif
2255  }
2256 
2257  if (n > nbvisiblepolygone) {
2258  // cout << "une nouvelle face est visible " << endl;
2259  newvisibleline = true;
2260  }
2261  else
2262  newvisibleline = false;
2263 
2264  nbvisiblepolygone = n;
2265 }
2266 
2283 {
2284  unsigned int nbpt = polygon.getNbPoint();
2285  if (nbpt > 0) {
2286  for (unsigned int i = 0; i < nbpt - 1; i++)
2287  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2288  vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
2289  }
2290 }
2307 {
2308  unsigned int nbpt = polygon.getNbPoint();
2309  if (nbpt > 0) {
2310  for (unsigned int i = 0; i < nbpt - 1; i++)
2311  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2312  }
2313 }
2314 
2315 unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
2316  unsigned int &nberrors_circles)
2317 {
2318  unsigned int nbrow = 0;
2319  nberrors_lines = 0;
2320  nberrors_cylinders = 0;
2321  nberrors_circles = 0;
2322 
2323  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2324  ++it) {
2325 
2326  vpMbtDistanceLine *l = *it;
2327 
2328  if (l->isTracked()) {
2330  nbrow += l->nbFeatureTotal;
2331  nberrors_lines += l->nbFeatureTotal;
2332  }
2333  }
2334 
2335  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2336  it != cylinders[scaleLevel].end(); ++it) {
2337  vpMbtDistanceCylinder *cy = *it;
2338 
2339  if (cy->isTracked()) {
2341  nbrow += cy->nbFeature;
2342  nberrors_cylinders += cy->nbFeature;
2343  }
2344  }
2345 
2346  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2347  it != circles[scaleLevel].end(); ++it) {
2348  vpMbtDistanceCircle *ci = *it;
2349 
2350  if (ci->isTracked()) {
2352  nbrow += ci->nbFeature;
2353  nberrors_circles += ci->nbFeature;
2354  }
2355  }
2356 
2357  return nbrow;
2358 }
2359 
2371 void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
2372  const std::string &name)
2373 {
2374  addCircle(p1, p2, p3, radius, (int)idFace, name);
2375 }
2376 
2387 void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
2388  const std::string &name)
2389 {
2390  addCylinder(p1, p2, radius, (int)idFace, name);
2391 }
2392 
2399 {
2400  m_cMo.eye();
2401  vpMbtDistanceLine *l;
2403  vpMbtDistanceCircle *ci;
2404 
2405  for (unsigned int i = 0; i < scales.size(); i += 1) {
2406  if (scales[i]) {
2407  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2408  l = *it;
2409  if (l != nullptr)
2410  delete l;
2411  l = nullptr;
2412  }
2413 
2414  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2415  ++it) {
2416  cy = *it;
2417  if (cy != nullptr)
2418  delete cy;
2419  cy = nullptr;
2420  }
2421 
2422  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2423  ci = *it;
2424  if (ci != nullptr)
2425  delete ci;
2426  ci = nullptr;
2427  }
2428  lines[i].clear();
2429  cylinders[i].clear();
2430  circles[i].clear();
2431  }
2432  }
2433 
2434  faces.reset();
2435 
2436  useScanLine = false;
2437 
2438 #ifdef VISP_HAVE_OGRE
2439  useOgre = false;
2440 #endif
2441 
2442  m_computeInteraction = true;
2443  nline = 0;
2444  ncylinder = 0;
2445  m_lambda = 1.0;
2446  nbvisiblepolygone = 0;
2447  percentageGdPt = 0.4;
2448 
2449  angleAppears = vpMath::rad(89);
2452 
2454 
2455  // reinitialization of the scales.
2456  this->setScales(scales);
2457 }
2458 
2471 void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2472  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
2473 {
2474  m_cMo.eye();
2475  vpMbtDistanceLine *l;
2477  vpMbtDistanceCircle *ci;
2478 
2479  for (unsigned int i = 0; i < scales.size(); i += 1) {
2480  if (scales[i]) {
2481  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2482  l = *it;
2483  if (l != nullptr)
2484  delete l;
2485  l = nullptr;
2486  }
2487 
2488  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2489  ++it) {
2490  cy = *it;
2491  if (cy != nullptr)
2492  delete cy;
2493  cy = nullptr;
2494  }
2495 
2496  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2497  ci = *it;
2498  if (ci != nullptr)
2499  delete ci;
2500  ci = nullptr;
2501  }
2502 
2503  lines[i].clear();
2504  cylinders[i].clear();
2505  circles[i].clear();
2506  }
2507  }
2508 
2509  faces.reset();
2510 
2511  // compute_interaction=1;
2512  nline = 0;
2513  ncylinder = 0;
2514  ncircle = 0;
2515  // lambda = 1;
2516  nbvisiblepolygone = 0;
2517 
2518  loadModel(cad_name, verbose, T);
2519  initFromPose(I, cMo);
2520 }
2521 
2532 unsigned int vpMbEdgeTracker::getNbPoints(unsigned int level) const
2533 {
2534  if ((level > scales.size()) || !scales[level]) {
2535  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2536  level);
2537  }
2538 
2539  unsigned int nbGoodPoints = 0;
2540  vpMbtDistanceLine *l;
2541  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2542  l = *it;
2543  if (l->isVisible() && l->isTracked()) {
2544  for (size_t a = 0; a < l->meline.size(); a++) {
2545  if (l->nbFeature[a] != 0)
2546  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2547  itme != l->meline[a]->getMeList().end(); ++itme) {
2548  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2549  nbGoodPoints++;
2550  }
2551  }
2552  }
2553  }
2554 
2556  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2557  ++it) {
2558  cy = *it;
2559  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != nullptr || cy->meline2 != nullptr)) {
2560  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2561  itme1 != cy->meline1->getMeList().end(); ++itme1) {
2562  if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2563  nbGoodPoints++;
2564  }
2565  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2566  itme2 != cy->meline2->getMeList().end(); ++itme2) {
2567  if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2568  nbGoodPoints++;
2569  }
2570  }
2571  }
2572 
2573  vpMbtDistanceCircle *ci;
2574  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2575  ci = *it;
2576  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != nullptr) {
2577  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2578  itme != ci->meEllipse->getMeList().end(); ++itme) {
2579  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2580  nbGoodPoints++;
2581  }
2582  }
2583  }
2584 
2585  return nbGoodPoints;
2586 }
2587 
2609 void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2610 {
2611  unsigned int nbActivatedLevels = 0;
2612  for (unsigned int i = 0; i < scale.size(); i++) {
2613  if (scale[i]) {
2614  nbActivatedLevels++;
2615  }
2616  }
2617 
2618  if (scale.empty() || (nbActivatedLevels == 0)) {
2619  vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2620  "tracking. Use the global one");
2621  this->scales.clear();
2622  this->scales.push_back(true);
2623 
2624  lines.resize(1);
2625  lines[0].clear();
2626 
2627  cylinders.resize(1);
2628  cylinders[0].clear();
2629 
2630  circles.resize(1);
2631  circles[0].clear();
2632  }
2633  else {
2634  this->scales = scale;
2635 
2636  lines.resize(scale.size());
2637  cylinders.resize(scale.size());
2638  circles.resize(scale.size());
2639 
2640  for (unsigned int i = 0; i < lines.size(); i++) {
2641  lines[i].clear();
2642  cylinders[i].clear();
2643  circles[i].clear();
2644  }
2645  }
2646 }
2647 
2654 {
2656  std::cerr << "Far clipping value cannot be inferior than near clipping "
2657  "value. Far clipping won't be considered."
2658  << std::endl;
2659  else if (dist < 0)
2660  std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2661  "won't be considered."
2662  << std::endl;
2663  else {
2665  vpMbtDistanceLine *l;
2666 
2667  for (unsigned int i = 0; i < scales.size(); i += 1) {
2668  if (scales[i]) {
2669  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2670  l = *it;
2672  }
2673  }
2674  }
2675  }
2676 }
2677 
2684 {
2686  std::cerr << "Near clipping value cannot be superior than far clipping "
2687  "value. Near clipping won't be considered."
2688  << std::endl;
2689  else if (dist < 0)
2690  std::cerr << "Near clipping value cannot be inferior than 0. Near "
2691  "clipping won't be considered."
2692  << std::endl;
2693  else {
2695  vpMbtDistanceLine *l;
2696 
2697  for (unsigned int i = 0; i < scales.size(); i += 1) {
2698  if (scales[i]) {
2699  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2700  l = *it;
2702  }
2703  }
2704  }
2705  }
2706 }
2707 
2715 void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2716 {
2717  vpMbTracker::setClipping(flags);
2718 
2719  vpMbtDistanceLine *l;
2720 
2721  for (unsigned int i = 0; i < scales.size(); i += 1) {
2722  if (scales[i]) {
2723  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2724  l = *it;
2726  }
2727  }
2728  }
2729 }
2730 
2747  std::vector<const vpImage<unsigned char> *> &_pyramid)
2748 {
2749  _pyramid.resize(scales.size());
2750 
2751  if (scales[0]) {
2752  _pyramid[0] = &_I;
2753  }
2754  else {
2755  _pyramid[0] = nullptr;
2756  }
2757 
2758  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2759  if (scales[i]) {
2760  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2761  vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2762  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2763  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2764  (*I)[k][l] = _I[ii][jj];
2765  }
2766  }
2767  _pyramid[i] = I;
2768  }
2769  else {
2770  _pyramid[i] = nullptr;
2771  }
2772  }
2773 }
2774 
2781 void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2782 {
2783  if (_pyramid.size() > 0) {
2784  _pyramid[0] = nullptr;
2785  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2786  if (_pyramid[i] != nullptr) {
2787  delete _pyramid[i];
2788  _pyramid[i] = nullptr;
2789  }
2790  }
2791  _pyramid.resize(0);
2792  }
2793 }
2794 
2805 void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level) const
2806 {
2807  if (level > scales.size() || !scales[level]) {
2808  std::ostringstream oss;
2809  oss << level;
2810  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2811  throw vpException(vpException::dimensionError, errorMsg);
2812  }
2813 
2814  linesList = lines[level];
2815 }
2816 
2827 void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level) const
2828 {
2829  if (level > scales.size() || !scales[level]) {
2830  std::ostringstream oss;
2831  oss << level;
2832  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2833  throw vpException(vpException::dimensionError, errorMsg);
2834  }
2835 
2836  cylindersList = cylinders[level];
2837 }
2838 
2849 void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level) const
2850 {
2851  if (level > scales.size() || !scales[level]) {
2852  std::ostringstream oss;
2853  oss << level;
2854  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2855  throw vpException(vpException::dimensionError, errorMsg);
2856  }
2857 
2858  circlesList = circles[level];
2859 }
2860 
2867 void vpMbEdgeTracker::downScale(const unsigned int _scale)
2868 {
2869  const double ratio = pow(2., (int)_scale);
2870  scaleLevel = _scale;
2871 
2872  vpMatrix K = m_cam.get_K();
2873 
2874  K[0][0] /= ratio;
2875  K[1][1] /= ratio;
2876  K[0][2] /= ratio;
2877  K[1][2] /= ratio;
2878 
2880 }
2881 
2888 void vpMbEdgeTracker::upScale(const unsigned int _scale)
2889 {
2890  const double ratio = pow(2., (int)_scale);
2891  scaleLevel = 0;
2892 
2893  vpMatrix K = m_cam.get_K();
2894 
2895  K[0][0] *= ratio;
2896  K[1][1] *= ratio;
2897  K[0][2] *= ratio;
2898  K[1][2] *= ratio;
2899 
2901 }
2902 
2910 void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2911 {
2912  unsigned int scaleLevel_1 = scaleLevel;
2913  scaleLevel = _lvl;
2914 
2915  vpMbtDistanceLine *l;
2916  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2917  ++it) {
2918  if ((*it)->isTracked()) {
2919  l = *it;
2920  l->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2921  }
2922  }
2923 
2925  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2926  it != cylinders[scaleLevel].end(); ++it) {
2927  if ((*it)->isTracked()) {
2928  cy = *it;
2929  cy->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2930  }
2931  }
2932 
2933  vpMbtDistanceCircle *ci;
2934  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2935  it != circles[scaleLevel].end(); ++it) {
2936  if ((*it)->isTracked()) {
2937  ci = *it;
2938  ci->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2939  }
2940  }
2941 
2942  trackMovingEdge(*Ipyramid[_lvl]);
2943  updateMovingEdge(*Ipyramid[_lvl]);
2944  scaleLevel = scaleLevel_1;
2945 }
2946 
2954 void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2955 {
2956  for (unsigned int i = 0; i < scales.size(); i += 1) {
2957  if (scales[i]) {
2958  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2959  /*(*it)->setTracked(useEdgeTracking);
2960  for(std::list<int>::const_iterator
2961  itpoly=(*it)->Lindex_polygon.begin();
2962  itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2963  if(faces[(*itpoly)]->getName() != name){
2964  (*it)->setTracked(true);
2965  break;
2966  }
2967  }*/
2968 
2969  (*it)->setTracked(name, useEdgeTracking);
2970  }
2971 
2972  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2973  ++it) {
2974  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2975  (*it)->setTracked(useEdgeTracking);
2976  }
2977  }
2978 
2979  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2980  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2981  (*it)->setTracked(useEdgeTracking);
2982  }
2983  }
2984  }
2985  }
2986 }
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:354
unsigned int getRows() const
Definition: vpArray2D.h:339
Generic class defining intrinsic camera parameters.
vpMatrix get_K() const
void computeFov(const unsigned int &w, const unsigned int &h)
void initFromCalibrationMatrix(const vpMatrix &_K)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor cyan
Definition: vpColor.h:220
static const vpColor blue
Definition: vpColor.h:217
static const vpColor purple
Definition: vpColor.h:222
static const vpColor yellow
Definition: vpColor.h:219
static const vpColor green
Definition: vpColor.h:214
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:66
@ dimensionError
Bad dimension.
Definition: vpException.h:70
@ fatalError
Fatal error.
Definition: vpException.h:71
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static double rad(double deg)
Definition: vpMath.h:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static double sqr(double x)
Definition: vpMath.h:201
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:454
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
static int sign(double x)
Definition: vpMath.h:424
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void eye()
Definition: vpMatrix.cpp:448
vpMatrix AtA() const
Definition: vpMatrix.cpp:645
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2343
void removeCircle(const std::string &name)
void computeVVS(const vpImage< unsigned char > &_I, unsigned int lvl)
vpColVector m_errorCircles
vpColVector m_w_edge
Robust weights.
void upScale(const unsigned int _scale)
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMe me
The moving edges parameters.
void displayFeaturesOnImage(const vpImage< unsigned char > &I)
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, unsigned int level=0) const
vpColVector m_wLines
void computeProjectionError(const vpImage< unsigned char > &_I)
virtual void computeVVSWeights()
vpColVector m_error_edge
(s - s*)
unsigned int ncylinder
void downScale(const unsigned int _scale)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
virtual ~vpMbEdgeTracker() vp_override
void removeCylinder(const std::string &name)
virtual void setNearClippingDistance(const double &dist) vp_override
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void computeVVSFirstPhase(const vpImage< unsigned char > &I, unsigned int iter, double &count, unsigned int lvl=0)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="") vp_override
virtual void setFarClippingDistance(const double &dist) vp_override
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
void resetTracker() vp_override
virtual void computeVVSInteractionMatrixAndResidu() vp_override
vpRobust m_robustCircles
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, unsigned int level=0) const
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int scaleLevel
virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override
void removeLine(const std::string &name)
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual void track(const vpImage< unsigned char > &I) vp_override
void setScales(const std::vector< bool > &_scales)
void trackMovingEdge(const vpImage< unsigned char > &I)
virtual void setClipping(const unsigned int &flags) vp_override
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual unsigned int getNbPoints(unsigned int level=0) const
vpColVector m_weightedError_edge
Weighted error.
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="") vp_override
unsigned int ncircle
virtual void setCameraParameters(const vpCameraParameters &cam) vp_override
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
void updateMovingEdge(const vpImage< unsigned char > &I)
vpMatrix m_L_edge
Interaction matrix.
vpRobust m_robustCylinders
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false) vp_override
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void reInitLevel(const unsigned int _lvl)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
virtual void computeVVSInit() vp_override
virtual void initFaceFromCorners(vpMbtPolygon &polygon) vp_override
vpRobust m_robustLines
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
void setMovingEdge(const vpMe &me)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo) vp_override
vpColVector m_wCylinders
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
virtual void testTracking() vp_override
vpColVector m_factor
Edge VVS variables.
vpColVector m_errorLines
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
vpColVector m_wCircles
virtual void init(const vpImage< unsigned char > &I) vp_override
void addPolygon(vpMbtPolygon &p)
unsigned int nline
vpColVector m_errorCylinders
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool isAppearing(unsigned int i)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void displayOgre(const vpHomogeneousMatrix &cMo)
void setOgreShowConfigDialog(bool showConfigDialog)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
bool modelInitialised
Definition: vpMbTracker.h:123
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
Definition: vpMbTracker.h:227
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
bool computeProjError
Definition: vpMbTracker.h:133
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=nullptr, const vpColVector *const m_w_prev=nullptr)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
double projectionError
Definition: vpMbTracker.h:136
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
virtual void computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=nullptr, vpColVector *const m_w_prev=nullptr)
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getFeaturesForDisplay()
void setCameraParameters(const vpCameraParameters &camera)
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void setIndex(unsigned int i)
std::string getName() const
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
void setMeanWeight(double _wmean)
bool Reinit
Indicates if the circle has to be reinitialized.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
Manage a cylinder used in the model-based tracker.
void setMeanWeight1(double wmean)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setMeanWeight2(double wmean)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
std::string getName() const
std::vector< std::vector< double > > getFeaturesForDisplay()
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setIndex(unsigned int i)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
std::string getName() const
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
std::vector< std::vector< double > > getFeaturesForDisplay()
bool isTracked() const
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setCameraParameters(const vpCameraParameters &camera)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
void setName(const std::string &line_name)
void setMeanWeight(double w_mean)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
void trackMovingEdge(const vpImage< unsigned char > &I)
vpMbtPolygon & getPolygon()
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:58
std::string getName() const
Definition: vpMbtPolygon.h:98
int getIndex() const
Definition: vpMbtPolygon.h:91
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void getCameraParameters(vpCameraParameters &cam) const
void setEdgeMe(const vpMe &ecm)
void getEdgeMe(vpMe &ecm) const
double getLodMinLineLengthThreshold() const
void setAngleDisappear(const double &adisappear)
void setAngleAppear(const double &aappear)
void parse(const std::string &filename)
void setCameraParameters(const vpCameraParameters &cam)
double getLodMinPolygonAreaThreshold() const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:65
@ TOO_NEAR
Point not tracked anymore, since too near from its neighbor.
Definition: vpMeSite.h:90
@ THRESHOLD
Point not tracked due to the likelihood that is below the threshold, but retained in the ME list.
Definition: vpMeSite.h:88
@ CONTRAST
Point not tracked due to a contrast problem, but retained in the ME list.
Definition: vpMeSite.h:84
@ M_ESTIMATOR
Point detected as an outlier during virtual visual-servoing.
Definition: vpMeSite.h:89
@ NO_SUPPRESSION
Point successfully tracked.
Definition: vpMeSite.h:83
vpMeSiteState getState() const
Definition: vpMeSite.h:266
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:256
Definition: vpMe.h:124
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 setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:189
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:127
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:202
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:76
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:182
@ 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:135
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.
vpVelocityTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:407
#define vpERROR_TRACE
Definition: vpDebug.h:384