Visual Servoing Platform  version 3.5.1 under development (2023-06-02)
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Make the complete tracking of an object by using its CAD model
33  *
34  * Authors:
35  * Nicolas Melchior
36  * Romain Tallonneau
37  * Eric Marchand
38  *
39  *****************************************************************************/
40 
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpExponentialMap.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMatrixException.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPolygon3D.h>
53 #include <visp3/core/vpTrackingException.h>
54 #include <visp3/core/vpVelocityTwistMatrix.h>
55 #include <visp3/mbt/vpMbEdgeTracker.h>
56 #include <visp3/mbt/vpMbtDistanceLine.h>
57 #include <visp3/mbt/vpMbtXmlGenericParser.h>
58 #include <visp3/vision/vpPose.h>
59 
60 #include <float.h>
61 #include <limits>
62 #include <map>
63 #include <sstream>
64 #include <string>
65 
70  : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
71  percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
72  m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
73  m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
74  m_robust_edge(), m_featuresToBeDisplayedEdge()
75 {
76  scales[0] = true;
77 
78 #ifdef VISP_HAVE_OGRE
79  faces.getOgreContext()->setWindowName("MBT Edge");
80 #endif
81 }
82 
87 {
91 
92  for (unsigned int i = 0; i < scales.size(); i += 1) {
93  if (scales[i]) {
94  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
95  l = *it;
96  if (l != NULL) {
97  delete l;
98  }
99  l = NULL;
100  }
101 
102  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
103  ++it) {
104  cy = *it;
105  if (cy != NULL) {
106  delete cy;
107  }
108  cy = NULL;
109  }
110 
111  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
112  ci = *it;
113  if (ci != NULL) {
114  delete ci;
115  }
116  ci = NULL;
117  }
118 
119  lines[i].clear();
120  cylinders[i].clear();
121  circles[i].clear();
122  }
123  }
124 
126 }
127 
134 {
135  this->me = p_me;
136 
137  for (unsigned int i = 0; i < scales.size(); i += 1) {
138  if (scales[i]) {
139  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
140  vpMbtDistanceLine *l = *it;
141  l->setMovingEdge(&(this->me));
142  }
143 
144  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
145  ++it) {
146  vpMbtDistanceCylinder *cy = *it;
147  cy->setMovingEdge(&(this->me));
148  }
149 
150  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
151  vpMbtDistanceCircle *ci = *it;
152  ci->setMovingEdge(&(this->me));
153  }
154  }
155  }
156 }
157 
167 void vpMbEdgeTracker::computeVVS(const vpImage<unsigned char> &_I, unsigned int lvl)
168 {
169  double residu_1 = 1e3;
170  double r = 1e3 - 1;
171 
172  unsigned int iter = 0;
173 
174  computeVVSInit();
175  unsigned int nbrow = m_error_edge.getRows();
176 
177  bool reloop = true;
178 
179  bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
180  if (isoJoIdentity)
181  oJo.eye();
182 
183  /*** First phase ***/
184 
185  while (reloop == true && iter < 10) {
186  double count = 0;
187 
188  computeVVSFirstPhase(_I, iter, count, lvl);
189 
190  count = count / (double)nbrow;
191  if (count >= 0.85) {
192  reloop = false;
193  }
194 
195  computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity);
196 
197  iter++;
198  }
199 
200  // std::cout << "\t First minimization in " << iter << " iteration give as
201  // initial cMo: \n" << cMo << std::endl;
202 
203  /*** Second phase ***/
204  vpHomogeneousMatrix cMoPrev;
205  vpColVector W_true(nbrow);
206  vpMatrix L_true;
207  vpMatrix LVJ_true;
208 
209  double mu = m_initialMu;
210  vpColVector m_error_prev;
211  vpColVector m_w_prev;
212 
213  // To avoid to create these matrices each iteration
214  vpMatrix LTL;
215  vpColVector LTR;
216  vpColVector v;
217 
218  iter = 0;
219  m_w_edge = 1;
220 
221  // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
222  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
224 
225  bool reStartFromLastIncrement = false;
226  computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
227  &m_w_edge, &m_w_prev);
228 
229  if (!reStartFromLastIncrement) {
231 
232  L_true = m_L_edge;
234 
235  if (computeCovariance) {
236  L_true = m_L_edge;
237  if (!isoJoIdentity) {
238  cVo.buildFrom(m_cMo);
239  LVJ_true = (m_L_edge * cVo * oJo);
240  }
241  }
242 
243  double wi = 0.0, eri = 0.0;
244  double num = 0.0, den = 0.0;
245  if ((iter == 0) || m_computeInteraction) {
246  for (unsigned int i = 0; i < nbrow; i++) {
247  wi = m_w_edge[i] * m_factor[i];
248  W_true[i] = wi;
249  eri = m_error_edge[i];
250  num += wi * vpMath::sqr(eri);
251  den += wi;
252 
253  m_weightedError_edge[i] = wi * eri;
254 
255  for (unsigned int j = 0; j < 6; j++) {
256  m_L_edge[i][j] = wi * m_L_edge[i][j];
257  }
258  }
259  } else {
260  for (unsigned int i = 0; i < nbrow; i++) {
261  wi = m_w_edge[i] * m_factor[i];
262  W_true[i] = wi;
263  eri = m_error_edge[i];
264  num += wi * vpMath::sqr(eri);
265  den += wi;
266 
267  m_weightedError_edge[i] = wi * eri;
268  }
269  }
270 
271  residu_1 = r;
272  r = sqrt(num / den); // Le critere d'arret prend en compte le poids
273 
274  computeVVSPoseEstimation(isoJoIdentity, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev,
275  LTR, mu, v, &m_w_edge, &m_w_prev);
276 
277  cMoPrev = m_cMo;
279 
280  } // endif(!restartFromLast)
281 
282  iter++;
283  }
284 
285  computeCovarianceMatrixVVS(isoJoIdentity, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);
286 
288 }
289 
290 void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_I, unsigned int iter, double &count,
291  unsigned int lvl)
292 {
296 
297  double limite = 3; // Une limite de 3 pixels
298  limite = limite / m_cam.get_px(); // Transformation limite pixel en limite metre.
299 
300  unsigned int n = 0;
301 
302  // Parametre pour la premiere phase d'asservissement
303  double e_prev = 0, e_cur, e_next;
304 
305  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
306  if ((*it)->isTracked()) {
307  l = *it;
309 
310  double fac = 1;
311  if (iter == 0) {
312  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
313  ++itindex) {
314  int index = *itindex;
315  if (l->hiddenface->isAppearing((unsigned int)index)) {
316  fac = 0.2;
317  break;
318  }
319  if (l->closeToImageBorder(_I, 10)) {
320  fac = 0.1;
321  break;
322  }
323  }
324  }
325 
326  std::list<vpMeSite>::const_iterator itListLine;
327 
328  unsigned int indexFeature = 0;
329 
330  for (size_t a = 0; a < l->meline.size(); a++) {
331  if (iter == 0 && l->meline[a] != NULL)
332  itListLine = l->meline[a]->getMeList().begin();
333 
334  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
335  for (unsigned int j = 0; j < 6; j++) {
336  m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale
337  }
338  m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur
339 
340  if (m_error_edge[n + i] <= limite)
341  count = count + 1.0; // Si erreur proche de 0 on incremente cur
342 
343  m_w_edge[n + i] = 0;
344 
345  if (iter == 0) {
346  m_factor[n + i] = fac;
347  vpMeSite site = *itListLine;
348  if (site.getState() != vpMeSite::NO_SUPPRESSION)
349  m_factor[n + i] = 0.2;
350  ++itListLine;
351  }
352 
353  // If pour la premiere extremite des moving edges
354  if (indexFeature == 0) {
355  e_cur = l->error[0];
356  if (l->nbFeature[a] > 1) {
357  e_next = l->error[1];
358  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
359  m_w_edge[n + i] = 1 /*0.5*/;
360  }
361  e_prev = e_cur;
362  } else
363  m_w_edge[n + i] = 1;
364  }
365 
366  // If pour la derniere extremite des moving edges
367  else if (indexFeature == l->nbFeatureTotal - 1) {
368  e_cur = l->error[indexFeature];
369  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
370  m_w_edge[n + i] += 1 /*0.5*/;
371  }
372  }
373 
374  else {
375  e_cur = l->error[indexFeature];
376  e_next = l->error[indexFeature + 1];
377  if (fabs(e_cur - e_prev) < limite) {
378  m_w_edge[n + i] += 0.5;
379  }
380  if (fabs(e_cur - e_next) < limite) {
381  m_w_edge[n + i] += 0.5;
382  }
383  e_prev = e_cur;
384  }
385  indexFeature++;
386  }
387  n += l->nbFeature[a];
388  }
389  }
390  }
391 
392  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
393  ++it) {
394  if ((*it)->isTracked()) {
395  cy = *it;
397  double fac = 1.0;
398 
399  std::list<vpMeSite>::const_iterator itCyl1;
400  std::list<vpMeSite>::const_iterator itCyl2;
401  if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)) {
402  itCyl1 = cy->meline1->getMeList().begin();
403  itCyl2 = cy->meline2->getMeList().begin();
404  }
405 
406  for (unsigned int i = 0; i < cy->nbFeature; i++) {
407  for (unsigned int j = 0; j < 6; j++) {
408  m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale
409  }
410  m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur
411 
412  if (m_error_edge[n + i] <= limite)
413  count = count + 1.0; // Si erreur proche de 0 on incremente cur
414 
415  m_w_edge[n + i] = 0;
416 
417  if (iter == 0) {
418  m_factor[n + i] = fac;
419  vpMeSite site;
420  if (i < cy->nbFeaturel1) {
421  site = *itCyl1;
422  ++itCyl1;
423  } else {
424  site = *itCyl2;
425  ++itCyl2;
426  }
427  if (site.getState() != vpMeSite::NO_SUPPRESSION)
428  m_factor[n + i] = 0.2;
429  }
430 
431  // If pour la premiere extremite des moving edges
432  if (i == 0) {
433  e_cur = cy->error[0];
434  if (cy->nbFeature > 1) {
435  e_next = cy->error[1];
436  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
437  m_w_edge[n + i] = 1 /*0.5*/;
438  }
439  e_prev = e_cur;
440  } else
441  m_w_edge[n + i] = 1;
442  }
443  if (i == cy->nbFeaturel1) {
444  e_cur = cy->error[i];
445  if (cy->nbFeaturel2 > 1) {
446  e_next = cy->error[i + 1];
447  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
448  m_w_edge[n + i] = 1 /*0.5*/;
449  }
450  e_prev = e_cur;
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 != NULL)) {
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  } else
527  m_w_edge[n + i] = 1;
528  }
529 
530  // If pour la derniere extremite des moving edges
531  else if (i == ci->nbFeature - 1) {
532  e_cur = ci->error[i];
533  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
534  m_w_edge[n + i] += 1 /*0.5*/;
535  }
536  }
537 
538  else {
539  e_cur = ci->error[i];
540  e_next = ci->error[i + 1];
541  if (fabs(e_cur - e_prev) < limite) {
542  m_w_edge[n + i] += 0.5;
543  }
544  if (fabs(e_cur - e_next) < limite) {
545  m_w_edge[n + i] += 0.5;
546  }
547  e_prev = e_cur;
548  }
549  }
550 
551  n += ci->nbFeature;
552  }
553  }
554 }
555 
557 {
561 
562  unsigned int n = 0;
563  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
564  if ((*it)->isTracked()) {
565  l = *it;
567 
568  double fac = 1;
569  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
570  ++itindex) {
571  int index = *itindex;
572  if (l->hiddenface->isAppearing((unsigned int)index)) {
573  fac = 0.2;
574  break;
575  }
576  if (l->closeToImageBorder(I, 10)) {
577  fac = 0.1;
578  break;
579  }
580  }
581 
582  for (size_t a = 0; a < l->meline.size(); a++) {
583  std::list<vpMeSite>::const_iterator itListLine;
584  if (l->meline[a] != NULL) {
585  itListLine = l->meline[a]->getMeList().begin();
586 
587  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
588  m_factor[n + i] = fac;
589  vpMeSite site = *itListLine;
590  if (site.getState() != vpMeSite::NO_SUPPRESSION)
591  m_factor[n + i] = 0.2;
592  ++itListLine;
593  }
594  n += l->nbFeature[a];
595  }
596  }
597  }
598  }
599 
600  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
601  ++it) {
602  if ((*it)->isTracked()) {
603  cy = *it;
605 
606  std::list<vpMeSite>::const_iterator itCyl1;
607  std::list<vpMeSite>::const_iterator itCyl2;
608  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
609  itCyl1 = cy->meline1->getMeList().begin();
610  itCyl2 = cy->meline2->getMeList().begin();
611 
612  double fac = 1.0;
613  for (unsigned int i = 0; i < cy->nbFeature; i++) {
614  m_factor[n + i] = fac;
615  vpMeSite site;
616  if (i < cy->nbFeaturel1) {
617  site = *itCyl1;
618  ++itCyl1;
619  } else {
620  site = *itCyl2;
621  ++itCyl2;
622  }
623  if (site.getState() != vpMeSite::NO_SUPPRESSION)
624  m_factor[n + i] = 0.2;
625  }
626  n += cy->nbFeature;
627  }
628  }
629  }
630 
631  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
632  if ((*it)->isTracked()) {
633  ci = *it;
635 
636  std::list<vpMeSite>::const_iterator itCir;
637  if (ci->meEllipse != NULL) {
638  itCir = ci->meEllipse->getMeList().begin();
639  double fac = 1.0;
640 
641  for (unsigned int i = 0; i < ci->nbFeature; i++) {
642  m_factor[n + i] = fac;
643  vpMeSite site = *itCir;
644  if (site.getState() != vpMeSite::NO_SUPPRESSION)
645  m_factor[n + i] = 0.2;
646  ++itCir;
647  }
648  n += ci->nbFeature;
649  }
650  }
651  }
652 }
653 
654 void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
655 {
656  unsigned int nerror = m_weightedError_edge.getRows();
657 
658  double wi, eri;
659  if ((iter == 0) || m_computeInteraction) {
660  for (unsigned int i = 0; i < nerror; i++) {
661  wi = m_w_edge[i] * m_factor[i];
662  eri = m_error_edge[i];
663 
664  m_weightedError_edge[i] = wi * eri;
665 
666  for (unsigned int j = 0; j < 6; j++) {
667  m_L_edge[i][j] = wi * m_L_edge[i][j];
668  }
669  }
670  } else {
671  for (unsigned int i = 0; i < nerror; i++) {
672  wi = m_w_edge[i] * m_factor[i];
673  eri = m_error_edge[i];
674 
675  m_weightedError_edge[i] = wi * eri;
676  }
677  }
678 
680 
681  // If all the 6 dof should be estimated, we check if the interaction matrix
682  // is full rank. If not we remove automatically the dof that cannot be
683  // estimated This is particularly useful when considering circles (rank 5) and
684  // cylinders (rank 4)
685  if (isoJoIdentity) {
686  cVo.buildFrom(m_cMo);
687 
688  vpMatrix K; // kernel
689  unsigned int rank = (m_L_edge * cVo).kernel(K);
690  if (rank == 0) {
691  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
692  }
693  if (rank != 6) {
694  vpMatrix I; // Identity
695  I.eye(6);
696  oJo = I - K.AtA();
697 
698  isoJoIdentity = false;
699  }
700  }
701 
702  vpColVector v;
703  vpMatrix LTL;
704  vpColVector LTR;
705 
706  if (isoJoIdentity) {
707  LTL = m_L_edge.AtA();
709  v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
710  } else {
711  cVo.buildFrom(m_cMo);
712  vpMatrix LVJ = (m_L_edge * cVo * oJo);
713  vpMatrix LVJTLVJ = (LVJ).AtA();
714  vpColVector LVJTR;
715  computeJTR(LVJ, m_weightedError_edge, LVJTR);
716  v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
717  v = cVo * v;
718  }
719 
721 }
722 
724 {
725  // Nombre de moving edges
726  unsigned int nbrow = 0;
727  unsigned int nberrors_lines = 0;
728  unsigned int nberrors_cylinders = 0;
729  unsigned int nberrors_circles = 0;
730 
731  nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
732 
733  if (nbrow == 0) {
735  "No data found to compute the interaction matrix...");
736  }
737 
738  m_L_edge.resize(nbrow, 6, false, false);
739  m_error_edge.resize(nbrow, false);
740 
741  m_weightedError_edge.resize(nbrow, false);
742  m_w_edge.resize(nbrow, false);
743  m_w_edge = 1;
744  m_factor.resize(nbrow, false);
745  m_factor = 1;
746 
750 
751  m_wLines.resize(nberrors_lines, false);
752  m_wLines = 1;
753  m_wCylinders.resize(nberrors_cylinders, false);
754  m_wCylinders = 1;
755  m_wCircles.resize(nberrors_circles, false);
756  m_wCircles = 1;
757 
758  m_errorLines.resize(nberrors_lines, false);
759  m_errorCylinders.resize(nberrors_cylinders, false);
760  m_errorCircles.resize(nberrors_circles, false);
761 }
762 
764 {
765  throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
766  "computeVVSInteractionMatrixAndR"
767  "esidu() should not be called!");
768 }
769 
771 {
775 
776  unsigned int n = 0;
777  unsigned int nlines = 0;
778  unsigned int ncylinders = 0;
779  unsigned int ncircles = 0;
780 
781  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
782  ++it) {
783  if ((*it)->isTracked()) {
784  l = *it;
786  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
787  for (unsigned int j = 0; j < 6; j++) {
788  m_L_edge[n + i][j] = l->L[i][j];
789  m_error_edge[n + i] = l->error[i];
790  m_errorLines[nlines + i] = m_error_edge[n + i];
791  }
792  }
793  n += l->nbFeatureTotal;
794  nlines += l->nbFeatureTotal;
795  }
796  }
797 
798  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
799  it != cylinders[scaleLevel].end(); ++it) {
800  if ((*it)->isTracked()) {
801  cy = *it;
803  for (unsigned int i = 0; i < cy->nbFeature; i++) {
804  for (unsigned int j = 0; j < 6; j++) {
805  m_L_edge[n + i][j] = cy->L[i][j];
806  m_error_edge[n + i] = cy->error[i];
807  m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
808  }
809  }
810 
811  n += cy->nbFeature;
812  ncylinders += cy->nbFeature;
813  }
814  }
815 
816  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
817  it != circles[scaleLevel].end(); ++it) {
818  if ((*it)->isTracked()) {
819  ci = *it;
821  for (unsigned int i = 0; i < ci->nbFeature; i++) {
822  for (unsigned int j = 0; j < 6; j++) {
823  m_L_edge[n + i][j] = ci->L[i][j];
824  m_error_edge[n + i] = ci->error[i];
825  m_errorCircles[ncircles + i] = m_error_edge[n + i];
826  }
827  }
828 
829  n += ci->nbFeature;
830  ncircles += ci->nbFeature;
831  }
832  }
833 }
834 
836 {
837  unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
838  nberrors_circles = m_errorCircles.getRows();
839 
840  if (nberrors_lines > 0)
842  if (nberrors_cylinders > 0)
844  if (nberrors_circles > 0)
846 
850 }
851 
861 {
862  projectionError = 0.0;
863  unsigned int nbFeatures = 0;
864  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
865  ++it) {
866  vpMbtDistanceLine *l = *it;
867  if (l->isVisible() && l->isTracked()) {
868  for (size_t a = 0; a < l->meline.size(); a++) {
869  if (l->meline[a] != NULL) {
870  double lineNormGradient;
871  unsigned int lineNbFeatures;
872  l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
875  projectionError += lineNormGradient;
876  nbFeatures += lineNbFeatures;
877  }
878  }
879  }
880  }
881 
882  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
883  it != cylinders[scaleLevel].end(); ++it) {
884  vpMbtDistanceCylinder *cy = *it;
885  if (cy->isVisible() && cy->isTracked()) {
886  if (cy->meline1 != NULL) {
887  double cylinderNormGradient = 0;
888  unsigned int cylinderNbFeatures = 0;
889  cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
892  projectionError += cylinderNormGradient;
893  nbFeatures += cylinderNbFeatures;
894  }
895 
896  if (cy->meline2 != NULL) {
897  double cylinderNormGradient = 0;
898  unsigned int cylinderNbFeatures = 0;
899  cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
902  projectionError += cylinderNormGradient;
903  nbFeatures += cylinderNbFeatures;
904  }
905  }
906  }
907 
908  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
909  it != circles[scaleLevel].end(); ++it) {
910  vpMbtDistanceCircle *c = *it;
911  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
912  double circleNormGradient = 0;
913  unsigned int circleNbFeatures = 0;
914  c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
917  projectionError += circleNormGradient;
918  nbFeatures += circleNbFeatures;
919  }
920  }
921 
922  if (nbFeatures > 0) {
923  projectionError = vpMath::deg(projectionError / (double)nbFeatures);
924  } else {
925  projectionError = 90.0;
926  }
927 
929  // std::cout << "Norm Gradient = " << errorGradient << std::endl;
930 }
931 
938 {
939  int nbExpectedPoint = 0;
940  int nbGoodPoint = 0;
941  int nbBadPoint = 0;
942 
943  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
944  ++it) {
945  vpMbtDistanceLine *l = *it;
946  if (l->isVisible() && l->isTracked()) {
947  for (size_t a = 0; a < l->meline.size(); a++) {
948  if (l->meline[a] != NULL) {
949  nbExpectedPoint += (int)l->meline[a]->expecteddensity;
950  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
951  itme != l->meline[a]->getMeList().end(); ++itme) {
952  vpMeSite pix = *itme;
953  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
954  nbGoodPoint++;
955  else
956  nbBadPoint++;
957  }
958  }
959  }
960  }
961  }
962 
963  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
964  it != cylinders[scaleLevel].end(); ++it) {
965  vpMbtDistanceCylinder *cy = *it;
966  if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) {
967  nbExpectedPoint += (int)cy->meline1->expecteddensity;
968  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
969  itme1 != cy->meline1->getMeList().end(); ++itme1) {
970  vpMeSite pix = *itme1;
971  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
972  nbGoodPoint++;
973  else
974  nbBadPoint++;
975  }
976  nbExpectedPoint += (int)cy->meline2->expecteddensity;
977  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
978  itme2 != cy->meline2->getMeList().end(); ++itme2) {
979  vpMeSite pix = *itme2;
980  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
981  nbGoodPoint++;
982  else
983  nbBadPoint++;
984  }
985  }
986  }
987 
988  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
989  it != circles[scaleLevel].end(); ++it) {
990  vpMbtDistanceCircle *ci = *it;
991  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
992  nbExpectedPoint += ci->meEllipse->getExpectedDensity();
993  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
994  itme != ci->meEllipse->getMeList().end(); ++itme) {
995  vpMeSite pix = *itme;
996  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
997  nbGoodPoint++;
998  else
999  nbBadPoint++;
1000  }
1001  }
1002  }
1003 
1004  // Compare the number of good points with the min between the number of
1005  // expected points and number of points that are tracked
1006  int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
1007  // int nb_min = (std::min)(val1, val2);
1008  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1009  std::ostringstream oss;
1010  oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
1011  << ". Try to reduce the threshold=" << percentageGdPt
1012  << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1014  }
1015 }
1016 
1025 {
1026  initPyramid(I, Ipyramid);
1027 
1028  unsigned int lvl = (unsigned int)scales.size();
1029  do {
1030  lvl--;
1031 
1032  projectionError = 90.0;
1033 
1034  if (scales[lvl]) {
1035  vpHomogeneousMatrix cMo_1 = m_cMo;
1036  try {
1037  downScale(lvl);
1038 
1039  try {
1040  trackMovingEdge(*Ipyramid[lvl]);
1041  } catch (...) {
1042  vpTRACE("Error in moving edge tracking");
1043  throw;
1044  }
1045 
1046  // initialize the vector that contains the error and the matrix that
1047  // contains the interaction matrix AY: Useless as it is done in
1048  // coputeVVS()
1049  /*
1050  for(std::list<vpMbtDistanceLine*>::const_iterator
1051  it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
1052  (l->isVisible()){ l->initInteractionMatrixError();
1053  }
1054  }
1055 
1056  for(std::list<vpMbtDistanceCylinder*>::const_iterator
1057  it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
1058  if(cy->isVisible()) {
1059  cy->initInteractionMatrixError();
1060  }
1061  }
1062 
1063  for(std::list<vpMbtDistanceCircle*>::const_iterator
1064  it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
1065  (ci->isVisible()){ ci->initInteractionMatrixError();
1066  }
1067  }
1068  */
1069 
1070  try {
1071  computeVVS(*Ipyramid[lvl], lvl);
1072  } catch (...) {
1073  covarianceMatrix = -1;
1074  throw; // throw the original exception
1075  }
1076 
1077  testTracking();
1078 
1079  if (displayFeatures) {
1081  }
1082 
1083  // Looking for new visible face
1084  bool newvisibleface = false;
1085  visibleFace(I, m_cMo, newvisibleface);
1086 
1087  // cam.computeFov(I.getWidth(), I.getHeight());
1088  if (useScanLine) {
1091  }
1092 
1093  updateMovingEdge(I);
1094 
1095  initMovingEdge(I, m_cMo);
1096  // Reinit the moving edge for the lines which need it.
1097  reinitMovingEdge(I, m_cMo);
1098 
1099  if (computeProjError)
1101 
1102  upScale(lvl);
1103  } catch (const vpException &e) {
1104  if (lvl != 0) {
1105  m_cMo = cMo_1;
1106  reInitLevel(lvl);
1107  upScale(lvl);
1108  } else {
1109  upScale(lvl);
1110  throw(e);
1111  }
1112  }
1113  }
1114  } while (lvl != 0);
1115 
1117 }
1118 
1120 {
1122  track(m_I);
1123 }
1124 
1131 {
1132  if (!modelInitialised) {
1133  throw vpException(vpException::fatalError, "model not initialized");
1134  }
1135 
1136  bool a = false;
1137 
1138 #ifdef VISP_HAVE_OGRE
1139  if (useOgre) {
1140  if (!faces.isOgreInitialised()) {
1143  faces.initOgre(m_cam);
1144  // Turn off Ogre config dialog display for the next call to this
1145  // function since settings are saved in the ogre.cfg file and used
1146  // during the next call
1147  ogreShowConfigDialog = false;
1148  }
1149  }
1150 #endif
1151 
1152  if (clippingFlag > 2)
1153  m_cam.computeFov(I.getWidth(), I.getHeight());
1154 
1155  visibleFace(I, m_cMo, a);
1156  resetMovingEdge();
1157 
1158  if (useScanLine) {
1159  if (clippingFlag <= 2)
1160  m_cam.computeFov(I.getWidth(), I.getHeight());
1161 
1164  }
1165 
1166  initPyramid(I, Ipyramid);
1167  unsigned int i = (unsigned int)scales.size();
1168  do {
1169  i--;
1170  if (scales[i]) {
1171  downScale(i);
1173  upScale(i);
1174  }
1175  } while (i != 0);
1176 
1178 }
1179 
1188 {
1189  m_cMo = cdMo;
1190 
1191  init(I);
1192 }
1193 
1202 {
1203  m_cMo = cdMo;
1204 
1205  vpImageConvert::convert(I_color, m_I);
1206  init(m_I);
1207 }
1208 
1220 void vpMbEdgeTracker::loadConfigFile(const std::string &configFile, bool verbose)
1221 {
1222  // Load projection error config
1223  vpMbTracker::loadConfigFile(configFile, verbose);
1224 
1226  xmlp.setVerbose(verbose);
1227  xmlp.setCameraParameters(m_cam);
1230  xmlp.setEdgeMe(me);
1231 
1232  try {
1233  if (verbose) {
1234  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1235  }
1236  xmlp.parse(configFile);
1237  } catch (...) {
1238  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
1239  }
1240 
1241  vpCameraParameters camera;
1242  vpMe meParser;
1243  xmlp.getCameraParameters(camera);
1244  xmlp.getEdgeMe(meParser);
1245 
1246  setCameraParameters(camera);
1247  setMovingEdge(meParser);
1250 
1251  if (xmlp.hasNearClippingDistance())
1253 
1254  if (xmlp.hasFarClippingDistance())
1256 
1257  if (xmlp.getFovClipping())
1259 
1260  useLodGeneral = xmlp.getLodState();
1263 
1264  applyLodSettingInConfig = false;
1265  if (this->getNbPolygon() > 0) {
1266  applyLodSettingInConfig = true;
1270  }
1271 }
1272 
1285  const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1286  bool displayFullModel)
1287 {
1288  // Display first the Moving-Edges
1289  if (displayFeatures) {
1291  }
1292 
1293  std::vector<std::vector<double> > models =
1294  vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1295 
1296  for (size_t i = 0; i < models.size(); i++) {
1297  if (vpMath::equal(models[i][0], 0)) {
1298  vpImagePoint ip1(models[i][1], models[i][2]);
1299  vpImagePoint ip2(models[i][3], models[i][4]);
1300  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1301  } else if (vpMath::equal(models[i][0], 1)) {
1302  vpImagePoint center(models[i][1], models[i][2]);
1303  double n20 = models[i][3];
1304  double n11 = models[i][4];
1305  double n02 = models[i][5];
1306  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1307  }
1308  }
1309 
1310 #ifdef VISP_HAVE_OGRE
1311  if (useOgre)
1312  faces.displayOgre(cMo);
1313 #endif
1314 }
1315 
1328  const vpColor &col, unsigned int thickness, bool displayFullModel)
1329 {
1330  // Display first the Moving-Edges
1331  if (displayFeatures) {
1333  }
1334 
1335  std::vector<std::vector<double> > models =
1336  vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1337 
1338  for (size_t i = 0; i < models.size(); i++) {
1339  if (vpMath::equal(models[i][0], 0)) {
1340  vpImagePoint ip1(models[i][1], models[i][2]);
1341  vpImagePoint ip2(models[i][3], models[i][4]);
1342  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1343  } else if (vpMath::equal(models[i][0], 1)) {
1344  vpImagePoint center(models[i][1], models[i][2]);
1345  double n20 = models[i][3];
1346  double n11 = models[i][4];
1347  double n02 = models[i][5];
1348  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1349  }
1350  }
1351 
1352 #ifdef VISP_HAVE_OGRE
1353  if (useOgre)
1354  faces.displayOgre(cMo);
1355 #endif
1356 }
1357 
1358 std::vector<std::vector<double> > vpMbEdgeTracker::getFeaturesForDisplayEdge()
1359 {
1360  std::vector<std::vector<double> > features;
1361 
1362  const unsigned int lvl = 0;
1363  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1364  vpMbtDistanceLine *l = *it;
1365  if (l->isVisible() && l->isTracked()) {
1366  std::vector<std::vector<double> > currentFeatures = l->getFeaturesForDisplay();
1367  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1368  }
1369  }
1370 
1371  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1372  ++it) {
1373  vpMbtDistanceCylinder *cy = *it;
1374  if (cy->isVisible() && cy->isTracked()) {
1375  std::vector<std::vector<double> > currentFeatures = cy->getFeaturesForDisplay();
1376  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1377  }
1378  }
1379 
1380  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1381  vpMbtDistanceCircle *ci = *it;
1382  if (ci->isVisible() && ci->isTracked()) {
1383  std::vector<std::vector<double> > currentFeatures = ci->getFeaturesForDisplay();
1384  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1385  }
1386  }
1387 
1388  return features;
1389 }
1390 
1406 std::vector<std::vector<double> > vpMbEdgeTracker::getModelForDisplay(unsigned int width, unsigned int height,
1407  const vpHomogeneousMatrix &cMo,
1408  const vpCameraParameters &cam,
1409  bool displayFullModel)
1410 {
1411  std::vector<std::vector<double> > models;
1412 
1413  for (unsigned int i = 0; i < scales.size(); i += 1) {
1414  if (scales[i]) {
1415  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1416  ++it) {
1417  std::vector<std::vector<double> > currentModel =
1418  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1419  models.insert(models.end(), currentModel.begin(), currentModel.end());
1420  }
1421 
1422  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1423  it != cylinders[scaleLevel].end(); ++it) {
1424  std::vector<std::vector<double> > currentModel =
1425  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1426  models.insert(models.end(), currentModel.begin(), currentModel.end());
1427  }
1428 
1429  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1430  it != circles[scaleLevel].end(); ++it) {
1431  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1432  if (!paramsCircle.empty()) {
1433  models.push_back(paramsCircle);
1434  }
1435  }
1436  break; // displaying model on one scale only
1437  }
1438  }
1439 
1440  return models;
1441 }
1442 
1444 {
1445  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1448  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1449 
1450  switch (state) {
1453  break;
1454 
1455  case vpMeSite::CONTRAST:
1456  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1457  break;
1458 
1459  case vpMeSite::THRESHOLD:
1461  break;
1462 
1463  case vpMeSite::M_ESTIMATOR:
1464  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1465  break;
1466 
1467  case vpMeSite::TOO_NEAR:
1468  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1469  break;
1470 
1471  default:
1473  }
1474  }
1475  }
1476 }
1477 
1479 {
1480  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1483  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1484 
1485  switch (state) {
1488  break;
1489 
1490  case vpMeSite::CONTRAST:
1491  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1492  break;
1493 
1494  case vpMeSite::THRESHOLD:
1496  break;
1497 
1498  case vpMeSite::M_ESTIMATOR:
1499  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1500  break;
1501 
1502  case vpMeSite::TOO_NEAR:
1503  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1504  break;
1505 
1506  default:
1508  }
1509  }
1510  }
1511 }
1512 
1522 {
1523  const bool doNotTrack = false;
1524 
1525  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1526  ++it) {
1527  vpMbtDistanceLine *l = *it;
1528  bool isvisible = false;
1529 
1530  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1531  ++itindex) {
1532  int index = *itindex;
1533  if (index == -1)
1534  isvisible = true;
1535  else {
1536  if (l->hiddenface->isVisible((unsigned int)index))
1537  isvisible = true;
1538  }
1539  }
1540 
1541  // Si la ligne n'appartient a aucune face elle est tout le temps visible
1542  if (l->Lindex_polygon.empty())
1543  isvisible = true; // Not sure that this can occur
1544 
1545  if (isvisible) {
1546  l->setVisible(true);
1547  l->updateTracked();
1548  if (l->meline.empty() && l->isTracked())
1549  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1550  } else {
1551  l->setVisible(false);
1552  for (size_t a = 0; a < l->meline.size(); a++) {
1553  if (l->meline[a] != NULL)
1554  delete l->meline[a];
1555  if (a < l->nbFeature.size())
1556  l->nbFeature[a] = 0;
1557  }
1558  l->nbFeatureTotal = 0;
1559  l->meline.clear();
1560  l->nbFeature.clear();
1561  }
1562  }
1563 
1564  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1565  it != cylinders[scaleLevel].end(); ++it) {
1566  vpMbtDistanceCylinder *cy = *it;
1567 
1568  bool isvisible = false;
1569 
1570  int index = cy->index_polygon;
1571  if (index == -1)
1572  isvisible = true;
1573  else {
1574  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1575  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1576  isvisible = true;
1577  }
1578  // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1579 
1580  if (isvisible) {
1581  cy->setVisible(true);
1582  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1583  if (cy->isTracked())
1584  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1585  }
1586  } else {
1587  cy->setVisible(false);
1588  if (cy->meline1 != NULL)
1589  delete cy->meline1;
1590  if (cy->meline2 != NULL)
1591  delete cy->meline2;
1592  cy->meline1 = NULL;
1593  cy->meline2 = NULL;
1594  cy->nbFeature = 0;
1595  cy->nbFeaturel1 = 0;
1596  cy->nbFeaturel2 = 0;
1597  }
1598  }
1599 
1600  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1601  it != circles[scaleLevel].end(); ++it) {
1602  vpMbtDistanceCircle *ci = *it;
1603  bool isvisible = false;
1604 
1605  int index = ci->index_polygon;
1606  if (index == -1)
1607  isvisible = true;
1608  else {
1609  if (ci->hiddenface->isVisible((unsigned int)index))
1610  isvisible = true;
1611  }
1612 
1613  if (isvisible) {
1614  ci->setVisible(true);
1615  if (ci->meEllipse == NULL) {
1616  if (ci->isTracked())
1617  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1618  }
1619  } else {
1620  ci->setVisible(false);
1621  if (ci->meEllipse != NULL)
1622  delete ci->meEllipse;
1623  ci->meEllipse = NULL;
1624  ci->nbFeature = 0;
1625  }
1626  }
1627 }
1628 
1635 {
1636  const bool doNotTrack = false;
1637 
1638  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1639  ++it) {
1640  vpMbtDistanceLine *l = *it;
1641  if (l->isVisible() && l->isTracked()) {
1642  if (l->meline.empty()) {
1643  l->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1644  }
1645  l->trackMovingEdge(I);
1646  }
1647  }
1648 
1649  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1650  it != cylinders[scaleLevel].end(); ++it) {
1651  vpMbtDistanceCylinder *cy = *it;
1652  if (cy->isVisible() && cy->isTracked()) {
1653  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1654  cy->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1655  }
1656  cy->trackMovingEdge(I, m_cMo);
1657  }
1658  }
1659 
1660  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1661  it != circles[scaleLevel].end(); ++it) {
1662  vpMbtDistanceCircle *ci = *it;
1663  if (ci->isVisible() && ci->isTracked()) {
1664  if (ci->meEllipse == NULL) {
1665  ci->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1666  }
1667  ci->trackMovingEdge(I, m_cMo);
1668  }
1669  }
1670 }
1671 
1678 {
1679  vpMbtDistanceLine *l;
1680  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1681  ++it) {
1682  if ((*it)->isTracked()) {
1683  l = *it;
1684  l->updateMovingEdge(I, m_cMo);
1685  if (l->nbFeatureTotal == 0 && l->isVisible()) {
1686  l->Reinit = true;
1687  }
1688  }
1689  }
1690 
1692  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1693  it != cylinders[scaleLevel].end(); ++it) {
1694  if ((*it)->isTracked()) {
1695  cy = *it;
1696  cy->updateMovingEdge(I, m_cMo);
1697  if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1698  cy->Reinit = true;
1699  }
1700  }
1701  }
1702 
1703  vpMbtDistanceCircle *ci;
1704  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1705  it != circles[scaleLevel].end(); ++it) {
1706  if ((*it)->isTracked()) {
1707  ci = *it;
1708  ci->updateMovingEdge(I, m_cMo);
1709  if (ci->nbFeature == 0 && ci->isVisible()) {
1710  ci->Reinit = true;
1711  }
1712  }
1713  }
1714 }
1715 
1717 {
1718  unsigned int n = 0;
1719 
1720  vpMbtDistanceLine *l;
1721  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1722  ++it) {
1723  if ((*it)->isTracked()) {
1724  l = *it;
1725  unsigned int indexLine = 0;
1726  double wmean = 0;
1727  for (size_t a = 0; a < l->meline.size(); a++) {
1728  if (l->nbFeature[a] > 0) {
1729  std::list<vpMeSite>::iterator itListLine;
1730  itListLine = l->meline[a]->getMeList().begin();
1731 
1732  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1733  wmean += m_w_edge[n + indexLine];
1734  vpMeSite p = *itListLine;
1735  if (m_w_edge[n + indexLine] < 0.5) {
1737 
1738  *itListLine = p;
1739  }
1740 
1741  ++itListLine;
1742  indexLine++;
1743  }
1744  }
1745  }
1746  n += l->nbFeatureTotal;
1747 
1748  if (l->nbFeatureTotal != 0)
1749  wmean /= l->nbFeatureTotal;
1750  else
1751  wmean = 1;
1752 
1753  l->setMeanWeight(wmean);
1754 
1755  if (wmean < 0.8)
1756  l->Reinit = true;
1757  }
1758  }
1759 
1760  // Same thing with cylinders as with lines
1762  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1763  it != cylinders[scaleLevel].end(); ++it) {
1764  if ((*it)->isTracked()) {
1765  cy = *it;
1766  double wmean = 0;
1767  std::list<vpMeSite>::iterator itListCyl1;
1768  std::list<vpMeSite>::iterator itListCyl2;
1769 
1770  if (cy->nbFeature > 0) {
1771  itListCyl1 = cy->meline1->getMeList().begin();
1772  itListCyl2 = cy->meline2->getMeList().begin();
1773 
1774  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1775  wmean += m_w_edge[n + i];
1776  vpMeSite p = *itListCyl1;
1777  if (m_w_edge[n + i] < 0.5) {
1779 
1780  *itListCyl1 = p;
1781  }
1782 
1783  ++itListCyl1;
1784  }
1785  }
1786 
1787  if (cy->nbFeaturel1 != 0)
1788  wmean /= cy->nbFeaturel1;
1789  else
1790  wmean = 1;
1791 
1792  cy->setMeanWeight1(wmean);
1793 
1794  if (wmean < 0.8) {
1795  cy->Reinit = true;
1796  }
1797 
1798  wmean = 0;
1799  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1800  wmean += m_w_edge[n + i];
1801  vpMeSite p = *itListCyl2;
1802  if (m_w_edge[n + i] < 0.5) {
1804 
1805  *itListCyl2 = p;
1806  }
1807 
1808  ++itListCyl2;
1809  }
1810 
1811  if (cy->nbFeaturel2 != 0)
1812  wmean /= cy->nbFeaturel2;
1813  else
1814  wmean = 1;
1815 
1816  cy->setMeanWeight2(wmean);
1817 
1818  if (wmean < 0.8) {
1819  cy->Reinit = true;
1820  }
1821 
1822  n += cy->nbFeature;
1823  }
1824  }
1825 
1826  // Same thing with circles as with lines
1827  vpMbtDistanceCircle *ci;
1828  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1829  it != circles[scaleLevel].end(); ++it) {
1830  if ((*it)->isTracked()) {
1831  ci = *it;
1832  double wmean = 0;
1833  std::list<vpMeSite>::iterator itListCir;
1834 
1835  if (ci->nbFeature > 0) {
1836  itListCir = ci->meEllipse->getMeList().begin();
1837  }
1838 
1839  wmean = 0;
1840  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1841  wmean += m_w_edge[n + i];
1842  vpMeSite p = *itListCir;
1843  if (m_w_edge[n + i] < 0.5) {
1845 
1846  *itListCir = p;
1847  }
1848 
1849  ++itListCir;
1850  }
1851 
1852  if (ci->nbFeature != 0)
1853  wmean /= ci->nbFeature;
1854  else
1855  wmean = 1;
1856 
1857  ci->setMeanWeight(wmean);
1858 
1859  if (wmean < 0.8) {
1860  ci->Reinit = true;
1861  }
1862 
1863  n += ci->nbFeature;
1864  }
1865  }
1866 }
1867 
1878 {
1879  vpMbtDistanceLine *l;
1880  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1881  ++it) {
1882  if ((*it)->isTracked()) {
1883  l = *it;
1884  if (l->Reinit && l->isVisible())
1885  l->reinitMovingEdge(I, _cMo, m_mask);
1886  }
1887  }
1888 
1890  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1891  it != cylinders[scaleLevel].end(); ++it) {
1892  if ((*it)->isTracked()) {
1893  cy = *it;
1894  if (cy->Reinit && cy->isVisible())
1895  cy->reinitMovingEdge(I, _cMo, m_mask);
1896  }
1897  }
1898 
1899  vpMbtDistanceCircle *ci;
1900  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1901  it != circles[scaleLevel].end(); ++it) {
1902  if ((*it)->isTracked()) {
1903  ci = *it;
1904  if (ci->Reinit && ci->isVisible())
1905  ci->reinitMovingEdge(I, _cMo, m_mask);
1906  }
1907  }
1908 }
1909 
1911 {
1912  // Clear ME to be displayed
1914 
1915  for (unsigned int i = 0; i < scales.size(); i += 1) {
1916  if (scales[i]) {
1917  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1918  for (size_t a = 0; a < (*it)->meline.size(); a++) {
1919  if ((*it)->meline[a] != NULL) {
1920  delete (*it)->meline[a];
1921  (*it)->meline[a] = NULL;
1922  }
1923  }
1924 
1925  (*it)->meline.clear();
1926  (*it)->nbFeature.clear();
1927  (*it)->nbFeatureTotal = 0;
1928  }
1929 
1930  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1931  ++it) {
1932  if ((*it)->meline1 != NULL) {
1933  delete (*it)->meline1;
1934  (*it)->meline1 = NULL;
1935  }
1936  if ((*it)->meline2 != NULL) {
1937  delete (*it)->meline2;
1938  (*it)->meline2 = NULL;
1939  }
1940 
1941  (*it)->nbFeature = 0;
1942  (*it)->nbFeaturel1 = 0;
1943  (*it)->nbFeaturel2 = 0;
1944  }
1945 
1946  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1947  if ((*it)->meEllipse != NULL) {
1948  delete (*it)->meEllipse;
1949  (*it)->meEllipse = NULL;
1950  }
1951  (*it)->nbFeature = 0;
1952  }
1953  }
1954  }
1955 }
1956 
1969 void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1970 {
1971  {
1972  // suppress line already in the model
1973  bool already_here = false;
1974  vpMbtDistanceLine *l;
1975 
1976  for (unsigned int i = 0; i < scales.size(); i += 1) {
1977  if (scales[i]) {
1978  downScale(i);
1979  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1980  l = *it;
1981  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
1982  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
1983  already_here = true;
1984  l->addPolygon(polygon);
1985  l->hiddenface = &faces;
1986  }
1987  }
1988 
1989  if (!already_here) {
1990  l = new vpMbtDistanceLine;
1991 
1993  l->buildFrom(P1, P2, m_rand);
1994  l->addPolygon(polygon);
1995  l->setMovingEdge(&me);
1996  l->hiddenface = &faces;
1997  l->useScanLine = useScanLine;
1998 
1999  l->setIndex(nline);
2000  l->setName(name);
2001 
2004 
2007 
2010 
2011  nline += 1;
2012  lines[i].push_back(l);
2013  }
2014  upScale(i);
2015  }
2016  }
2017  }
2018 }
2019 
2025 void vpMbEdgeTracker::removeLine(const std::string &name)
2026 {
2027  vpMbtDistanceLine *l;
2028 
2029  for (unsigned int i = 0; i < scales.size(); i++) {
2030  if (scales[i]) {
2031  for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2032  l = *it;
2033  if (name.compare(l->getName()) == 0) {
2034  lines[i].erase(it);
2035  break;
2036  }
2037  }
2038  }
2039  }
2040 }
2041 
2052 void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
2053  const std::string &name)
2054 {
2055  {
2056  bool already_here = false;
2057  vpMbtDistanceCircle *ci;
2058 
2059  for (unsigned int i = 0; i < scales.size(); i += 1) {
2060  if (scales[i]) {
2061  downScale(i);
2062  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2063  ci = *it;
2064  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
2065  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
2066  already_here =
2067  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
2068  }
2069  }
2070 
2071  if (!already_here) {
2072  ci = new vpMbtDistanceCircle;
2073 
2075  ci->buildFrom(P1, P2, P3, r);
2076  ci->setMovingEdge(&me);
2077  ci->setIndex(ncircle);
2078  ci->setName(name);
2079  ci->index_polygon = idFace;
2080  ci->hiddenface = &faces;
2081 
2082  // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
2083  // ci->getPolygon().setClipping(clippingFlag);
2084 
2085  // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
2086  // vpPolygon3D::NEAR_CLIPPING)
2087  // ci->getPolygon().setNearClippingDistance(distNearClip);
2088 
2089  // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
2090  // vpPolygon3D::FAR_CLIPPING)
2091  // ci->getPolygon().setFarClippingDistance(distFarClip);
2092 
2093  ncircle += 1;
2094  circles[i].push_back(ci);
2095  }
2096  upScale(i);
2097  }
2098  }
2099  }
2100 }
2101 
2111 void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace, const std::string &name)
2112 {
2113  {
2114  bool already_here = false;
2116 
2117  for (unsigned int i = 0; i < scales.size(); i += 1) {
2118  if (scales[i]) {
2119  downScale(i);
2120  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2121  ++it) {
2122  cy = *it;
2123  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
2124  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
2125  already_here =
2126  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
2127  }
2128  }
2129 
2130  if (!already_here) {
2131  cy = new vpMbtDistanceCylinder;
2132 
2134  cy->buildFrom(P1, P2, r);
2135  cy->setMovingEdge(&me);
2136  cy->setIndex(ncylinder);
2137  cy->setName(name);
2138  cy->index_polygon = idFace;
2139  cy->hiddenface = &faces;
2140  ncylinder += 1;
2141  cylinders[i].push_back(cy);
2142  }
2143  upScale(i);
2144  }
2145  }
2146  }
2147 }
2148 
2154 void vpMbEdgeTracker::removeCylinder(const std::string &name)
2155 {
2157 
2158  for (unsigned int i = 0; i < scales.size(); i++) {
2159  if (scales[i]) {
2160  for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
2161  cy = *it;
2162  if (name.compare(cy->getName()) == 0) {
2163  cylinders[i].erase(it);
2164  break;
2165  }
2166  }
2167  }
2168  }
2169 }
2170 
2176 void vpMbEdgeTracker::removeCircle(const std::string &name)
2177 {
2178  vpMbtDistanceCircle *ci;
2179 
2180  for (unsigned int i = 0; i < scales.size(); i++) {
2181  if (scales[i]) {
2182  for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2183  ci = *it;
2184  if (name.compare(ci->getName()) == 0) {
2185  circles[i].erase(it);
2186  break;
2187  }
2188  }
2189  }
2190  }
2191 }
2192 
2199 {
2200  unsigned int nbpt = p.getNbPoint();
2201  if (nbpt > 0) {
2202  for (unsigned int i = 0; i < nbpt - 1; i++)
2203  addLine(p.p[i], p.p[i + 1], p.getIndex());
2204  addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
2205  }
2206 }
2207 
2219 void vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, bool &newvisibleline)
2220 {
2221  unsigned int n;
2222  bool changed = false;
2223 
2224  if (!useOgre) {
2225  // n = faces.setVisible(_I.getWidth(), I.getHeight(), m_cam, cMo, vpMath::rad(89), vpMath::rad(89),
2226  // changed);
2227  n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2228  } else {
2229 #ifdef VISP_HAVE_OGRE
2230  n = faces.setVisibleOgre(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2231 #else
2232  n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2233 #endif
2234  }
2235 
2236  if (n > nbvisiblepolygone) {
2237  // cout << "une nouvelle face est visible " << endl;
2238  newvisibleline = true;
2239  } else
2240  newvisibleline = false;
2241 
2242  nbvisiblepolygone = n;
2243 }
2244 
2261 {
2262  unsigned int nbpt = polygon.getNbPoint();
2263  if (nbpt > 0) {
2264  for (unsigned int i = 0; i < nbpt - 1; i++)
2265  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2266  vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
2267  }
2268 }
2285 {
2286  unsigned int nbpt = polygon.getNbPoint();
2287  if (nbpt > 0) {
2288  for (unsigned int i = 0; i < nbpt - 1; i++)
2289  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2290  }
2291 }
2292 
2293 unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
2294  unsigned int &nberrors_circles)
2295 {
2296  unsigned int nbrow = 0;
2297  nberrors_lines = 0;
2298  nberrors_cylinders = 0;
2299  nberrors_circles = 0;
2300 
2301  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2302  ++it) {
2303 
2304  vpMbtDistanceLine *l = *it;
2305 
2306  if (l->isTracked()) {
2308  nbrow += l->nbFeatureTotal;
2309  nberrors_lines += l->nbFeatureTotal;
2310  }
2311  }
2312 
2313  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2314  it != cylinders[scaleLevel].end(); ++it) {
2315  vpMbtDistanceCylinder *cy = *it;
2316 
2317  if (cy->isTracked()) {
2319  nbrow += cy->nbFeature;
2320  nberrors_cylinders += cy->nbFeature;
2321  }
2322  }
2323 
2324  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2325  it != circles[scaleLevel].end(); ++it) {
2326  vpMbtDistanceCircle *ci = *it;
2327 
2328  if (ci->isTracked()) {
2330  nbrow += ci->nbFeature;
2331  nberrors_circles += ci->nbFeature;
2332  }
2333  }
2334 
2335  return nbrow;
2336 }
2337 
2349 void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
2350  const std::string &name)
2351 {
2352  addCircle(p1, p2, p3, radius, (int)idFace, name);
2353 }
2354 
2365 void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
2366  const std::string &name)
2367 {
2368  addCylinder(p1, p2, radius, (int)idFace, name);
2369 }
2370 
2377 {
2378  m_cMo.eye();
2379  vpMbtDistanceLine *l;
2381  vpMbtDistanceCircle *ci;
2382 
2383  for (unsigned int i = 0; i < scales.size(); i += 1) {
2384  if (scales[i]) {
2385  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2386  l = *it;
2387  if (l != NULL)
2388  delete l;
2389  l = NULL;
2390  }
2391 
2392  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2393  ++it) {
2394  cy = *it;
2395  if (cy != NULL)
2396  delete cy;
2397  cy = NULL;
2398  }
2399 
2400  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2401  ci = *it;
2402  if (ci != NULL)
2403  delete ci;
2404  ci = NULL;
2405  }
2406  lines[i].clear();
2407  cylinders[i].clear();
2408  circles[i].clear();
2409  }
2410  }
2411 
2412  faces.reset();
2413 
2414  useScanLine = false;
2415 
2416 #ifdef VISP_HAVE_OGRE
2417  useOgre = false;
2418 #endif
2419 
2420  m_computeInteraction = true;
2421  nline = 0;
2422  ncylinder = 0;
2423  m_lambda = 1.0;
2424  nbvisiblepolygone = 0;
2425  percentageGdPt = 0.4;
2426 
2427  angleAppears = vpMath::rad(89);
2430 
2432 
2433  // reinitialization of the scales.
2434  this->setScales(scales);
2435 }
2436 
2449 void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2450  const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
2451 {
2452  m_cMo.eye();
2453  vpMbtDistanceLine *l;
2455  vpMbtDistanceCircle *ci;
2456 
2457  for (unsigned int i = 0; i < scales.size(); i += 1) {
2458  if (scales[i]) {
2459  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2460  l = *it;
2461  if (l != NULL)
2462  delete l;
2463  l = NULL;
2464  }
2465 
2466  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2467  ++it) {
2468  cy = *it;
2469  if (cy != NULL)
2470  delete cy;
2471  cy = NULL;
2472  }
2473 
2474  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2475  ci = *it;
2476  if (ci != NULL)
2477  delete ci;
2478  ci = NULL;
2479  }
2480 
2481  lines[i].clear();
2482  cylinders[i].clear();
2483  circles[i].clear();
2484  }
2485  }
2486 
2487  faces.reset();
2488 
2489  // compute_interaction=1;
2490  nline = 0;
2491  ncylinder = 0;
2492  ncircle = 0;
2493  // lambda = 1;
2494  nbvisiblepolygone = 0;
2495 
2496  loadModel(cad_name, verbose, T);
2497  initFromPose(I, cMo);
2498 }
2499 
2510 unsigned int vpMbEdgeTracker::getNbPoints(unsigned int level) const
2511 {
2512  if ((level > scales.size()) || !scales[level]) {
2513  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2514  level);
2515  }
2516 
2517  unsigned int nbGoodPoints = 0;
2518  vpMbtDistanceLine *l;
2519  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2520  l = *it;
2521  if (l->isVisible() && l->isTracked()) {
2522  for (size_t a = 0; a < l->meline.size(); a++) {
2523  if (l->nbFeature[a] != 0)
2524  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2525  itme != l->meline[a]->getMeList().end(); ++itme) {
2526  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2527  nbGoodPoints++;
2528  }
2529  }
2530  }
2531  }
2532 
2534  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2535  ++it) {
2536  cy = *it;
2537  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
2538  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2539  itme1 != cy->meline1->getMeList().end(); ++itme1) {
2540  if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2541  nbGoodPoints++;
2542  }
2543  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2544  itme2 != cy->meline2->getMeList().end(); ++itme2) {
2545  if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2546  nbGoodPoints++;
2547  }
2548  }
2549  }
2550 
2551  vpMbtDistanceCircle *ci;
2552  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2553  ci = *it;
2554  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
2555  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2556  itme != ci->meEllipse->getMeList().end(); ++itme) {
2557  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2558  nbGoodPoints++;
2559  }
2560  }
2561  }
2562 
2563  return nbGoodPoints;
2564 }
2565 
2587 void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2588 {
2589  unsigned int nbActivatedLevels = 0;
2590  for (unsigned int i = 0; i < scale.size(); i++) {
2591  if (scale[i]) {
2592  nbActivatedLevels++;
2593  }
2594  }
2595 
2596  if (scale.empty() || (nbActivatedLevels == 0)) {
2597  vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2598  "tracking. Use the global one");
2599  this->scales.resize(0);
2600  this->scales.push_back(true);
2601 
2602  lines.resize(1);
2603  lines[0].clear();
2604 
2605  cylinders.resize(1);
2606  cylinders[0].clear();
2607 
2608  circles.resize(1);
2609  circles[0].clear();
2610  } else {
2611  this->scales = scale;
2612 
2613  lines.resize(scale.size());
2614  cylinders.resize(scale.size());
2615  circles.resize(scale.size());
2616 
2617  for (unsigned int i = 0; i < lines.size(); i++) {
2618  lines[i].clear();
2619  cylinders[i].clear();
2620  circles[i].clear();
2621  }
2622  }
2623 }
2624 
2631 {
2633  std::cerr << "Far clipping value cannot be inferior than near clipping "
2634  "value. Far clipping won't be considered."
2635  << std::endl;
2636  else if (dist < 0)
2637  std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2638  "won't be considered."
2639  << std::endl;
2640  else {
2642  vpMbtDistanceLine *l;
2643 
2644  for (unsigned int i = 0; i < scales.size(); i += 1) {
2645  if (scales[i]) {
2646  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2647  l = *it;
2649  }
2650  }
2651  }
2652  }
2653 }
2654 
2661 {
2663  std::cerr << "Near clipping value cannot be superior than far clipping "
2664  "value. Near clipping won't be considered."
2665  << std::endl;
2666  else if (dist < 0)
2667  std::cerr << "Near clipping value cannot be inferior than 0. Near "
2668  "clipping won't be considered."
2669  << std::endl;
2670  else {
2672  vpMbtDistanceLine *l;
2673 
2674  for (unsigned int i = 0; i < scales.size(); i += 1) {
2675  if (scales[i]) {
2676  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2677  l = *it;
2679  }
2680  }
2681  }
2682  }
2683 }
2684 
2692 void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2693 {
2694  vpMbTracker::setClipping(flags);
2695 
2696  vpMbtDistanceLine *l;
2697 
2698  for (unsigned int i = 0; i < scales.size(); i += 1) {
2699  if (scales[i]) {
2700  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2701  l = *it;
2703  }
2704  }
2705  }
2706 }
2707 
2724  std::vector<const vpImage<unsigned char> *> &_pyramid)
2725 {
2726  _pyramid.resize(scales.size());
2727 
2728  if (scales[0]) {
2729  _pyramid[0] = &_I;
2730  } else {
2731  _pyramid[0] = NULL;
2732  }
2733 
2734  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2735  if (scales[i]) {
2736  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2737  vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2738 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2739  IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
2740  vpI0->imageData = (char *)(_I.bitmap);
2741  IplImage *vpI =
2742  cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
2743  cvResize(vpI0, vpI, CV_INTER_NN);
2744  vpImageConvert::convert(vpI, *I);
2745  cvReleaseImage(&vpI);
2746  vpI0->imageData = NULL;
2747  cvReleaseImageHeader(&vpI0);
2748 #else
2749  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2750  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2751  (*I)[k][l] = _I[ii][jj];
2752  }
2753  }
2754 #endif
2755  _pyramid[i] = I;
2756  } else {
2757  _pyramid[i] = NULL;
2758  }
2759  }
2760 }
2761 
2768 void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2769 {
2770  if (_pyramid.size() > 0) {
2771  _pyramid[0] = NULL;
2772  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2773  if (_pyramid[i] != NULL) {
2774  delete _pyramid[i];
2775  _pyramid[i] = NULL;
2776  }
2777  }
2778  _pyramid.resize(0);
2779  }
2780 }
2781 
2792 void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level) const
2793 {
2794  if (level > scales.size() || !scales[level]) {
2795  std::ostringstream oss;
2796  oss << level;
2797  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2798  throw vpException(vpException::dimensionError, errorMsg);
2799  }
2800 
2801  linesList = lines[level];
2802 }
2803 
2814 void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level) const
2815 {
2816  if (level > scales.size() || !scales[level]) {
2817  std::ostringstream oss;
2818  oss << level;
2819  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2820  throw vpException(vpException::dimensionError, errorMsg);
2821  }
2822 
2823  cylindersList = cylinders[level];
2824 }
2825 
2836 void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level) const
2837 {
2838  if (level > scales.size() || !scales[level]) {
2839  std::ostringstream oss;
2840  oss << level;
2841  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2842  throw vpException(vpException::dimensionError, errorMsg);
2843  }
2844 
2845  circlesList = circles[level];
2846 }
2847 
2854 void vpMbEdgeTracker::downScale(const unsigned int _scale)
2855 {
2856  const double ratio = pow(2., (int)_scale);
2857  scaleLevel = _scale;
2858 
2859  vpMatrix K = m_cam.get_K();
2860 
2861  K[0][0] /= ratio;
2862  K[1][1] /= ratio;
2863  K[0][2] /= ratio;
2864  K[1][2] /= ratio;
2865 
2867 }
2868 
2875 void vpMbEdgeTracker::upScale(const unsigned int _scale)
2876 {
2877  const double ratio = pow(2., (int)_scale);
2878  scaleLevel = 0;
2879 
2880  vpMatrix K = m_cam.get_K();
2881 
2882  K[0][0] *= ratio;
2883  K[1][1] *= ratio;
2884  K[0][2] *= ratio;
2885  K[1][2] *= ratio;
2886 
2888 }
2889 
2897 void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2898 {
2899  unsigned int scaleLevel_1 = scaleLevel;
2900  scaleLevel = _lvl;
2901 
2902  vpMbtDistanceLine *l;
2903  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2904  ++it) {
2905  if ((*it)->isTracked()) {
2906  l = *it;
2907  l->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2908  }
2909  }
2910 
2912  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2913  it != cylinders[scaleLevel].end(); ++it) {
2914  if ((*it)->isTracked()) {
2915  cy = *it;
2916  cy->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2917  }
2918  }
2919 
2920  vpMbtDistanceCircle *ci;
2921  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2922  it != circles[scaleLevel].end(); ++it) {
2923  if ((*it)->isTracked()) {
2924  ci = *it;
2925  ci->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2926  }
2927  }
2928 
2929  trackMovingEdge(*Ipyramid[_lvl]);
2930  updateMovingEdge(*Ipyramid[_lvl]);
2931  scaleLevel = scaleLevel_1;
2932 }
2933 
2941 void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2942 {
2943  for (unsigned int i = 0; i < scales.size(); i += 1) {
2944  if (scales[i]) {
2945  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2946  /*(*it)->setTracked(useEdgeTracking);
2947  for(std::list<int>::const_iterator
2948  itpoly=(*it)->Lindex_polygon.begin();
2949  itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2950  if(faces[(*itpoly)]->getName() != name){
2951  (*it)->setTracked(true);
2952  break;
2953  }
2954  }*/
2955 
2956  (*it)->setTracked(name, useEdgeTracking);
2957  }
2958 
2959  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2960  ++it) {
2961  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2962  (*it)->setTracked(useEdgeTracking);
2963  }
2964  }
2965 
2966  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2967  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2968  (*it)->setTracked(useEdgeTracking);
2969  }
2970  }
2971  }
2972  }
2973 }
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:307
unsigned int getRows() const
Definition: vpArray2D.h:292
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:172
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor yellow
Definition: vpColor.h:225
static const vpColor green
Definition: vpColor.h:220
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 emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
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:89
unsigned int getWidth() const
Definition: vpImage.h:247
Type * bitmap
points toward the bitmap
Definition: vpImage.h:144
unsigned int getHeight() const
Definition: vpImage.h:189
static double rad(double deg)
Definition: vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:170
static double sqr(double x)
Definition: vpMath.h:122
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:367
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:178
static int sign(double x)
Definition: vpMath.h:340
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:447
vpMatrix AtA() const
Definition: vpMatrix.cpp:623
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2232
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)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setNearClippingDistance(const double &dist)
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
virtual void setFarClippingDistance(const double &dist)
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 computeVVSInteractionMatrixAndResidu()
virtual void track(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)
void removeCylinder(const std::string &name)
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.
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="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void setClipping(const unsigned int &flags)
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
void removeLine(const std::string &name)
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
void setScales(const std::vector< bool > &_scales)
void trackMovingEdge(const vpImage< unsigned char > &I)
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual unsigned int getNbPoints(unsigned int level=0) const
vpColVector m_weightedError_edge
Weighted error.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
unsigned int ncircle
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
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
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void reInitLevel(const unsigned int _lvl)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
vpRobust m_robustLines
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void setMovingEdge(const vpMe &me)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
vpColVector m_wCylinders
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
vpColVector m_factor
Edge VVS variables.
vpColVector m_errorLines
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
vpColVector m_wCircles
void addPolygon(vpMbtPolygon &p)
virtual ~vpMbEdgeTracker()
virtual void init(const vpImage< unsigned char > &I)
unsigned int nline
virtual void testTracking()
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
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=NULL, vpColVector *const m_w_prev=NULL)
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
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
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
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
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 reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
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.
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
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 setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
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)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
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
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
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)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
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 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:67
std::string getName() const
Definition: vpMbtPolygon.h:108
int getIndex() const
Definition: vpMbtPolygon.h:101
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:67
@ TOO_NEAR
Point removed because too near image borders.
Definition: vpMeSite.h:81
@ THRESHOLD
Point removed due to a threshold problem.
Definition: vpMeSite.h:79
@ CONTRAST
Point removed due to a contrast problem.
Definition: vpMeSite.h:75
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:80
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:74
vpMeSiteState getState() const
Definition: vpMeSite.h:189
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:175
Definition: vpMe.h:119
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393