Visual Servoing Platform  version 3.2.1 under development (2019-08-08)
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, const 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_ = 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(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 = 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, const unsigned int iter, double &count,
291  const unsigned int lvl)
292 {
296 
297  double limite = 3; // Une limite de 3 pixels
298  limite = limite / 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  unsigned int indexFeature = 0;
583  for (size_t a = 0; a < l->meline.size(); a++) {
584  std::list<vpMeSite>::const_iterator itListLine;
585  if (l->meline[a] != NULL) {
586  itListLine = l->meline[a]->getMeList().begin();
587 
588  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
589  m_factor[n + i] = fac;
590  vpMeSite site = *itListLine;
591  if (site.getState() != vpMeSite::NO_SUPPRESSION)
592  m_factor[n + i] = 0.2;
593  ++itListLine;
594  indexFeature++;
595  }
596  n += l->nbFeature[a];
597  }
598  }
599  }
600  }
601 
602  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
603  ++it) {
604  if ((*it)->isTracked()) {
605  cy = *it;
607 
608  std::list<vpMeSite>::const_iterator itCyl1;
609  std::list<vpMeSite>::const_iterator itCyl2;
610  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
611  itCyl1 = cy->meline1->getMeList().begin();
612  itCyl2 = cy->meline2->getMeList().begin();
613 
614  double fac = 1.0;
615  for (unsigned int i = 0; i < cy->nbFeature; i++) {
616  m_factor[n + i] = fac;
617  vpMeSite site;
618  if (i < cy->nbFeaturel1) {
619  site = *itCyl1;
620  ++itCyl1;
621  } else {
622  site = *itCyl2;
623  ++itCyl2;
624  }
625  if (site.getState() != vpMeSite::NO_SUPPRESSION)
626  m_factor[n + i] = 0.2;
627  }
628  n += cy->nbFeature;
629  }
630  }
631  }
632 
633  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
634  if ((*it)->isTracked()) {
635  ci = *it;
637 
638  std::list<vpMeSite>::const_iterator itCir;
639  if (ci->meEllipse != NULL) {
640  itCir = ci->meEllipse->getMeList().begin();
641  double fac = 1.0;
642 
643  for (unsigned int i = 0; i < ci->nbFeature; i++) {
644  m_factor[n + i] = fac;
645  vpMeSite site = *itCir;
646  if (site.getState() != vpMeSite::NO_SUPPRESSION)
647  m_factor[n + i] = 0.2;
648  ++itCir;
649  }
650  n += ci->nbFeature;
651  }
652  }
653  }
654 }
655 
656 void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
657 {
658  unsigned int nerror = m_weightedError_edge.getRows();
659 
660  double wi, eri;
661  if ((iter == 0) || m_computeInteraction) {
662  for (unsigned int i = 0; i < nerror; i++) {
663  wi = m_w_edge[i] * m_factor[i];
664  eri = m_error_edge[i];
665 
666  m_weightedError_edge[i] = wi * eri;
667 
668  for (unsigned int j = 0; j < 6; j++) {
669  m_L_edge[i][j] = wi * m_L_edge[i][j];
670  }
671  }
672  } else {
673  for (unsigned int i = 0; i < nerror; i++) {
674  wi = m_w_edge[i] * m_factor[i];
675  eri = m_error_edge[i];
676 
677  m_weightedError_edge[i] = wi * eri;
678  }
679  }
680 
682 
683  // If all the 6 dof should be estimated, we check if the interaction matrix
684  // is full rank. If not we remove automatically the dof that cannot be
685  // estimated This is particularly useful when consering circles (rank 5) and
686  // cylinders (rank 4)
687  if (isoJoIdentity_) {
688  cVo.buildFrom(cMo);
689 
690  vpMatrix K; // kernel
691  unsigned int rank = (m_L_edge * cVo).kernel(K);
692  if (rank == 0) {
693  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
694  }
695  if (rank != 6) {
696  vpMatrix I; // Identity
697  I.eye(6);
698  oJo = I - K.AtA();
699 
700  isoJoIdentity_ = false;
701  }
702  }
703 
704  vpColVector v;
705  vpMatrix LTL;
706  vpColVector LTR;
707 
708  if (isoJoIdentity_) {
709  LTL = m_L_edge.AtA();
711  v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
712  } else {
713  cVo.buildFrom(cMo);
714  vpMatrix LVJ = (m_L_edge * cVo * oJo);
715  vpMatrix LVJTLVJ = (LVJ).AtA();
716  vpColVector LVJTR;
717  computeJTR(LVJ, m_weightedError_edge, LVJTR);
718  v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
719  v = cVo * v;
720  }
721 
723 }
724 
726 {
727  // Nombre de moving edges
728  unsigned int nbrow = 0;
729  unsigned int nberrors_lines = 0;
730  unsigned int nberrors_cylinders = 0;
731  unsigned int nberrors_circles = 0;
732 
733  nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
734 
735  if (nbrow == 0) {
737  "No data found to compute the interaction matrix...");
738  }
739 
740  m_L_edge.resize(nbrow, 6, false, false);
741  m_error_edge.resize(nbrow, false);
742 
743  m_weightedError_edge.resize(nbrow, false);
744  m_w_edge.resize(nbrow, false);
745  m_w_edge = 1;
746  m_factor.resize(nbrow, false);
747  m_factor = 1;
748 
749  m_robustLines.resize(nberrors_lines);
750  m_robustCylinders.resize(nberrors_cylinders);
751  m_robustCircles.resize(nberrors_circles);
755 
756  m_wLines.resize(nberrors_lines, false);
757  m_wLines = 1;
758  m_wCylinders.resize(nberrors_cylinders, false);
759  m_wCylinders = 1;
760  m_wCircles.resize(nberrors_circles, false);
761  m_wCircles = 1;
762 
763  m_errorLines.resize(nberrors_lines, false);
764  m_errorCylinders.resize(nberrors_cylinders, false);
765  m_errorCircles.resize(nberrors_circles, false);
766 }
767 
769 {
770  throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
771  "computeVVSInteractionMatrixAndR"
772  "esidu() should not be called!");
773 }
774 
776 {
780 
781  unsigned int n = 0;
782  unsigned int nlines = 0;
783  unsigned int ncylinders = 0;
784  unsigned int ncircles = 0;
785 
786  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
787  ++it) {
788  if ((*it)->isTracked()) {
789  l = *it;
791  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
792  for (unsigned int j = 0; j < 6; j++) {
793  m_L_edge[n + i][j] = l->L[i][j];
794  m_error_edge[n + i] = l->error[i];
795  m_errorLines[nlines + i] = m_error_edge[n + i];
796  }
797  }
798  n += l->nbFeatureTotal;
799  nlines += l->nbFeatureTotal;
800  }
801  }
802 
803  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
804  it != cylinders[scaleLevel].end(); ++it) {
805  if ((*it)->isTracked()) {
806  cy = *it;
808  for (unsigned int i = 0; i < cy->nbFeature; i++) {
809  for (unsigned int j = 0; j < 6; j++) {
810  m_L_edge[n + i][j] = cy->L[i][j];
811  m_error_edge[n + i] = cy->error[i];
812  m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
813  }
814  }
815 
816  n += cy->nbFeature;
817  ncylinders += cy->nbFeature;
818  }
819  }
820 
821  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
822  it != circles[scaleLevel].end(); ++it) {
823  if ((*it)->isTracked()) {
824  ci = *it;
826  for (unsigned int i = 0; i < ci->nbFeature; i++) {
827  for (unsigned int j = 0; j < 6; j++) {
828  m_L_edge[n + i][j] = ci->L[i][j];
829  m_error_edge[n + i] = ci->error[i];
830  m_errorCircles[ncircles + i] = m_error_edge[n + i];
831  }
832  }
833 
834  n += ci->nbFeature;
835  ncircles += ci->nbFeature;
836  }
837  }
838 }
839 
841 {
842  unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
843  nberrors_circles = m_errorCircles.getRows();
844 
845  if (nberrors_lines > 0)
847  if (nberrors_cylinders > 0)
849  if (nberrors_circles > 0)
851 
855 }
856 
866 {
867  projectionError = 0.0;
868  unsigned int nbFeatures = 0;
869  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
870  ++it) {
871  vpMbtDistanceLine *l = *it;
872  if (l->isVisible() && l->isTracked()) {
873  for (size_t a = 0; a < l->meline.size(); a++) {
874  if (l->meline[a] != NULL) {
875  double lineNormGradient;
876  unsigned int lineNbFeatures;
877  l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
880  projectionError += lineNormGradient;
881  nbFeatures += lineNbFeatures;
882  }
883  }
884  }
885  }
886 
887  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
888  it != cylinders[scaleLevel].end(); ++it) {
889  vpMbtDistanceCylinder *cy = *it;
890  if (cy->isVisible() && cy->isTracked()) {
891  if (cy->meline1 != NULL) {
892  double cylinderNormGradient = 0;
893  unsigned int cylinderNbFeatures = 0;
894  cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
897  projectionError += cylinderNormGradient;
898  nbFeatures += cylinderNbFeatures;
899  }
900 
901  if (cy->meline2 != NULL) {
902  double cylinderNormGradient = 0;
903  unsigned int cylinderNbFeatures = 0;
904  cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
907  projectionError += cylinderNormGradient;
908  nbFeatures += cylinderNbFeatures;
909  }
910  }
911  }
912 
913  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
914  it != circles[scaleLevel].end(); ++it) {
915  vpMbtDistanceCircle *c = *it;
916  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
917  double circleNormGradient = 0;
918  unsigned int circleNbFeatures = 0;
919  c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
922  projectionError += circleNormGradient;
923  nbFeatures += circleNbFeatures;
924  }
925  }
926 
927  if (nbFeatures > 0) {
928  projectionError = vpMath::deg(projectionError / (double)nbFeatures);
929  } else {
930  projectionError = 90.0;
931  }
932 
934  // std::cout << "Norm Gradient = " << errorGradient << std::endl;
935 }
936 
943 {
944  int nbExpectedPoint = 0;
945  int nbGoodPoint = 0;
946  int nbBadPoint = 0;
947 
948  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
949  ++it) {
950  vpMbtDistanceLine *l = *it;
951  if (l->isVisible() && l->isTracked()) {
952  for (size_t a = 0; a < l->meline.size(); a++) {
953  if (l->meline[a] != NULL) {
954  nbExpectedPoint += (int)l->meline[a]->expecteddensity;
955  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
956  itme != l->meline[a]->getMeList().end(); ++itme) {
957  vpMeSite pix = *itme;
958  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
959  nbGoodPoint++;
960  else
961  nbBadPoint++;
962  }
963  }
964  }
965  }
966  }
967 
968  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
969  it != cylinders[scaleLevel].end(); ++it) {
970  vpMbtDistanceCylinder *cy = *it;
971  if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) {
972  nbExpectedPoint += (int)cy->meline1->expecteddensity;
973  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
974  itme1 != cy->meline1->getMeList().end(); ++itme1) {
975  vpMeSite pix = *itme1;
976  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
977  nbGoodPoint++;
978  else
979  nbBadPoint++;
980  }
981  nbExpectedPoint += (int)cy->meline2->expecteddensity;
982  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
983  itme2 != cy->meline2->getMeList().end(); ++itme2) {
984  vpMeSite pix = *itme2;
985  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
986  nbGoodPoint++;
987  else
988  nbBadPoint++;
989  }
990  }
991  }
992 
993  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
994  it != circles[scaleLevel].end(); ++it) {
995  vpMbtDistanceCircle *ci = *it;
996  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
997  nbExpectedPoint += ci->meEllipse->getExpectedDensity();
998  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
999  itme != ci->meEllipse->getMeList().end(); ++itme) {
1000  vpMeSite pix = *itme;
1001  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
1002  nbGoodPoint++;
1003  else
1004  nbBadPoint++;
1005  }
1006  }
1007  }
1008 
1009  // Compare the number of good points with the min between the number of
1010  // expected points and number of points that are tracked
1011  int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
1012  // int nb_min = (std::min)(val1, val2);
1013  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1014  std::ostringstream oss;
1015  oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
1016  << ". Try to reduce the threshold=" << percentageGdPt
1017  << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1019  }
1020 }
1021 
1030 {
1031  initPyramid(I, Ipyramid);
1032 
1033  unsigned int lvl = (unsigned int)scales.size();
1034  do {
1035  lvl--;
1036 
1037  projectionError = 90.0;
1038 
1039  if (scales[lvl]) {
1040  vpHomogeneousMatrix cMo_1 = cMo;
1041  try {
1042  downScale(lvl);
1043 
1044  try {
1045  trackMovingEdge(*Ipyramid[lvl]);
1046  } catch (...) {
1047  vpTRACE("Error in moving edge tracking");
1048  throw;
1049  }
1050 
1051  // initialize the vector that contains the error and the matrix that
1052  // contains the interaction matrix AY: Useless as it is done in
1053  // coputeVVS()
1054  /*
1055  for(std::list<vpMbtDistanceLine*>::const_iterator
1056  it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
1057  (l->isVisible()){ l->initInteractionMatrixError();
1058  }
1059  }
1060 
1061  for(std::list<vpMbtDistanceCylinder*>::const_iterator
1062  it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
1063  if(cy->isVisible()) {
1064  cy->initInteractionMatrixError();
1065  }
1066  }
1067 
1068  for(std::list<vpMbtDistanceCircle*>::const_iterator
1069  it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
1070  (ci->isVisible()){ ci->initInteractionMatrixError();
1071  }
1072  }
1073  */
1074 
1075  try {
1076  computeVVS(*Ipyramid[lvl], lvl);
1077  } catch (...) {
1078  covarianceMatrix = -1;
1079  throw; // throw the original exception
1080  }
1081 
1082  testTracking();
1083 
1084  if (displayFeatures) {
1086  }
1087 
1088  // Looking for new visible face
1089  bool newvisibleface = false;
1090  visibleFace(I, cMo, newvisibleface);
1091 
1092  // cam.computeFov(I.getWidth(), I.getHeight());
1093  if (useScanLine) {
1096  }
1097 
1098  updateMovingEdge(I);
1099 
1100  initMovingEdge(I, cMo);
1101  // Reinit the moving edge for the lines which need it.
1102  reinitMovingEdge(I, cMo);
1103 
1104  if (computeProjError)
1106 
1107  upScale(lvl);
1108  } catch (const vpException &e) {
1109  if (lvl != 0) {
1110  cMo = cMo_1;
1111  reInitLevel(lvl);
1112  upScale(lvl);
1113  } else {
1114  upScale(lvl);
1115  throw(e);
1116  }
1117  }
1118  }
1119  } while (lvl != 0);
1120 
1122 }
1123 
1125 {
1127  track(m_I);
1128 }
1129 
1136 {
1137  if (!modelInitialised) {
1138  throw vpException(vpException::fatalError, "model not initialized");
1139  }
1140 
1141  bool a = false;
1142 
1143 #ifdef VISP_HAVE_OGRE
1144  if (useOgre) {
1145  if (!faces.isOgreInitialised()) {
1148  faces.initOgre(cam);
1149  // Turn off Ogre config dialog display for the next call to this
1150  // function since settings are saved in the ogre.cfg file and used
1151  // during the next call
1152  ogreShowConfigDialog = false;
1153  }
1154  }
1155 #endif
1156 
1157  if (clippingFlag > 2)
1158  cam.computeFov(I.getWidth(), I.getHeight());
1159 
1160  visibleFace(I, cMo, a);
1161  resetMovingEdge();
1162 
1163  if (useScanLine) {
1164  if (clippingFlag <= 2)
1165  cam.computeFov(I.getWidth(), I.getHeight());
1166 
1169  }
1170 
1171  initPyramid(I, Ipyramid);
1172  unsigned int i = (unsigned int)scales.size();
1173  do {
1174  i--;
1175  if (scales[i]) {
1176  downScale(i);
1177  initMovingEdge(*Ipyramid[i], cMo);
1178  upScale(i);
1179  }
1180  } while (i != 0);
1181 
1183 }
1184 
1193 {
1194  cMo = cdMo;
1195 
1196  init(I);
1197 }
1198 
1207 {
1208  cMo = cdMo;
1209 
1210  vpImageConvert::convert(I_color, m_I);
1211  init(m_I);
1212 }
1213 
1224 void vpMbEdgeTracker::loadConfigFile(const std::string &configFile)
1225 {
1226  // Load projection error config
1227  vpMbTracker::loadConfigFile(configFile);
1228 
1229 #ifdef VISP_HAVE_PUGIXML
1231 
1232  xmlp.setCameraParameters(cam);
1235  xmlp.setEdgeMe(me);
1236 
1237  try {
1238  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1239  xmlp.parse(configFile);
1240  } catch (...) {
1241  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
1242  }
1243 
1244  vpCameraParameters camera;
1245  vpMe meParser;
1246  xmlp.getCameraParameters(camera);
1247  xmlp.getEdgeMe(meParser);
1248 
1249  setCameraParameters(camera);
1250  setMovingEdge(meParser);
1253 
1254  if (xmlp.hasNearClippingDistance())
1256 
1257  if (xmlp.hasFarClippingDistance())
1259 
1260  if (xmlp.getFovClipping())
1262 
1263  useLodGeneral = xmlp.getLodState();
1266 
1267  applyLodSettingInConfig = false;
1268  if (this->getNbPolygon() > 0) {
1269  applyLodSettingInConfig = true;
1273  }
1274 
1275 #else
1276  std::cerr << "pugixml third-party is not properly built to read config file: " << configFile << std::endl;
1277 #endif
1278 }
1279 
1292  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1293  const bool displayFullModel)
1294 {
1295  //Display first the Moving-Edges
1296  if (displayFeatures) {
1298  }
1299 
1300  std::vector<std::vector<double> > models = vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, camera, displayFullModel);
1301 
1302  for (size_t i = 0; i < models.size(); i++) {
1303  if (vpMath::equal(models[i][0], 0)) {
1304  vpImagePoint ip1(models[i][1], models[i][2]);
1305  vpImagePoint ip2(models[i][3], models[i][4]);
1306  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1307  } else if (vpMath::equal(models[i][0], 1)) {
1308  vpImagePoint center(models[i][1], models[i][2]);
1309  double mu20 = models[i][3];
1310  double mu11 = models[i][4];
1311  double mu02 = models[i][5];
1312  vpDisplay::displayEllipse(I, center, mu20, mu11, mu02, true, col, thickness);
1313  }
1314  }
1315 
1316 #ifdef VISP_HAVE_OGRE
1317  if (useOgre)
1318  faces.displayOgre(cMo_);
1319 #endif
1320 }
1321 
1334  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1335  const bool displayFullModel)
1336 {
1337  //Display first the Moving-Edges
1338  if (displayFeatures) {
1340  }
1341 
1342  std::vector<std::vector<double> > models = vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo_, camera, displayFullModel);
1343 
1344  for (size_t i = 0; i < models.size(); i++) {
1345  if (vpMath::equal(models[i][0], 0)) {
1346  vpImagePoint ip1(models[i][1], models[i][2]);
1347  vpImagePoint ip2(models[i][3], models[i][4]);
1348  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1349  } else if (vpMath::equal(models[i][0], 1)) {
1350  vpImagePoint center(models[i][1], models[i][2]);
1351  double mu20 = models[i][3];
1352  double mu11 = models[i][4];
1353  double mu02 = models[i][5];
1354  vpDisplay::displayEllipse(I, center, mu20, mu11, mu02, true, col, thickness);
1355  }
1356  }
1357 
1358 #ifdef VISP_HAVE_OGRE
1359  if (useOgre)
1360  faces.displayOgre(cMo_);
1361 #endif
1362 }
1363 
1364 std::vector<std::vector<double> > vpMbEdgeTracker::getFeaturesForDisplayEdge()
1365 {
1366  std::vector<std::vector<double> > features;
1367 
1368  const unsigned int lvl = 0;
1369  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1370  vpMbtDistanceLine *l = *it;
1371  if (l->isVisible() && l->isTracked()) {
1372  std::vector<std::vector<double> > currentFeatures = l->getFeaturesForDisplay();
1373  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1374  }
1375  }
1376 
1377  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1378  ++it) {
1379  vpMbtDistanceCylinder *cy = *it;
1380  if (cy->isVisible() && cy->isTracked()) {
1381  std::vector<std::vector<double> > currentFeatures = cy->getFeaturesForDisplay();
1382  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1383  }
1384  }
1385 
1386  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1387  vpMbtDistanceCircle *ci = *it;
1388  if (ci->isVisible() && ci->isTracked()) {
1389  std::vector<std::vector<double> > currentFeatures = ci->getFeaturesForDisplay();
1390  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1391  }
1392  }
1393 
1394  return features;
1395 }
1396 
1410 std::vector<std::vector<double> > vpMbEdgeTracker::getModelForDisplay(unsigned int width, unsigned int height,
1411  const vpHomogeneousMatrix &cMo_,
1412  const vpCameraParameters &camera,
1413  const bool displayFullModel)
1414 {
1415  std::vector<std::vector<double> > models;
1416 
1417  for (unsigned int i = 0; i < scales.size(); i += 1) {
1418  if (scales[i]) {
1419  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1420  ++it) {
1421  std::vector<std::vector<double> > currentModel =
1422  (*it)->getModelForDisplay(width, height, cMo_, camera, displayFullModel);
1423  models.insert(models.end(), currentModel.begin(), currentModel.end());
1424  }
1425 
1426  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1427  it != cylinders[scaleLevel].end(); ++it) {
1428  std::vector<std::vector<double> > currentModel =
1429  (*it)->getModelForDisplay(width, height, cMo_, camera, displayFullModel);
1430  models.insert(models.end(), currentModel.begin(), currentModel.end());
1431  }
1432 
1433  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1434  it != circles[scaleLevel].end(); ++it) {
1435  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo_, camera, displayFullModel);
1436  models.push_back(paramsCircle);
1437  }
1438  break; // displaying model on one scale only
1439  }
1440  }
1441 
1442  return models;
1443 }
1444 
1446 {
1447  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1450  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1451 
1452  switch (state) {
1455  break;
1456 
1457  case vpMeSite::CONSTRAST:
1458  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1459  break;
1460 
1461  case vpMeSite::THRESHOLD:
1463  break;
1464 
1465  case vpMeSite::M_ESTIMATOR:
1466  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1467  break;
1468 
1469  case vpMeSite::TOO_NEAR:
1470  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1471  break;
1472 
1473  default:
1475  }
1476  }
1477  }
1478 }
1479 
1481 {
1482  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1485  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1486 
1487  switch (state) {
1490  break;
1491 
1492  case vpMeSite::CONSTRAST:
1493  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1494  break;
1495 
1496  case vpMeSite::THRESHOLD:
1498  break;
1499 
1500  case vpMeSite::M_ESTIMATOR:
1501  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1502  break;
1503 
1504  case vpMeSite::TOO_NEAR:
1505  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1506  break;
1507 
1508  default:
1510  }
1511  }
1512  }
1513 }
1514 
1524 {
1525  const bool doNotTrack = false;
1526 
1527  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1528  ++it) {
1529  vpMbtDistanceLine *l = *it;
1530  bool isvisible = false;
1531 
1532  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1533  ++itindex) {
1534  int index = *itindex;
1535  if (index == -1)
1536  isvisible = true;
1537  else {
1538  if (l->hiddenface->isVisible((unsigned int)index))
1539  isvisible = true;
1540  }
1541  }
1542 
1543  // Si la ligne n'appartient a aucune face elle est tout le temps visible
1544  if (l->Lindex_polygon.empty())
1545  isvisible = true; // Not sure that this can occur
1546 
1547  if (isvisible) {
1548  l->setVisible(true);
1549  l->updateTracked();
1550  if (l->meline.empty() && l->isTracked())
1551  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1552  } else {
1553  l->setVisible(false);
1554  for (size_t a = 0; a < l->meline.size(); a++) {
1555  if (l->meline[a] != NULL)
1556  delete l->meline[a];
1557  if (a < l->nbFeature.size())
1558  l->nbFeature[a] = 0;
1559  }
1560  l->nbFeatureTotal = 0;
1561  l->meline.clear();
1562  l->nbFeature.clear();
1563  }
1564  }
1565 
1566  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1567  it != cylinders[scaleLevel].end(); ++it) {
1568  vpMbtDistanceCylinder *cy = *it;
1569 
1570  bool isvisible = false;
1571 
1572  int index = cy->index_polygon;
1573  if (index == -1)
1574  isvisible = true;
1575  else {
1576  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1577  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1578  isvisible = true;
1579  }
1580  // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1581 
1582  if (isvisible) {
1583  cy->setVisible(true);
1584  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1585  if (cy->isTracked())
1586  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1587  }
1588  } else {
1589  cy->setVisible(false);
1590  if (cy->meline1 != NULL)
1591  delete cy->meline1;
1592  if (cy->meline2 != NULL)
1593  delete cy->meline2;
1594  cy->meline1 = NULL;
1595  cy->meline2 = NULL;
1596  cy->nbFeature = 0;
1597  cy->nbFeaturel1 = 0;
1598  cy->nbFeaturel2 = 0;
1599  }
1600  }
1601 
1602  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1603  it != circles[scaleLevel].end(); ++it) {
1604  vpMbtDistanceCircle *ci = *it;
1605  bool isvisible = false;
1606 
1607  int index = ci->index_polygon;
1608  if (index == -1)
1609  isvisible = true;
1610  else {
1611  if (ci->hiddenface->isVisible((unsigned int)index))
1612  isvisible = true;
1613  }
1614 
1615  if (isvisible) {
1616  ci->setVisible(true);
1617  if (ci->meEllipse == NULL) {
1618  if (ci->isTracked())
1619  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1620  }
1621  } else {
1622  ci->setVisible(false);
1623  if (ci->meEllipse != NULL)
1624  delete ci->meEllipse;
1625  ci->meEllipse = NULL;
1626  ci->nbFeature = 0;
1627  }
1628  }
1629 }
1630 
1637 {
1638  const bool doNotTrack = false;
1639 
1640  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1641  ++it) {
1642  vpMbtDistanceLine *l = *it;
1643  if (l->isVisible() && l->isTracked()) {
1644  if (l->meline.empty()) {
1645  l->initMovingEdge(I, cMo, doNotTrack, m_mask);
1646  }
1647  l->trackMovingEdge(I);
1648  }
1649  }
1650 
1651  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1652  it != cylinders[scaleLevel].end(); ++it) {
1653  vpMbtDistanceCylinder *cy = *it;
1654  if (cy->isVisible() && cy->isTracked()) {
1655  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1656  cy->initMovingEdge(I, cMo, doNotTrack, m_mask);
1657  }
1658  cy->trackMovingEdge(I, cMo);
1659  }
1660  }
1661 
1662  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1663  it != circles[scaleLevel].end(); ++it) {
1664  vpMbtDistanceCircle *ci = *it;
1665  if (ci->isVisible() && ci->isTracked()) {
1666  if (ci->meEllipse == NULL) {
1667  ci->initMovingEdge(I, cMo, doNotTrack, m_mask);
1668  }
1669  ci->trackMovingEdge(I, cMo);
1670  }
1671  }
1672 }
1673 
1680 {
1681  vpMbtDistanceLine *l;
1682  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1683  ++it) {
1684  if ((*it)->isTracked()) {
1685  l = *it;
1686  l->updateMovingEdge(I, cMo);
1687  if (l->nbFeatureTotal == 0 && l->isVisible()) {
1688  l->Reinit = true;
1689  }
1690  }
1691  }
1692 
1694  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1695  it != cylinders[scaleLevel].end(); ++it) {
1696  if ((*it)->isTracked()) {
1697  cy = *it;
1698  cy->updateMovingEdge(I, cMo);
1699  if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1700  cy->Reinit = true;
1701  }
1702  }
1703  }
1704 
1705  vpMbtDistanceCircle *ci;
1706  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1707  it != circles[scaleLevel].end(); ++it) {
1708  if ((*it)->isTracked()) {
1709  ci = *it;
1710  ci->updateMovingEdge(I, cMo);
1711  if (ci->nbFeature == 0 && ci->isVisible()) {
1712  ci->Reinit = true;
1713  }
1714  }
1715  }
1716 }
1717 
1719 {
1720  unsigned int n = 0;
1721 
1722  vpMbtDistanceLine *l;
1723  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1724  ++it) {
1725  if ((*it)->isTracked()) {
1726  l = *it;
1727  unsigned int indexLine = 0;
1728  double wmean = 0;
1729  for (size_t a = 0; a < l->meline.size(); a++) {
1730  if (l->nbFeature[a] > 0) {
1731  std::list<vpMeSite>::iterator itListLine;
1732  itListLine = l->meline[a]->getMeList().begin();
1733 
1734  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1735  wmean += m_w_edge[n + indexLine];
1736  vpMeSite p = *itListLine;
1737  if (m_w_edge[n + indexLine] < 0.5) {
1739 
1740  *itListLine = p;
1741  }
1742 
1743  ++itListLine;
1744  indexLine++;
1745  }
1746  }
1747  }
1748  n += l->nbFeatureTotal;
1749 
1750  if (l->nbFeatureTotal != 0)
1751  wmean /= l->nbFeatureTotal;
1752  else
1753  wmean = 1;
1754 
1755  l->setMeanWeight(wmean);
1756 
1757  if (wmean < 0.8)
1758  l->Reinit = true;
1759  }
1760  }
1761 
1762  // Same thing with cylinders as with lines
1764  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1765  it != cylinders[scaleLevel].end(); ++it) {
1766  if ((*it)->isTracked()) {
1767  cy = *it;
1768  double wmean = 0;
1769  std::list<vpMeSite>::iterator itListCyl1;
1770  std::list<vpMeSite>::iterator itListCyl2;
1771 
1772  if (cy->nbFeature > 0) {
1773  itListCyl1 = cy->meline1->getMeList().begin();
1774  itListCyl2 = cy->meline2->getMeList().begin();
1775 
1776  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1777  wmean += m_w_edge[n + i];
1778  vpMeSite p = *itListCyl1;
1779  if (m_w_edge[n + i] < 0.5) {
1781 
1782  *itListCyl1 = p;
1783  }
1784 
1785  ++itListCyl1;
1786  }
1787  }
1788 
1789  if (cy->nbFeaturel1 != 0)
1790  wmean /= cy->nbFeaturel1;
1791  else
1792  wmean = 1;
1793 
1794  cy->setMeanWeight1(wmean);
1795 
1796  if (wmean < 0.8) {
1797  cy->Reinit = true;
1798  }
1799 
1800  wmean = 0;
1801  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1802  wmean += m_w_edge[n + i];
1803  vpMeSite p = *itListCyl2;
1804  if (m_w_edge[n + i] < 0.5) {
1806 
1807  *itListCyl2 = p;
1808  }
1809 
1810  ++itListCyl2;
1811  }
1812 
1813  if (cy->nbFeaturel2 != 0)
1814  wmean /= cy->nbFeaturel2;
1815  else
1816  wmean = 1;
1817 
1818  cy->setMeanWeight2(wmean);
1819 
1820  if (wmean < 0.8) {
1821  cy->Reinit = true;
1822  }
1823 
1824  n += cy->nbFeature;
1825  }
1826  }
1827 
1828  // Same thing with circles as with lines
1829  vpMbtDistanceCircle *ci;
1830  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1831  it != circles[scaleLevel].end(); ++it) {
1832  if ((*it)->isTracked()) {
1833  ci = *it;
1834  double wmean = 0;
1835  std::list<vpMeSite>::iterator itListCir;
1836 
1837  if (ci->nbFeature > 0) {
1838  itListCir = ci->meEllipse->getMeList().begin();
1839  }
1840 
1841  wmean = 0;
1842  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1843  wmean += m_w_edge[n + i];
1844  vpMeSite p = *itListCir;
1845  if (m_w_edge[n + i] < 0.5) {
1847 
1848  *itListCir = p;
1849  }
1850 
1851  ++itListCir;
1852  }
1853 
1854  if (ci->nbFeature != 0)
1855  wmean /= ci->nbFeature;
1856  else
1857  wmean = 1;
1858 
1859  ci->setMeanWeight(wmean);
1860 
1861  if (wmean < 0.8) {
1862  ci->Reinit = true;
1863  }
1864 
1865  n += ci->nbFeature;
1866  }
1867  }
1868 }
1869 
1880 {
1881  vpMbtDistanceLine *l;
1882  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1883  ++it) {
1884  if ((*it)->isTracked()) {
1885  l = *it;
1886  if (l->Reinit && l->isVisible())
1887  l->reinitMovingEdge(I, _cMo, m_mask);
1888  }
1889  }
1890 
1892  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1893  it != cylinders[scaleLevel].end(); ++it) {
1894  if ((*it)->isTracked()) {
1895  cy = *it;
1896  if (cy->Reinit && cy->isVisible())
1897  cy->reinitMovingEdge(I, _cMo, m_mask);
1898  }
1899  }
1900 
1901  vpMbtDistanceCircle *ci;
1902  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1903  it != circles[scaleLevel].end(); ++it) {
1904  if ((*it)->isTracked()) {
1905  ci = *it;
1906  if (ci->Reinit && ci->isVisible())
1907  ci->reinitMovingEdge(I, _cMo, m_mask);
1908  }
1909  }
1910 }
1911 
1913 {
1914  for (unsigned int i = 0; i < scales.size(); i += 1) {
1915  if (scales[i]) {
1916  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1917  for (size_t a = 0; a < (*it)->meline.size(); a++) {
1918  if ((*it)->meline[a] != NULL) {
1919  delete (*it)->meline[a];
1920  (*it)->meline[a] = NULL;
1921  }
1922  }
1923 
1924  (*it)->meline.clear();
1925  (*it)->nbFeature.clear();
1926  (*it)->nbFeatureTotal = 0;
1927  }
1928 
1929  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1930  ++it) {
1931  if ((*it)->meline1 != NULL) {
1932  delete (*it)->meline1;
1933  (*it)->meline1 = NULL;
1934  }
1935  if ((*it)->meline2 != NULL) {
1936  delete (*it)->meline2;
1937  (*it)->meline2 = NULL;
1938  }
1939 
1940  (*it)->nbFeature = 0;
1941  (*it)->nbFeaturel1 = 0;
1942  (*it)->nbFeaturel2 = 0;
1943  }
1944 
1945  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1946  if ((*it)->meEllipse != NULL) {
1947  delete (*it)->meEllipse;
1948  (*it)->meEllipse = NULL;
1949  }
1950  (*it)->nbFeature = 0;
1951  }
1952  }
1953  }
1954 }
1955 
1968 void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1969 {
1970  {
1971  // suppress line already in the model
1972  bool already_here = false;
1973  vpMbtDistanceLine *l;
1974 
1975  for (unsigned int i = 0; i < scales.size(); i += 1) {
1976  if (scales[i]) {
1977  downScale(i);
1978  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1979  l = *it;
1980  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
1981  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
1982  already_here = true;
1983  l->addPolygon(polygon);
1984  l->hiddenface = &faces;
1985  }
1986  }
1987 
1988  if (!already_here) {
1989  l = new vpMbtDistanceLine;
1990 
1992  l->buildFrom(P1, P2);
1993  l->addPolygon(polygon);
1994  l->setMovingEdge(&me);
1995  l->hiddenface = &faces;
1996  l->useScanLine = useScanLine;
1997 
1998  l->setIndex(nline);
1999  l->setName(name);
2000 
2003 
2004  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2006 
2007  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2009 
2010  nline += 1;
2011  lines[i].push_back(l);
2012  }
2013  upScale(i);
2014  }
2015  }
2016  }
2017 }
2018 
2024 void vpMbEdgeTracker::removeLine(const std::string &name)
2025 {
2026  vpMbtDistanceLine *l;
2027 
2028  for (unsigned int i = 0; i < scales.size(); i++) {
2029  if (scales[i]) {
2030  for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2031  l = *it;
2032  if (name.compare(l->getName()) == 0) {
2033  lines[i].erase(it);
2034  break;
2035  }
2036  }
2037  }
2038  }
2039 }
2040 
2051 void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace,
2052  const std::string &name)
2053 {
2054  {
2055  bool already_here = false;
2056  vpMbtDistanceCircle *ci;
2057 
2058  for (unsigned int i = 0; i < scales.size(); i += 1) {
2059  if (scales[i]) {
2060  downScale(i);
2061  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2062  ci = *it;
2063  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
2064  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
2065  already_here =
2066  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
2067  }
2068  }
2069 
2070  if (!already_here) {
2071  ci = new vpMbtDistanceCircle;
2072 
2073  ci->setCameraParameters(cam);
2074  ci->buildFrom(P1, P2, P3, r);
2075  ci->setMovingEdge(&me);
2076  ci->setIndex(ncircle);
2077  ci->setName(name);
2078  ci->index_polygon = idFace;
2079  ci->hiddenface = &faces;
2080 
2081  // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
2082  // ci->getPolygon().setClipping(clippingFlag);
2083 
2084  // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
2085  // vpPolygon3D::NEAR_CLIPPING)
2086  // ci->getPolygon().setNearClippingDistance(distNearClip);
2087 
2088  // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
2089  // vpPolygon3D::FAR_CLIPPING)
2090  // ci->getPolygon().setFarClippingDistance(distFarClip);
2091 
2092  ncircle += 1;
2093  circles[i].push_back(ci);
2094  }
2095  upScale(i);
2096  }
2097  }
2098  }
2099 }
2100 
2110 void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace,
2111  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 
2133  cy->setCameraParameters(cam);
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 
2220  bool &newvisibleline)
2221 {
2222  unsigned int n;
2223  bool changed = false;
2224 
2225  if (!useOgre) {
2226  // n = faces.setVisible(_I.getWidth(), _I.getHeight(), cam, _cMo, vpMath::rad(89), vpMath::rad(89),
2227  // changed);
2228  n = faces.setVisible(_I.getWidth(), _I.getHeight(), cam, _cMo, angleAppears, angleDisappears, changed);
2229  } else {
2230 #ifdef VISP_HAVE_OGRE
2231  n = faces.setVisibleOgre(_I.getWidth(), _I.getHeight(), cam, _cMo, angleAppears, angleDisappears, changed);
2232 #else
2233  n = faces.setVisible(_I.getWidth(), _I.getHeight(), cam, _cMo, angleAppears, angleDisappears, changed);
2234 #endif
2235  }
2236 
2237  if (n > nbvisiblepolygone) {
2238  // cout << "une nouvelle face est visible " << endl;
2239  newvisibleline = true;
2240  } else
2241  newvisibleline = false;
2242 
2243  nbvisiblepolygone = n;
2244 }
2245 
2262 {
2263  unsigned int nbpt = polygon.getNbPoint();
2264  if (nbpt > 0) {
2265  for (unsigned int i = 0; i < nbpt - 1; i++)
2266  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2267  vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
2268  }
2269 }
2286 {
2287  unsigned int nbpt = polygon.getNbPoint();
2288  if (nbpt > 0) {
2289  for (unsigned int i = 0; i < nbpt - 1; i++)
2290  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2291  }
2292 }
2293 
2294 unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
2295  unsigned int &nberrors_circles)
2296 {
2297  unsigned int nbrow = 0;
2298  nberrors_lines = 0;
2299  nberrors_cylinders = 0;
2300  nberrors_circles = 0;
2301 
2302  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2303  ++it) {
2304 
2305  vpMbtDistanceLine *l = *it;
2306 
2307  if (l->isTracked()) {
2309  nbrow += l->nbFeatureTotal;
2310  nberrors_lines += l->nbFeatureTotal;
2311  }
2312  }
2313 
2314  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2315  it != cylinders[scaleLevel].end(); ++it) {
2316  vpMbtDistanceCylinder *cy = *it;
2317 
2318  if (cy->isTracked()) {
2320  nbrow += cy->nbFeature;
2321  nberrors_cylinders += cy->nbFeature;
2322  }
2323  }
2324 
2325  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2326  it != circles[scaleLevel].end(); ++it) {
2327  vpMbtDistanceCircle *ci = *it;
2328 
2329  if (ci->isTracked()) {
2331  nbrow += ci->nbFeature;
2332  nberrors_circles += ci->nbFeature;
2333  }
2334  }
2335 
2336  return nbrow;
2337 }
2338 
2350 void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
2351  const int idFace, const std::string &name)
2352 {
2353  addCircle(p1, p2, p3, radius, (int)idFace, name);
2354 }
2355 
2366 void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
2367  const std::string &name)
2368 {
2369  addCylinder(p1, p2, radius, (int)idFace, name);
2370 }
2371 
2378 {
2379  this->cMo.eye();
2380  vpMbtDistanceLine *l;
2382  vpMbtDistanceCircle *ci;
2383 
2384  for (unsigned int i = 0; i < scales.size(); i += 1) {
2385  if (scales[i]) {
2386  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2387  l = *it;
2388  if (l != NULL)
2389  delete l;
2390  l = NULL;
2391  }
2392 
2393  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2394  ++it) {
2395  cy = *it;
2396  if (cy != NULL)
2397  delete cy;
2398  cy = NULL;
2399  }
2400 
2401  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2402  ci = *it;
2403  if (ci != NULL)
2404  delete ci;
2405  ci = NULL;
2406  }
2407  lines[i].clear();
2408  cylinders[i].clear();
2409  circles[i].clear();
2410  }
2411  }
2412 
2413  faces.reset();
2414 
2415  useScanLine = false;
2416 
2417 #ifdef VISP_HAVE_OGRE
2418  useOgre = false;
2419 #endif
2420 
2421  m_computeInteraction = true;
2422  nline = 0;
2423  ncylinder = 0;
2424  m_lambda = 1.0;
2425  nbvisiblepolygone = 0;
2426  percentageGdPt = 0.4;
2427 
2428  angleAppears = vpMath::rad(89);
2431 
2433 
2434  // reinitialization of the scales.
2435  this->setScales(scales);
2436 }
2437 
2450 void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2451  const vpHomogeneousMatrix &cMo_, const bool verbose,
2452  const vpHomogeneousMatrix &T)
2453 {
2454  this->cMo.eye();
2455  vpMbtDistanceLine *l;
2457  vpMbtDistanceCircle *ci;
2458 
2459  for (unsigned int i = 0; i < scales.size(); i += 1) {
2460  if (scales[i]) {
2461  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2462  l = *it;
2463  if (l != NULL)
2464  delete l;
2465  l = NULL;
2466  }
2467 
2468  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2469  ++it) {
2470  cy = *it;
2471  if (cy != NULL)
2472  delete cy;
2473  cy = NULL;
2474  }
2475 
2476  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2477  ci = *it;
2478  if (ci != NULL)
2479  delete ci;
2480  ci = NULL;
2481  }
2482 
2483  lines[i].clear();
2484  cylinders[i].clear();
2485  circles[i].clear();
2486  }
2487  }
2488 
2489  faces.reset();
2490 
2491  // compute_interaction=1;
2492  nline = 0;
2493  ncylinder = 0;
2494  ncircle = 0;
2495  // lambda = 1;
2496  nbvisiblepolygone = 0;
2497 
2498  loadModel(cad_name, verbose, T);
2499  initFromPose(I, cMo_);
2500 }
2501 
2512 unsigned int vpMbEdgeTracker::getNbPoints(const unsigned int level) const
2513 {
2514  if ((level > scales.size()) || !scales[level]) {
2515  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2516  level);
2517  }
2518 
2519  unsigned int nbGoodPoints = 0;
2520  vpMbtDistanceLine *l;
2521  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2522  l = *it;
2523  if (l->isVisible() && l->isTracked()) {
2524  for (size_t a = 0; a < l->meline.size(); a++) {
2525  if (l->nbFeature[a] != 0)
2526  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2527  itme != l->meline[a]->getMeList().end(); ++itme) {
2528  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2529  nbGoodPoints++;
2530  }
2531  }
2532  }
2533  }
2534 
2536  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2537  ++it) {
2538  cy = *it;
2539  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
2540  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2541  itme1 != cy->meline1->getMeList().end(); ++itme1) {
2542  if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2543  nbGoodPoints++;
2544  }
2545  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2546  itme2 != cy->meline2->getMeList().end(); ++itme2) {
2547  if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2548  nbGoodPoints++;
2549  }
2550  }
2551  }
2552 
2553  vpMbtDistanceCircle *ci;
2554  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2555  ci = *it;
2556  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
2557  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2558  itme != ci->meEllipse->getMeList().end(); ++itme) {
2559  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2560  nbGoodPoints++;
2561  }
2562  }
2563  }
2564 
2565  return nbGoodPoints;
2566 }
2567 
2589 void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2590 {
2591  unsigned int nbActivatedLevels = 0;
2592  for (unsigned int i = 0; i < scale.size(); i++) {
2593  if (scale[i]) {
2594  nbActivatedLevels++;
2595  }
2596  }
2597 
2598  if (scale.empty() || (nbActivatedLevels == 0)) {
2599  vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2600  "tracking. Use the global one");
2601  this->scales.resize(0);
2602  this->scales.push_back(true);
2603 
2604  lines.resize(1);
2605  lines[0].clear();
2606 
2607  cylinders.resize(1);
2608  cylinders[0].clear();
2609 
2610  circles.resize(1);
2611  circles[0].clear();
2612  } else {
2613  this->scales = scale;
2614 
2615  lines.resize(scale.size());
2616  cylinders.resize(scale.size());
2617  circles.resize(scale.size());
2618 
2619  for (unsigned int i = 0; i < lines.size(); i++) {
2620  lines[i].clear();
2621  cylinders[i].clear();
2622  circles[i].clear();
2623  }
2624  }
2625 }
2626 
2633 {
2634  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2635  std::cerr << "Far clipping value cannot be inferior than near clipping "
2636  "value. Far clipping won't be considered."
2637  << std::endl;
2638  else if (dist < 0)
2639  std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2640  "won't be considered."
2641  << std::endl;
2642  else {
2644  vpMbtDistanceLine *l;
2645 
2646  for (unsigned int i = 0; i < scales.size(); i += 1) {
2647  if (scales[i]) {
2648  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2649  l = *it;
2651  }
2652  }
2653  }
2654  }
2655 }
2656 
2663 {
2664  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2665  std::cerr << "Near clipping value cannot be superior than far clipping "
2666  "value. Near clipping won't be considered."
2667  << std::endl;
2668  else if (dist < 0)
2669  std::cerr << "Near clipping value cannot be inferior than 0. Near "
2670  "clipping won't be considered."
2671  << std::endl;
2672  else {
2674  vpMbtDistanceLine *l;
2675 
2676  for (unsigned int i = 0; i < scales.size(); i += 1) {
2677  if (scales[i]) {
2678  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2679  l = *it;
2681  }
2682  }
2683  }
2684  }
2685 }
2686 
2694 void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2695 {
2696  vpMbTracker::setClipping(flags);
2697 
2698  vpMbtDistanceLine *l;
2699 
2700  for (unsigned int i = 0; i < scales.size(); i += 1) {
2701  if (scales[i]) {
2702  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2703  l = *it;
2705  }
2706  }
2707  }
2708 }
2709 
2726  std::vector<const vpImage<unsigned char> *> &_pyramid)
2727 {
2728  _pyramid.resize(scales.size());
2729 
2730  if (scales[0]) {
2731  _pyramid[0] = &_I;
2732  } else {
2733  _pyramid[0] = NULL;
2734  }
2735 
2736  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2737  if (scales[i]) {
2738  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2739  vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2740 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2741  IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
2742  vpI0->imageData = (char *)(_I.bitmap);
2743  IplImage *vpI =
2744  cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
2745  cvResize(vpI0, vpI, CV_INTER_NN);
2746  vpImageConvert::convert(vpI, *I);
2747  cvReleaseImage(&vpI);
2748  vpI0->imageData = NULL;
2749  cvReleaseImageHeader(&vpI0);
2750 #else
2751  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2752  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2753  (*I)[k][l] = _I[ii][jj];
2754  }
2755  }
2756 #endif
2757  _pyramid[i] = I;
2758  } else {
2759  _pyramid[i] = NULL;
2760  }
2761  }
2762 }
2763 
2770 void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2771 {
2772  if (_pyramid.size() > 0) {
2773  _pyramid[0] = NULL;
2774  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2775  if (_pyramid[i] != NULL) {
2776  delete _pyramid[i];
2777  _pyramid[i] = NULL;
2778  }
2779  }
2780  _pyramid.resize(0);
2781  }
2782 }
2783 
2794 void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
2795 {
2796  if (level > scales.size() || !scales[level]) {
2797  std::ostringstream oss;
2798  oss << level;
2799  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2800  throw vpException(vpException::dimensionError, errorMsg);
2801  }
2802 
2803  linesList = lines[level];
2804 }
2805 
2816 void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, const unsigned int level) const
2817 {
2818  if (level > scales.size() || !scales[level]) {
2819  std::ostringstream oss;
2820  oss << level;
2821  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2822  throw vpException(vpException::dimensionError, errorMsg);
2823  }
2824 
2825  cylindersList = cylinders[level];
2826 }
2827 
2838 void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
2839 {
2840  if (level > scales.size() || !scales[level]) {
2841  std::ostringstream oss;
2842  oss << level;
2843  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2844  throw vpException(vpException::dimensionError, errorMsg);
2845  }
2846 
2847  circlesList = circles[level];
2848 }
2849 
2856 void vpMbEdgeTracker::downScale(const unsigned int _scale)
2857 {
2858  const double ratio = pow(2., (int)_scale);
2859  scaleLevel = _scale;
2860 
2861  vpMatrix K = cam.get_K();
2862 
2863  K[0][0] /= ratio;
2864  K[1][1] /= ratio;
2865  K[0][2] /= ratio;
2866  K[1][2] /= ratio;
2867 
2869 }
2870 
2877 void vpMbEdgeTracker::upScale(const unsigned int _scale)
2878 {
2879  const double ratio = pow(2., (int)_scale);
2880  scaleLevel = 0;
2881 
2882  vpMatrix K = cam.get_K();
2883 
2884  K[0][0] *= ratio;
2885  K[1][1] *= ratio;
2886  K[0][2] *= ratio;
2887  K[1][2] *= ratio;
2888 
2890 }
2891 
2899 void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2900 {
2901  unsigned int scaleLevel_1 = scaleLevel;
2902  scaleLevel = _lvl;
2903 
2904  vpMbtDistanceLine *l;
2905  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2906  ++it) {
2907  if ((*it)->isTracked()) {
2908  l = *it;
2909  l->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2910  }
2911  }
2912 
2914  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2915  it != cylinders[scaleLevel].end(); ++it) {
2916  if ((*it)->isTracked()) {
2917  cy = *it;
2918  cy->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2919  }
2920  }
2921 
2922  vpMbtDistanceCircle *ci;
2923  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2924  it != circles[scaleLevel].end(); ++it) {
2925  if ((*it)->isTracked()) {
2926  ci = *it;
2927  ci->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2928  }
2929  }
2930 
2931  trackMovingEdge(*Ipyramid[_lvl]);
2932  updateMovingEdge(*Ipyramid[_lvl]);
2933  scaleLevel = scaleLevel_1;
2934 }
2935 
2943 void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2944 {
2945  for (unsigned int i = 0; i < scales.size(); i += 1) {
2946  if (scales[i]) {
2947  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2948  /*(*it)->setTracked(useEdgeTracking);
2949  for(std::list<int>::const_iterator
2950  itpoly=(*it)->Lindex_polygon.begin();
2951  itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2952  if(faces[(*itpoly)]->getName() != name){
2953  (*it)->setTracked(true);
2954  break;
2955  }
2956  }*/
2957 
2958  (*it)->setTracked(name, useEdgeTracking);
2959  }
2960 
2961  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2962  ++it) {
2963  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2964  (*it)->setTracked(useEdgeTracking);
2965  }
2966  }
2967 
2968  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2969  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2970  (*it)->setTracked(useEdgeTracking);
2971  }
2972  }
2973  }
2974  }
2975 }
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
bool computeProjError
Definition: vpMbTracker.h:133
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0) const
unsigned int ncylinder
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
void initFromCalibrationMatrix(const vpMatrix &_K)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setMovingEdge(const vpMe &me)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1)
void trackMovingEdge(const vpImage< unsigned char > &I)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
double getFarClippingDistance() const
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setCameraParameters(const vpCameraParameters &camera)
vpColVector m_error_edge
(s - s*)
void upScale(const unsigned int _scale)
std::string getName() const
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpColVector m_weightedError_edge
Weighted error.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
bool isAppearing(const unsigned int i)
unsigned int getWidth() const
Definition: vpImage.h:244
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(const double wmean)
double getNearClippingDistance() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)
virtual void track(const vpImage< unsigned char > &I)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
unsigned int scaleLevel
std::list< int > Lindex_polygon
Index of the faces which contain the line.
Type * bitmap
points toward the bitmap
Definition: vpImage.h:141
vpMatrix L
The interaction matrix.
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
#define vpERROR_TRACE
Definition: vpDebug.h:393
virtual void loadConfigFile(const std::string &configFile)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0) const
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:296
vpColVector m_factor
Edge VVS variables.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:113
vpPoint * p1
The first extremity.
void setOgreShowConfigDialog(const bool showConfigDialog)
virtual ~vpMbEdgeTracker()
unsigned int ncircle
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector error
The error vector.
bool modelInitialised
Definition: vpMbTracker.h:123
vpColVector m_w_edge
Robust weights.
void getCameraParameters(vpCameraParameters &cam) const
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
vpColVector m_wCircles
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void reInitLevel(const unsigned int _lvl)
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
Definition: vpMe.h:60
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Manage the line of a polygon used in the model-based tracker.
unsigned int nbFeature
The number of moving edges.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void removeCircle(const std::string &name)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
double getLodMinLineLengthThreshold() const
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
void setIndex(const unsigned int i)
virtual void setCameraParameters(const vpCameraParameters &camera)
void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
void computeVVS(const vpImage< unsigned char > &_I, const unsigned int lvl)
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void setEdgeMe(const vpMe &ecm)
void removeCylinder(const std::string &name)
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
void loadConfigFile(const std::string &configFile)
static const vpColor green
Definition: vpColor.h:183
vpMe me
The moving edges parameters.
void updateMovingEdge(const vpImage< unsigned char > &I)
virtual unsigned int getNbPoints(const unsigned int level=0) const
vpPoint * p1
The first extremity on the axe.
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void downScale(const unsigned int _scale)
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:189
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:111
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
virtual void setNearClippingDistance(const double &dist)
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
double projectionError
Definition: vpMbTracker.h:136
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0) const
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool Reinit
Indicates if the circle has to be reinitialized.
Error that can be emited by the vpTracker class and its derivates.
std::vector< std::vector< double > > getFeaturesForDisplay()
static const vpColor cyan
Definition: vpColor.h:189
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setAngleDisappear(const double &adisappear)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setName(const std::string &circle_name)
std::string getName() const
Definition: vpMbtPolygon.h:108
vpMatrix AtA() const
Definition: vpMatrix.cpp:524
vpPoint * p2
The second extremity.
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:114
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
virtual void testTracking()
vpColVector error
The error vector.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
int getIndex() const
Definition: vpMbtPolygon.h:101
bool isVisible() const
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
Generic class defining intrinsic camera parameters.
std::vector< std::vector< double > > getFeaturesForDisplay()
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
bool isTracked() const
void setIndex(const unsigned int i)
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
virtual void computeVVSInteractionMatrixAndResidu()
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual void setFarClippingDistance(const double &dist)
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
unsigned int getRows() const
Definition: vpArray2D.h:289
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpColVector m_errorCylinders
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:175
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpColVector m_errorCircles
void getEdgeMe(vpMe &ecm) const
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
vpColVector m_wCylinders
vpRobust m_robustLines
double get_px() const
std::string getName() const
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:108
void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
void setMeanWeight1(const double wmean)
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
void computeProjectionError(const vpImage< unsigned char > &_I)
void setVisible(bool _isvisible)
void insert(unsigned int i, const vpColVector &v)
vpRobust m_robustCylinders
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpRobust m_robustCircles
void setName(const std::string &line_name)
void setScales(const std::vector< bool > &_scales)
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
void setCameraParameters(const vpCameraParameters &camera)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void setIndex(const unsigned int i)
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:101
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const 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)
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
vpHomogeneousMatrix inverse() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
std::vector< std::vector< double > > getFeaturesForDisplay()
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
void setMeanWeight(const double _wmean)
bool isVisible(const unsigned int i)
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:364
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void computeVVSInit()
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
unsigned int getHeight() const
Definition: vpImage.h:186
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
void setMeanWeight(const double w_mean)
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
vpColVector m_wLines
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2014
virtual void setClipping(const unsigned int &flags)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
bool Reinit
Indicates if the line has to be reinitialized.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double radius
The radius of the cylinder.
std::vector< vpMbtMeLine * > meline
The moving edge container.
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
vpMatrix m_L_edge
Interaction matrix.
unsigned int nline
void addPolygon(vpMbtPolygon &p)
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
virtual void setClipping(const unsigned int &flags)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:115
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void computeVVSFirstPhase(const vpImage< unsigned char > &I, const unsigned int iter, double &count, const unsigned int lvl=0)
void setName(const std::string &cyl_name)
static int() sign(double x)
Definition: vpMath.h:268
vpColVector m_errorLines
static const vpColor yellow
Definition: vpColor.h:188
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
void setCameraParameters(const vpCameraParameters &cam)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void removeLine(const std::string &name)
static const vpColor purple
Definition: vpColor.h:191
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
double getLodMinPolygonAreaThreshold() const
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpMatrix L
The interaction matrix.
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
std::vector< unsigned int > nbFeature
The number of moving edges.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void eye()
Definition: vpMatrix.cpp:360
virtual void computeVVSCheckLevenbergMarquardt(const 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)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const bool displayFullModel=false)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
std::string getName() const
void displayFeaturesOnImage(const vpImage< unsigned char > &I)
vpPoint * p1
The center of the circle.
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static const vpColor blue
Definition: vpColor.h:186
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)