Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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/vpMbtXmlParser.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()
75 {
78 
79  scales[0] = true;
80 
81 #ifdef VISP_HAVE_OGRE
82  faces.getOgreContext()->setWindowName("MBT Edge");
83 #endif
84 }
85 
90 {
94 
95  for (unsigned int i = 0; i < scales.size(); i += 1) {
96  if (scales[i]) {
97  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
98  l = *it;
99  if (l != NULL) {
100  delete l;
101  }
102  l = NULL;
103  }
104 
105  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
106  ++it) {
107  cy = *it;
108  if (cy != NULL) {
109  delete cy;
110  }
111  cy = NULL;
112  }
113 
114  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
115  ci = *it;
116  if (ci != NULL) {
117  delete ci;
118  }
119  ci = NULL;
120  }
121 
122  lines[i].clear();
123  cylinders[i].clear();
124  circles[i].clear();
125  }
126  }
127 
129 }
130 
137 {
138  this->me = p_me;
139 
140  for (unsigned int i = 0; i < scales.size(); i += 1) {
141  if (scales[i]) {
142  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
143  vpMbtDistanceLine *l = *it;
144  l->setMovingEdge(&(this->me));
145  }
146 
147  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
148  ++it) {
149  vpMbtDistanceCylinder *cy = *it;
150  cy->setMovingEdge(&(this->me));
151  }
152 
153  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
154  vpMbtDistanceCircle *ci = *it;
155  ci->setMovingEdge(&(this->me));
156  }
157  }
158  }
159 }
160 
170 void vpMbEdgeTracker::computeVVS(const vpImage<unsigned char> &_I, const unsigned int lvl)
171 {
172  double residu_1 = 1e3;
173  double r = 1e3 - 1;
174 
175  unsigned int iter = 0;
176 
177  computeVVSInit();
178  unsigned int nbrow = m_error_edge.getRows();
179 
180  bool reloop = true;
181 
182  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
183  if (isoJoIdentity_)
184  oJo.eye();
185 
186  /*** First phase ***/
187 
188  while (reloop == true && iter < 10) {
189  double count = 0;
190 
191  computeVVSFirstPhase(_I, iter, count, lvl);
192 
193  count = count / (double)nbrow;
194  if (count >= 0.85) {
195  reloop = false;
196  }
197 
198  computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);
199 
200  iter++;
201  }
202 
203  // std::cout << "\t First minimization in " << iter << " iteration give as
204  // initial cMo: \n" << cMo << std::endl;
205 
206  /*** Second phase ***/
207  vpHomogeneousMatrix cMoPrev;
208  vpColVector W_true(nbrow);
209  vpMatrix L_true;
210  vpMatrix LVJ_true;
211 
212  double mu = m_initialMu;
213  vpColVector m_error_prev;
214  vpColVector m_w_prev;
215 
216  // To avoid to create these matrices each iteration
217  vpMatrix LTL;
218  vpColVector LTR;
219  vpColVector v;
220 
221  iter = 0;
222  m_w_edge = 1;
223 
224  // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
225  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
227 
228  bool reStartFromLastIncrement = false;
229  computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
230  &m_w_edge, &m_w_prev);
231 
232  if (!reStartFromLastIncrement) {
234 
235  L_true = m_L_edge;
237 
238  if (computeCovariance) {
239  L_true = m_L_edge;
240  if (!isoJoIdentity_) {
241  cVo.buildFrom(cMo);
242  LVJ_true = (m_L_edge * cVo * oJo);
243  }
244  }
245 
246  double wi = 0.0, eri = 0.0;
247  double num = 0.0, den = 0.0;
248  if ((iter == 0) || m_computeInteraction) {
249  for (unsigned int i = 0; i < nbrow; i++) {
250  wi = m_w_edge[i] * m_factor[i];
251  W_true[i] = wi;
252  eri = m_error_edge[i];
253  num += wi * vpMath::sqr(eri);
254  den += wi;
255 
256  m_weightedError_edge[i] = wi * eri;
257 
258  for (unsigned int j = 0; j < 6; j++) {
259  m_L_edge[i][j] = wi * m_L_edge[i][j];
260  }
261  }
262  } else {
263  for (unsigned int i = 0; i < nbrow; i++) {
264  wi = m_w_edge[i] * m_factor[i];
265  W_true[i] = wi;
266  eri = m_error_edge[i];
267  num += wi * vpMath::sqr(eri);
268  den += wi;
269 
270  m_weightedError_edge[i] = wi * eri;
271  }
272  }
273 
274  residu_1 = r;
275  r = sqrt(num / den); // Le critere d'arret prend en compte le poids
276 
277  computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev,
278  LTR, mu, v, &m_w_edge, &m_w_prev);
279 
280  cMoPrev = cMo;
282 
283  } // endif(!restartFromLast)
284 
285  iter++;
286  }
287 
288  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);
289 
291 }
292 
293 void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_I, const unsigned int iter, double &count,
294  const unsigned int lvl)
295 {
299 
300  double limite = 3; // Une limite de 3 pixels
301  limite = limite / cam.get_px(); // Transformation limite pixel en limite metre.
302 
303  unsigned int n = 0;
304 
305  // Parametre pour la premiere phase d'asservissement
306  double e_prev = 0, e_cur, e_next;
307 
308  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
309  if ((*it)->isTracked()) {
310  l = *it;
312 
313  double fac = 1;
314  if (iter == 0) {
315  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
316  ++itindex) {
317  int index = *itindex;
318  if (l->hiddenface->isAppearing((unsigned int)index)) {
319  fac = 0.2;
320  break;
321  }
322  if (l->closeToImageBorder(_I, 10)) {
323  fac = 0.1;
324  break;
325  }
326  }
327  }
328 
329  std::list<vpMeSite>::const_iterator itListLine;
330 
331  unsigned int indexFeature = 0;
332 
333  for (size_t a = 0; a < l->meline.size(); a++) {
334  if (iter == 0 && l->meline[a] != NULL)
335  itListLine = l->meline[a]->getMeList().begin();
336 
337  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
338  for (unsigned int j = 0; j < 6; j++) {
339  m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale
340  }
341  m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur
342 
343  if (m_error_edge[n + i] <= limite)
344  count = count + 1.0; // Si erreur proche de 0 on incremente cur
345 
346  m_w_edge[n + i] = 0;
347 
348  if (iter == 0) {
349  m_factor[n + i] = fac;
350  vpMeSite site = *itListLine;
351  if (site.getState() != vpMeSite::NO_SUPPRESSION)
352  m_factor[n + i] = 0.2;
353  ++itListLine;
354  }
355 
356  // If pour la premiere extremite des moving edges
357  if (indexFeature == 0) {
358  e_cur = l->error[0];
359  if (l->nbFeature[a] > 1) {
360  e_next = l->error[1];
361  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
362  m_w_edge[n + i] = 1 /*0.5*/;
363  }
364  e_prev = e_cur;
365  } else
366  m_w_edge[n + i] = 1;
367  }
368 
369  // If pour la derniere extremite des moving edges
370  else if (indexFeature == l->nbFeatureTotal - 1) {
371  e_cur = l->error[indexFeature];
372  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
373  m_w_edge[n + i] += 1 /*0.5*/;
374  }
375  }
376 
377  else {
378  e_cur = l->error[indexFeature];
379  e_next = l->error[indexFeature + 1];
380  if (fabs(e_cur - e_prev) < limite) {
381  m_w_edge[n + i] += 0.5;
382  }
383  if (fabs(e_cur - e_next) < limite) {
384  m_w_edge[n + i] += 0.5;
385  }
386  e_prev = e_cur;
387  }
388  indexFeature++;
389  }
390  n += l->nbFeature[a];
391  }
392  }
393  }
394 
395  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
396  ++it) {
397  if ((*it)->isTracked()) {
398  cy = *it;
400  double fac = 1.0;
401 
402  std::list<vpMeSite>::const_iterator itCyl1;
403  std::list<vpMeSite>::const_iterator itCyl2;
404  if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)) {
405  itCyl1 = cy->meline1->getMeList().begin();
406  itCyl2 = cy->meline2->getMeList().begin();
407  }
408 
409  for (unsigned int i = 0; i < cy->nbFeature; i++) {
410  for (unsigned int j = 0; j < 6; j++) {
411  m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale
412  }
413  m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur
414 
415  if (m_error_edge[n + i] <= limite)
416  count = count + 1.0; // Si erreur proche de 0 on incremente cur
417 
418  m_w_edge[n + i] = 0;
419 
420  if (iter == 0) {
421  m_factor[n + i] = fac;
422  vpMeSite site;
423  if (i < cy->nbFeaturel1) {
424  site = *itCyl1;
425  ++itCyl1;
426  } else {
427  site = *itCyl2;
428  ++itCyl2;
429  }
430  if (site.getState() != vpMeSite::NO_SUPPRESSION)
431  m_factor[n + i] = 0.2;
432  }
433 
434  // If pour la premiere extremite des moving edges
435  if (i == 0) {
436  e_cur = cy->error[0];
437  if (cy->nbFeature > 1) {
438  e_next = cy->error[1];
439  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
440  m_w_edge[n + i] = 1 /*0.5*/;
441  }
442  e_prev = e_cur;
443  } else
444  m_w_edge[n + i] = 1;
445  }
446  if (i == cy->nbFeaturel1) {
447  e_cur = cy->error[i];
448  if (cy->nbFeaturel2 > 1) {
449  e_next = cy->error[i + 1];
450  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
451  m_w_edge[n + i] = 1 /*0.5*/;
452  }
453  e_prev = e_cur;
454  } else
455  m_w_edge[n + i] = 1;
456  }
457 
458  // If pour la derniere extremite des moving edges
459  else if (i == cy->nbFeaturel1 - 1) {
460  e_cur = cy->error[i];
461  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
462  m_w_edge[n + i] += 1 /*0.5*/;
463  }
464  }
465  // If pour la derniere extremite des moving edges
466  else if (i == cy->nbFeature - 1) {
467  e_cur = cy->error[i];
468  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
469  m_w_edge[n + i] += 1 /*0.5*/;
470  }
471  }
472 
473  else {
474  e_cur = cy->error[i];
475  e_next = cy->error[i + 1];
476  if (fabs(e_cur - e_prev) < limite) {
477  m_w_edge[n + i] += 0.5;
478  }
479  if (fabs(e_cur - e_next) < limite) {
480  m_w_edge[n + i] += 0.5;
481  }
482  e_prev = e_cur;
483  }
484  }
485 
486  n += cy->nbFeature;
487  }
488  }
489 
490  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
491  if ((*it)->isTracked()) {
492  ci = *it;
494  double fac = 1.0;
495 
496  std::list<vpMeSite>::const_iterator itCir;
497  if (iter == 0 && (ci->meEllipse != NULL)) {
498  itCir = ci->meEllipse->getMeList().begin();
499  }
500 
501  for (unsigned int i = 0; i < ci->nbFeature; i++) {
502  for (unsigned int j = 0; j < 6; j++) {
503  m_L_edge[n + i][j] = ci->L[i][j]; // On remplit la matrice d'interaction globale
504  }
505  m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur
506 
507  if (m_error_edge[n + i] <= limite)
508  count = count + 1.0; // Si erreur proche de 0 on incremente cur
509 
510  m_w_edge[n + i] = 0;
511 
512  if (iter == 0) {
513  m_factor[n + i] = fac;
514  vpMeSite site = *itCir;
515  if (site.getState() != vpMeSite::NO_SUPPRESSION)
516  m_factor[n + i] = 0.2;
517  ++itCir;
518  }
519 
520  // If pour la premiere extremite des moving edges
521  if (i == 0) {
522  e_cur = ci->error[0];
523  if (ci->nbFeature > 1) {
524  e_next = ci->error[1];
525  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
526  m_w_edge[n + i] = 1 /*0.5*/;
527  }
528  e_prev = e_cur;
529  } else
530  m_w_edge[n + i] = 1;
531  }
532 
533  // If pour la derniere extremite des moving edges
534  else if (i == ci->nbFeature - 1) {
535  e_cur = ci->error[i];
536  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
537  m_w_edge[n + i] += 1 /*0.5*/;
538  }
539  }
540 
541  else {
542  e_cur = ci->error[i];
543  e_next = ci->error[i + 1];
544  if (fabs(e_cur - e_prev) < limite) {
545  m_w_edge[n + i] += 0.5;
546  }
547  if (fabs(e_cur - e_next) < limite) {
548  m_w_edge[n + i] += 0.5;
549  }
550  e_prev = e_cur;
551  }
552  }
553 
554  n += ci->nbFeature;
555  }
556  }
557 }
558 
560 {
564 
565  unsigned int n = 0;
566  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
567  if ((*it)->isTracked()) {
568  l = *it;
570 
571  double fac = 1;
572  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
573  ++itindex) {
574  int index = *itindex;
575  if (l->hiddenface->isAppearing((unsigned int)index)) {
576  fac = 0.2;
577  break;
578  }
579  if (l->closeToImageBorder(I, 10)) {
580  fac = 0.1;
581  break;
582  }
583  }
584 
585  unsigned int indexFeature = 0;
586  for (size_t a = 0; a < l->meline.size(); a++) {
587  std::list<vpMeSite>::const_iterator itListLine;
588  if (l->meline[a] != NULL) {
589  itListLine = l->meline[a]->getMeList().begin();
590 
591  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
592  m_factor[n + i] = fac;
593  vpMeSite site = *itListLine;
594  if (site.getState() != vpMeSite::NO_SUPPRESSION)
595  m_factor[n + i] = 0.2;
596  ++itListLine;
597  indexFeature++;
598  }
599  n += l->nbFeature[a];
600  }
601  }
602  }
603  }
604 
605  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
606  ++it) {
607  if ((*it)->isTracked()) {
608  cy = *it;
610 
611  std::list<vpMeSite>::const_iterator itCyl1;
612  std::list<vpMeSite>::const_iterator itCyl2;
613  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
614  itCyl1 = cy->meline1->getMeList().begin();
615  itCyl2 = cy->meline2->getMeList().begin();
616 
617  double fac = 1.0;
618  for (unsigned int i = 0; i < cy->nbFeature; i++) {
619  m_factor[n + i] = fac;
620  vpMeSite site;
621  if (i < cy->nbFeaturel1) {
622  site = *itCyl1;
623  ++itCyl1;
624  } else {
625  site = *itCyl2;
626  ++itCyl2;
627  }
628  if (site.getState() != vpMeSite::NO_SUPPRESSION)
629  m_factor[n + i] = 0.2;
630  }
631  n += cy->nbFeature;
632  }
633  }
634  }
635 
636  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
637  if ((*it)->isTracked()) {
638  ci = *it;
640 
641  std::list<vpMeSite>::const_iterator itCir;
642  if (ci->meEllipse != NULL) {
643  itCir = ci->meEllipse->getMeList().begin();
644  double fac = 1.0;
645 
646  for (unsigned int i = 0; i < ci->nbFeature; i++) {
647  m_factor[n + i] = fac;
648  vpMeSite site = *itCir;
649  if (site.getState() != vpMeSite::NO_SUPPRESSION)
650  m_factor[n + i] = 0.2;
651  ++itCir;
652  }
653  n += ci->nbFeature;
654  }
655  }
656  }
657 }
658 
659 void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
660 {
661  unsigned int nerror = m_weightedError_edge.getRows();
662 
663  double wi, eri;
664  if ((iter == 0) || m_computeInteraction) {
665  for (unsigned int i = 0; i < nerror; i++) {
666  wi = m_w_edge[i] * m_factor[i];
667  eri = m_error_edge[i];
668 
669  m_weightedError_edge[i] = wi * eri;
670 
671  for (unsigned int j = 0; j < 6; j++) {
672  m_L_edge[i][j] = wi * m_L_edge[i][j];
673  }
674  }
675  } else {
676  for (unsigned int i = 0; i < nerror; i++) {
677  wi = m_w_edge[i] * m_factor[i];
678  eri = m_error_edge[i];
679 
680  m_weightedError_edge[i] = wi * eri;
681  }
682  }
683 
685 
686  // If all the 6 dof should be estimated, we check if the interaction matrix
687  // is full rank. If not we remove automatically the dof that cannot be
688  // estimated This is particularly useful when consering circles (rank 5) and
689  // cylinders (rank 4)
690  if (isoJoIdentity_) {
691  cVo.buildFrom(cMo);
692 
693  vpMatrix K; // kernel
694  unsigned int rank = (m_L_edge * cVo).kernel(K);
695  if (rank == 0) {
696  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
697  }
698  if (rank != 6) {
699  vpMatrix I; // Identity
700  I.eye(6);
701  oJo = I - K.AtA();
702 
703  isoJoIdentity_ = false;
704  }
705  }
706 
707  vpColVector v;
708  vpMatrix LTL;
709  vpColVector LTR;
710 
711  if (isoJoIdentity_) {
712  LTL = m_L_edge.AtA();
714  v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
715  } else {
716  cVo.buildFrom(cMo);
717  vpMatrix LVJ = (m_L_edge * cVo * oJo);
718  vpMatrix LVJTLVJ = (LVJ).AtA();
719  vpColVector LVJTR;
720  computeJTR(LVJ, m_weightedError_edge, LVJTR);
721  v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
722  v = cVo * v;
723  }
724 
726 }
727 
729 {
730  // Nombre de moving edges
731  unsigned int nbrow = 0;
732  unsigned int nberrors_lines = 0;
733  unsigned int nberrors_cylinders = 0;
734  unsigned int nberrors_circles = 0;
735 
736  nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
737 
738  if (nbrow == 0) {
740  "No data found to compute the interaction matrix...");
741  }
742 
743  m_L_edge.resize(nbrow, 6, false, false);
744  m_error_edge.resize(nbrow, false);
745 
746  m_weightedError_edge.resize(nbrow, false);
747  m_w_edge.resize(nbrow, false);
748  m_w_edge = 1;
749  m_factor.resize(nbrow, false);
750  m_factor = 1;
751 
752  m_robustLines.resize(nberrors_lines);
753  m_robustCylinders.resize(nberrors_cylinders);
754  m_robustCircles.resize(nberrors_circles);
758 
759  m_wLines.resize(nberrors_lines, false);
760  m_wLines = 1;
761  m_wCylinders.resize(nberrors_cylinders, false);
762  m_wCylinders = 1;
763  m_wCircles.resize(nberrors_circles, false);
764  m_wCircles = 1;
765 
766  m_errorLines.resize(nberrors_lines, false);
767  m_errorCylinders.resize(nberrors_cylinders, false);
768  m_errorCircles.resize(nberrors_circles, false);
769 }
770 
772 {
773  throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
774  "computeVVSInteractionMatrixAndR"
775  "esidu() should not be called!");
776 }
777 
779 {
783 
784  unsigned int n = 0;
785  unsigned int nlines = 0;
786  unsigned int ncylinders = 0;
787  unsigned int ncircles = 0;
788 
789  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
790  ++it) {
791  if ((*it)->isTracked()) {
792  l = *it;
794  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
795  for (unsigned int j = 0; j < 6; j++) {
796  m_L_edge[n + i][j] = l->L[i][j];
797  m_error_edge[n + i] = l->error[i];
798  m_errorLines[nlines + i] = m_error_edge[n + i];
799  }
800  }
801  n += l->nbFeatureTotal;
802  nlines += l->nbFeatureTotal;
803  }
804  }
805 
806  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
807  it != cylinders[scaleLevel].end(); ++it) {
808  if ((*it)->isTracked()) {
809  cy = *it;
811  for (unsigned int i = 0; i < cy->nbFeature; i++) {
812  for (unsigned int j = 0; j < 6; j++) {
813  m_L_edge[n + i][j] = cy->L[i][j];
814  m_error_edge[n + i] = cy->error[i];
815  m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
816  }
817  }
818 
819  n += cy->nbFeature;
820  ncylinders += cy->nbFeature;
821  }
822  }
823 
824  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
825  it != circles[scaleLevel].end(); ++it) {
826  if ((*it)->isTracked()) {
827  ci = *it;
829  for (unsigned int i = 0; i < ci->nbFeature; i++) {
830  for (unsigned int j = 0; j < 6; j++) {
831  m_L_edge[n + i][j] = ci->L[i][j];
832  m_error_edge[n + i] = ci->error[i];
833  m_errorCircles[ncircles + i] = m_error_edge[n + i];
834  }
835  }
836 
837  n += ci->nbFeature;
838  ncircles += ci->nbFeature;
839  }
840  }
841 }
842 
844 {
845  unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
846  nberrors_circles = m_errorCircles.getRows();
847 
848  if (nberrors_lines > 0)
850  if (nberrors_cylinders > 0)
852  if (nberrors_circles > 0)
854 
858 }
859 
869 {
870  projectionError = 0.0;
871  unsigned int nbFeatures = 0;
872  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
873  ++it) {
874  vpMbtDistanceLine *l = *it;
875  if (l->isVisible() && l->isTracked()) {
876  for (size_t a = 0; a < l->meline.size(); a++) {
877  if (l->meline[a] != NULL) {
878  double lineNormGradient;
879  unsigned int lineNbFeatures;
880  l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
883  projectionError += lineNormGradient;
884  nbFeatures += lineNbFeatures;
885  }
886  }
887  }
888  }
889 
890  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
891  it != cylinders[scaleLevel].end(); ++it) {
892  vpMbtDistanceCylinder *cy = *it;
893  if (cy->isVisible() && cy->isTracked()) {
894  if (cy->meline1 != NULL) {
895  double cylinderNormGradient = 0;
896  unsigned int cylinderNbFeatures = 0;
897  cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
900  projectionError += cylinderNormGradient;
901  nbFeatures += cylinderNbFeatures;
902  }
903 
904  if (cy->meline2 != NULL) {
905  double cylinderNormGradient = 0;
906  unsigned int cylinderNbFeatures = 0;
907  cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
910  projectionError += cylinderNormGradient;
911  nbFeatures += cylinderNbFeatures;
912  }
913  }
914  }
915 
916  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
917  it != circles[scaleLevel].end(); ++it) {
918  vpMbtDistanceCircle *c = *it;
919  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
920  double circleNormGradient = 0;
921  unsigned int circleNbFeatures = 0;
922  c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
925  projectionError += circleNormGradient;
926  nbFeatures += circleNbFeatures;
927  }
928  }
929 
930  if (nbFeatures > 0) {
931  projectionError = vpMath::deg(projectionError / (double)nbFeatures);
932  } else {
933  projectionError = 90.0;
934  }
935 
937  // std::cout << "Norm Gradient = " << errorGradient << std::endl;
938 }
939 
946 {
947  int nbExpectedPoint = 0;
948  int nbGoodPoint = 0;
949  int nbBadPoint = 0;
950 
951  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
952  ++it) {
953  vpMbtDistanceLine *l = *it;
954  if (l->isVisible() && l->isTracked()) {
955  for (size_t a = 0; a < l->meline.size(); a++) {
956  if (l->meline[a] != NULL) {
957  nbExpectedPoint += (int)l->meline[a]->expecteddensity;
958  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
959  itme != l->meline[a]->getMeList().end(); ++itme) {
960  vpMeSite pix = *itme;
961  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
962  nbGoodPoint++;
963  else
964  nbBadPoint++;
965  }
966  }
967  }
968  }
969  }
970 
971  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
972  it != cylinders[scaleLevel].end(); ++it) {
973  vpMbtDistanceCylinder *cy = *it;
974  if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) {
975  nbExpectedPoint += (int)cy->meline1->expecteddensity;
976  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
977  itme1 != cy->meline1->getMeList().end(); ++itme1) {
978  vpMeSite pix = *itme1;
979  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
980  nbGoodPoint++;
981  else
982  nbBadPoint++;
983  }
984  nbExpectedPoint += (int)cy->meline2->expecteddensity;
985  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
986  itme2 != cy->meline2->getMeList().end(); ++itme2) {
987  vpMeSite pix = *itme2;
988  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
989  nbGoodPoint++;
990  else
991  nbBadPoint++;
992  }
993  }
994  }
995 
996  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
997  it != circles[scaleLevel].end(); ++it) {
998  vpMbtDistanceCircle *ci = *it;
999  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
1000  nbExpectedPoint += ci->meEllipse->getExpectedDensity();
1001  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
1002  itme != ci->meEllipse->getMeList().end(); ++itme) {
1003  vpMeSite pix = *itme;
1004  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
1005  nbGoodPoint++;
1006  else
1007  nbBadPoint++;
1008  }
1009  }
1010  }
1011 
1012  // Compare the number of good points with the min between the number of
1013  // expected points and number of points that are tracked
1014  int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
1015  // int nb_min = (std::min)(val1, val2);
1016  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1017  std::ostringstream oss;
1018  oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
1019  << ". Try to reduce the threshold=" << percentageGdPt
1020  << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1022  }
1023 }
1024 
1033 {
1034  initPyramid(I, Ipyramid);
1035 
1036  // for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
1037  unsigned int lvl = (unsigned int)scales.size();
1038  do {
1039  lvl--;
1040 
1041  projectionError = 90.0;
1042 
1043  if (scales[lvl]) {
1044  vpHomogeneousMatrix cMo_1 = cMo;
1045  try {
1046  downScale(lvl);
1047 
1048  try {
1049  trackMovingEdge(*Ipyramid[lvl]);
1050  } catch (...) {
1051  vpTRACE("Error in moving edge tracking");
1052  throw;
1053  }
1054 
1055  // initialize the vector that contains the error and the matrix that
1056  // contains the interaction matrix AY: Useless as it is done in
1057  // coputeVVS()
1058  /*
1059  for(std::list<vpMbtDistanceLine*>::const_iterator
1060  it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
1061  (l->isVisible()){ l->initInteractionMatrixError();
1062  }
1063  }
1064 
1065  for(std::list<vpMbtDistanceCylinder*>::const_iterator
1066  it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
1067  if(cy->isVisible()) {
1068  cy->initInteractionMatrixError();
1069  }
1070  }
1071 
1072  for(std::list<vpMbtDistanceCircle*>::const_iterator
1073  it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
1074  (ci->isVisible()){ ci->initInteractionMatrixError();
1075  }
1076  }
1077  */
1078 
1079  try {
1080  computeVVS(*Ipyramid[lvl], lvl);
1081  } catch (...) {
1082  covarianceMatrix = -1;
1083  throw; // throw the original exception
1084  }
1085 
1086  testTracking();
1087 
1088  if (displayFeatures) {
1089  displayFeaturesOnImage(I, lvl);
1090  }
1091 
1092  // Looking for new visible face
1093  bool newvisibleface = false;
1094  visibleFace(I, cMo, newvisibleface);
1095 
1096  // cam.computeFov(I.getWidth(), I.getHeight());
1097  if (useScanLine) {
1100  }
1101 
1102  updateMovingEdge(I);
1103 
1104  initMovingEdge(I, cMo);
1105  // Reinit the moving edge for the lines which need it.
1106  reinitMovingEdge(I, cMo);
1107 
1108  if (computeProjError)
1110 
1111  upScale(lvl);
1112  } catch (const vpException &e) {
1113  if (lvl != 0) {
1114  cMo = cMo_1;
1115  reInitLevel(lvl);
1116  upScale(lvl);
1117  } else {
1118  upScale(lvl);
1119  throw(e);
1120  }
1121  }
1122  }
1123  } while (lvl != 0);
1124 
1126 }
1127 
1134 {
1135  if (!modelInitialised) {
1136  throw vpException(vpException::fatalError, "model not initialized");
1137  }
1138 
1139  bool a = false;
1140 
1141 #ifdef VISP_HAVE_OGRE
1142  if (useOgre) {
1143  if (!faces.isOgreInitialised()) {
1146  faces.initOgre(cam);
1147  // Turn off Ogre config dialog display for the next call to this
1148  // function since settings are saved in the ogre.cfg file and used
1149  // during the next call
1150  ogreShowConfigDialog = false;
1151  }
1152  }
1153 #endif
1154 
1155  if (clippingFlag > 2)
1156  cam.computeFov(I.getWidth(), I.getHeight());
1157 
1158  visibleFace(I, cMo, a);
1159  resetMovingEdge();
1160 
1161  if (useScanLine) {
1162  if (clippingFlag <= 2)
1163  cam.computeFov(I.getWidth(), I.getHeight());
1164 
1167  }
1168 
1169  initPyramid(I, Ipyramid);
1170  unsigned int i = (unsigned int)scales.size();
1171  do {
1172  i--;
1173  if (scales[i]) {
1174  downScale(i);
1175  initMovingEdge(*Ipyramid[i], cMo);
1176  upScale(i);
1177  }
1178  } while (i != 0);
1179 
1181 }
1182 
1191 {
1192  cMo = cdMo;
1193 
1194  init(I);
1195 }
1196 
1210 void vpMbEdgeTracker::loadConfigFile(const std::string &configFile)
1211 {
1212  // Load projection error config
1213  vpMbTracker::loadConfigFile(configFile);
1214 
1215 #ifdef VISP_HAVE_XML2
1216  vpMbtXmlParser xmlp;
1217 
1218  xmlp.setCameraParameters(cam);
1221  xmlp.setMovingEdge(me);
1222 
1223  try {
1224  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1225  xmlp.parse(configFile);
1226  } catch (...) {
1227  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
1228  }
1229 
1230  vpCameraParameters camera;
1231  vpMe meParser;
1232  xmlp.getCameraParameters(camera);
1233  xmlp.getMe(meParser);
1234 
1235  setCameraParameters(camera);
1236  setMovingEdge(meParser);
1239 
1240  if (xmlp.hasNearClippingDistance())
1242 
1243  if (xmlp.hasFarClippingDistance())
1245 
1246  if (xmlp.getFovClipping())
1248 
1249  useLodGeneral = xmlp.getLodState();
1252 
1253  applyLodSettingInConfig = false;
1254  if (this->getNbPolygon() > 0) {
1255  applyLodSettingInConfig = true;
1259  }
1260 
1261 #else
1262  vpTRACE("You need the libXML2 to read the config file %s", configFile.c_str());
1263 #endif
1264 }
1265 
1278  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1279  const bool displayFullModel)
1280 {
1281  for (unsigned int i = 0; i < scales.size(); i += 1) {
1282  if (scales[i]) {
1283  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1284  ++it) {
1285  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1286  }
1287 
1288  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1289  it != cylinders[scaleLevel].end(); ++it) {
1290  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1291  }
1292 
1293  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1294  it != circles[scaleLevel].end(); ++it) {
1295  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1296  }
1297 
1298  break; // displaying model on one scale only
1299  }
1300  }
1301 
1302 #ifdef VISP_HAVE_OGRE
1303  if (useOgre)
1304  faces.displayOgre(cMo_);
1305 #endif
1306 }
1307 
1320  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1321  const bool displayFullModel)
1322 {
1323  for (unsigned int i = 0; i < scales.size(); i += 1) {
1324  if (scales[i]) {
1325  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1326  ++it) {
1327  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1328  }
1329 
1330  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1331  it != cylinders[scaleLevel].end(); ++it) {
1332  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1333  }
1334 
1335  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1336  it != circles[scaleLevel].end(); ++it) {
1337  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1338  }
1339  break; // displaying model on one scale only
1340  }
1341  }
1342 
1343 #ifdef VISP_HAVE_OGRE
1344  if (useOgre)
1345  faces.displayOgre(cMo_);
1346 #endif
1347 }
1348 
1350 {
1351  if (lvl == 0) {
1352  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1353  vpMbtDistanceLine *l = *it;
1354  if (l->isVisible() && l->isTracked()) {
1355  l->displayMovingEdges(I);
1356  }
1357  }
1358 
1359  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1360  ++it) {
1361  vpMbtDistanceCylinder *cy = *it;
1362  if (cy->isVisible() && cy->isTracked()) {
1363  cy->displayMovingEdges(I);
1364  }
1365  }
1366 
1367  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1368  vpMbtDistanceCircle *ci = *it;
1369  if (ci->isVisible() && ci->isTracked()) {
1370  ci->displayMovingEdges(I);
1371  }
1372  }
1373  }
1374 }
1375 
1385 {
1386  const bool doNotTrack = false;
1387 
1388  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1389  ++it) {
1390  vpMbtDistanceLine *l = *it;
1391  bool isvisible = false;
1392 
1393  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1394  ++itindex) {
1395  int index = *itindex;
1396  if (index == -1)
1397  isvisible = true;
1398  else {
1399  if (l->hiddenface->isVisible((unsigned int)index))
1400  isvisible = true;
1401  }
1402  }
1403 
1404  // Si la ligne n'appartient a aucune face elle est tout le temps visible
1405  if (l->Lindex_polygon.empty())
1406  isvisible = true; // Not sure that this can occur
1407 
1408  if (isvisible) {
1409  l->setVisible(true);
1410  l->updateTracked();
1411  if (l->meline.empty() && l->isTracked())
1412  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1413  } else {
1414  l->setVisible(false);
1415  for (size_t a = 0; a < l->meline.size(); a++) {
1416  if (l->meline[a] != NULL)
1417  delete l->meline[a];
1418  if (a < l->nbFeature.size())
1419  l->nbFeature[a] = 0;
1420  }
1421  l->nbFeatureTotal = 0;
1422  l->meline.clear();
1423  l->nbFeature.clear();
1424  }
1425  }
1426 
1427  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1428  it != cylinders[scaleLevel].end(); ++it) {
1429  vpMbtDistanceCylinder *cy = *it;
1430 
1431  bool isvisible = false;
1432 
1433  int index = cy->index_polygon;
1434  if (index == -1)
1435  isvisible = true;
1436  else {
1437  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1438  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1439  isvisible = true;
1440  }
1441  // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1442 
1443  if (isvisible) {
1444  cy->setVisible(true);
1445  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1446  if (cy->isTracked())
1447  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1448  }
1449  } else {
1450  cy->setVisible(false);
1451  if (cy->meline1 != NULL)
1452  delete cy->meline1;
1453  if (cy->meline2 != NULL)
1454  delete cy->meline2;
1455  cy->meline1 = NULL;
1456  cy->meline2 = NULL;
1457  cy->nbFeature = 0;
1458  cy->nbFeaturel1 = 0;
1459  cy->nbFeaturel2 = 0;
1460  }
1461  }
1462 
1463  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1464  it != circles[scaleLevel].end(); ++it) {
1465  vpMbtDistanceCircle *ci = *it;
1466  bool isvisible = false;
1467 
1468  int index = ci->index_polygon;
1469  if (index == -1)
1470  isvisible = true;
1471  else {
1472  if (ci->hiddenface->isVisible((unsigned int)index))
1473  isvisible = true;
1474  }
1475 
1476  if (isvisible) {
1477  ci->setVisible(true);
1478  if (ci->meEllipse == NULL) {
1479  if (ci->isTracked())
1480  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1481  }
1482  } else {
1483  ci->setVisible(false);
1484  if (ci->meEllipse != NULL)
1485  delete ci->meEllipse;
1486  ci->meEllipse = NULL;
1487  ci->nbFeature = 0;
1488  }
1489  }
1490 }
1491 
1498 {
1499  const bool doNotTrack = false;
1500 
1501  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1502  ++it) {
1503  vpMbtDistanceLine *l = *it;
1504  if (l->isVisible() && l->isTracked()) {
1505  if (l->meline.empty()) {
1506  l->initMovingEdge(I, cMo, doNotTrack, m_mask);
1507  }
1508  l->trackMovingEdge(I);
1509  }
1510  }
1511 
1512  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1513  it != cylinders[scaleLevel].end(); ++it) {
1514  vpMbtDistanceCylinder *cy = *it;
1515  if (cy->isVisible() && cy->isTracked()) {
1516  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1517  cy->initMovingEdge(I, cMo, doNotTrack, m_mask);
1518  }
1519  cy->trackMovingEdge(I, cMo);
1520  }
1521  }
1522 
1523  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1524  it != circles[scaleLevel].end(); ++it) {
1525  vpMbtDistanceCircle *ci = *it;
1526  if (ci->isVisible() && ci->isTracked()) {
1527  if (ci->meEllipse == NULL) {
1528  ci->initMovingEdge(I, cMo, doNotTrack, m_mask);
1529  }
1530  ci->trackMovingEdge(I, cMo);
1531  }
1532  }
1533 }
1534 
1541 {
1542  vpMbtDistanceLine *l;
1543  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1544  ++it) {
1545  if ((*it)->isTracked()) {
1546  l = *it;
1547  l->updateMovingEdge(I, cMo);
1548  if (l->nbFeatureTotal == 0 && l->isVisible()) {
1549  l->Reinit = true;
1550  }
1551  }
1552  }
1553 
1555  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1556  it != cylinders[scaleLevel].end(); ++it) {
1557  if ((*it)->isTracked()) {
1558  cy = *it;
1559  cy->updateMovingEdge(I, cMo);
1560  if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1561  cy->Reinit = true;
1562  }
1563  }
1564  }
1565 
1566  vpMbtDistanceCircle *ci;
1567  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1568  it != circles[scaleLevel].end(); ++it) {
1569  if ((*it)->isTracked()) {
1570  ci = *it;
1571  ci->updateMovingEdge(I, cMo);
1572  if (ci->nbFeature == 0 && ci->isVisible()) {
1573  ci->Reinit = true;
1574  }
1575  }
1576  }
1577 }
1578 
1580 {
1581  unsigned int n = 0;
1582 
1583  vpMbtDistanceLine *l;
1584  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1585  ++it) {
1586  if ((*it)->isTracked()) {
1587  l = *it;
1588  unsigned int indexLine = 0;
1589  double wmean = 0;
1590  for (size_t a = 0; a < l->meline.size(); a++) {
1591  if (l->nbFeature[a] > 0) {
1592  std::list<vpMeSite>::iterator itListLine;
1593  itListLine = l->meline[a]->getMeList().begin();
1594 
1595  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1596  wmean += m_w_edge[n + indexLine];
1597  vpMeSite p = *itListLine;
1598  if (m_w_edge[n + indexLine] < 0.5) {
1600 
1601  *itListLine = p;
1602  }
1603 
1604  ++itListLine;
1605  indexLine++;
1606  }
1607  }
1608  }
1609  n += l->nbFeatureTotal;
1610 
1611  if (l->nbFeatureTotal != 0)
1612  wmean /= l->nbFeatureTotal;
1613  else
1614  wmean = 1;
1615 
1616  l->setMeanWeight(wmean);
1617 
1618  if (wmean < 0.8)
1619  l->Reinit = true;
1620  }
1621  }
1622 
1623  // Same thing with cylinders as with lines
1625  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1626  it != cylinders[scaleLevel].end(); ++it) {
1627  if ((*it)->isTracked()) {
1628  cy = *it;
1629  double wmean = 0;
1630  std::list<vpMeSite>::iterator itListCyl1;
1631  std::list<vpMeSite>::iterator itListCyl2;
1632 
1633  if (cy->nbFeature > 0) {
1634  itListCyl1 = cy->meline1->getMeList().begin();
1635  itListCyl2 = cy->meline2->getMeList().begin();
1636 
1637  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1638  wmean += m_w_edge[n + i];
1639  vpMeSite p = *itListCyl1;
1640  if (m_w_edge[n + i] < 0.5) {
1642 
1643  *itListCyl1 = p;
1644  }
1645 
1646  ++itListCyl1;
1647  }
1648  }
1649 
1650  if (cy->nbFeaturel1 != 0)
1651  wmean /= cy->nbFeaturel1;
1652  else
1653  wmean = 1;
1654 
1655  cy->setMeanWeight1(wmean);
1656 
1657  if (wmean < 0.8) {
1658  cy->Reinit = true;
1659  }
1660 
1661  wmean = 0;
1662  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1663  wmean += m_w_edge[n + i];
1664  vpMeSite p = *itListCyl2;
1665  if (m_w_edge[n + i] < 0.5) {
1667 
1668  *itListCyl2 = p;
1669  }
1670 
1671  ++itListCyl2;
1672  }
1673 
1674  if (cy->nbFeaturel2 != 0)
1675  wmean /= cy->nbFeaturel2;
1676  else
1677  wmean = 1;
1678 
1679  cy->setMeanWeight2(wmean);
1680 
1681  if (wmean < 0.8) {
1682  cy->Reinit = true;
1683  }
1684 
1685  n += cy->nbFeature;
1686  }
1687  }
1688 
1689  // Same thing with circles as with lines
1690  vpMbtDistanceCircle *ci;
1691  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1692  it != circles[scaleLevel].end(); ++it) {
1693  if ((*it)->isTracked()) {
1694  ci = *it;
1695  double wmean = 0;
1696  std::list<vpMeSite>::iterator itListCir;
1697 
1698  if (ci->nbFeature > 0) {
1699  itListCir = ci->meEllipse->getMeList().begin();
1700  }
1701 
1702  wmean = 0;
1703  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1704  wmean += m_w_edge[n + i];
1705  vpMeSite p = *itListCir;
1706  if (m_w_edge[n + i] < 0.5) {
1708 
1709  *itListCir = p;
1710  }
1711 
1712  ++itListCir;
1713  }
1714 
1715  if (ci->nbFeature != 0)
1716  wmean /= ci->nbFeature;
1717  else
1718  wmean = 1;
1719 
1720  ci->setMeanWeight(wmean);
1721 
1722  if (wmean < 0.8) {
1723  ci->Reinit = true;
1724  }
1725 
1726  n += ci->nbFeature;
1727  }
1728  }
1729 }
1730 
1741 {
1742  vpMbtDistanceLine *l;
1743  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1744  ++it) {
1745  if ((*it)->isTracked()) {
1746  l = *it;
1747  if (l->Reinit && l->isVisible())
1748  l->reinitMovingEdge(I, _cMo, m_mask);
1749  }
1750  }
1751 
1753  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1754  it != cylinders[scaleLevel].end(); ++it) {
1755  if ((*it)->isTracked()) {
1756  cy = *it;
1757  if (cy->Reinit && cy->isVisible())
1758  cy->reinitMovingEdge(I, _cMo, m_mask);
1759  }
1760  }
1761 
1762  vpMbtDistanceCircle *ci;
1763  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1764  it != circles[scaleLevel].end(); ++it) {
1765  if ((*it)->isTracked()) {
1766  ci = *it;
1767  if (ci->Reinit && ci->isVisible())
1768  ci->reinitMovingEdge(I, _cMo, m_mask);
1769  }
1770  }
1771 }
1772 
1774 {
1775  for (unsigned int i = 0; i < scales.size(); i += 1) {
1776  if (scales[i]) {
1777  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1778  for (size_t a = 0; a < (*it)->meline.size(); a++) {
1779  if ((*it)->meline[a] != NULL) {
1780  delete (*it)->meline[a];
1781  (*it)->meline[a] = NULL;
1782  }
1783  }
1784 
1785  (*it)->meline.clear();
1786  (*it)->nbFeature.clear();
1787  (*it)->nbFeatureTotal = 0;
1788  }
1789 
1790  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1791  ++it) {
1792  if ((*it)->meline1 != NULL) {
1793  delete (*it)->meline1;
1794  (*it)->meline1 = NULL;
1795  }
1796  if ((*it)->meline2 != NULL) {
1797  delete (*it)->meline2;
1798  (*it)->meline2 = NULL;
1799  }
1800 
1801  (*it)->nbFeature = 0;
1802  (*it)->nbFeaturel1 = 0;
1803  (*it)->nbFeaturel2 = 0;
1804  }
1805 
1806  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1807  if ((*it)->meEllipse != NULL) {
1808  delete (*it)->meEllipse;
1809  (*it)->meEllipse = NULL;
1810  }
1811  (*it)->nbFeature = 0;
1812  }
1813  }
1814  }
1815 }
1816 
1829 void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1830 {
1831  {
1832  // suppress line already in the model
1833  bool already_here = false;
1834  vpMbtDistanceLine *l;
1835 
1836  for (unsigned int i = 0; i < scales.size(); i += 1) {
1837  if (scales[i]) {
1838  downScale(i);
1839  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1840  l = *it;
1841  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
1842  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
1843  already_here = true;
1844  l->addPolygon(polygon);
1845  l->hiddenface = &faces;
1846  }
1847  }
1848 
1849  if (!already_here) {
1850  l = new vpMbtDistanceLine;
1851 
1853  l->buildFrom(P1, P2);
1854  l->addPolygon(polygon);
1855  l->setMovingEdge(&me);
1856  l->hiddenface = &faces;
1857  l->useScanLine = useScanLine;
1858 
1859  l->setIndex(nline);
1860  l->setName(name);
1861 
1864 
1865  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
1867 
1868  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
1870 
1871  nline += 1;
1872  lines[i].push_back(l);
1873  }
1874  upScale(i);
1875  }
1876  }
1877  }
1878 }
1879 
1885 void vpMbEdgeTracker::removeLine(const std::string &name)
1886 {
1887  vpMbtDistanceLine *l;
1888 
1889  for (unsigned int i = 0; i < scales.size(); i++) {
1890  if (scales[i]) {
1891  for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1892  l = *it;
1893  if (name.compare(l->getName()) == 0) {
1894  lines[i].erase(it);
1895  break;
1896  }
1897  }
1898  }
1899  }
1900 }
1901 
1912 void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace,
1913  const std::string &name)
1914 {
1915  {
1916  bool already_here = false;
1917  vpMbtDistanceCircle *ci;
1918 
1919  for (unsigned int i = 0; i < scales.size(); i += 1) {
1920  if (scales[i]) {
1921  downScale(i);
1922  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1923  ci = *it;
1924  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
1925  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
1926  already_here =
1927  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
1928  }
1929  }
1930 
1931  if (!already_here) {
1932  ci = new vpMbtDistanceCircle;
1933 
1934  ci->setCameraParameters(cam);
1935  ci->buildFrom(P1, P2, P3, r);
1936  ci->setMovingEdge(&me);
1937  ci->setIndex(ncircle);
1938  ci->setName(name);
1939  ci->index_polygon = idFace;
1940  ci->hiddenface = &faces;
1941 
1942  // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
1943  // ci->getPolygon().setClipping(clippingFlag);
1944 
1945  // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
1946  // vpPolygon3D::NEAR_CLIPPING)
1947  // ci->getPolygon().setNearClippingDistance(distNearClip);
1948 
1949  // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
1950  // vpPolygon3D::FAR_CLIPPING)
1951  // ci->getPolygon().setFarClippingDistance(distFarClip);
1952 
1953  ncircle += 1;
1954  circles[i].push_back(ci);
1955  }
1956  upScale(i);
1957  }
1958  }
1959  }
1960 }
1961 
1971 void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace,
1972  const std::string &name)
1973 {
1974  {
1975  bool already_here = false;
1977 
1978  for (unsigned int i = 0; i < scales.size(); i += 1) {
1979  if (scales[i]) {
1980  downScale(i);
1981  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1982  ++it) {
1983  cy = *it;
1984  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
1985  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
1986  already_here =
1987  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
1988  }
1989  }
1990 
1991  if (!already_here) {
1992  cy = new vpMbtDistanceCylinder;
1993 
1994  cy->setCameraParameters(cam);
1995  cy->buildFrom(P1, P2, r);
1996  cy->setMovingEdge(&me);
1997  cy->setIndex(ncylinder);
1998  cy->setName(name);
1999  cy->index_polygon = idFace;
2000  cy->hiddenface = &faces;
2001  ncylinder += 1;
2002  cylinders[i].push_back(cy);
2003  }
2004  upScale(i);
2005  }
2006  }
2007  }
2008 }
2009 
2015 void vpMbEdgeTracker::removeCylinder(const std::string &name)
2016 {
2018 
2019  for (unsigned int i = 0; i < scales.size(); i++) {
2020  if (scales[i]) {
2021  for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
2022  cy = *it;
2023  if (name.compare(cy->getName()) == 0) {
2024  cylinders[i].erase(it);
2025  break;
2026  }
2027  }
2028  }
2029  }
2030 }
2031 
2037 void vpMbEdgeTracker::removeCircle(const std::string &name)
2038 {
2039  vpMbtDistanceCircle *ci;
2040 
2041  for (unsigned int i = 0; i < scales.size(); i++) {
2042  if (scales[i]) {
2043  for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2044  ci = *it;
2045  if (name.compare(ci->getName()) == 0) {
2046  circles[i].erase(it);
2047  break;
2048  }
2049  }
2050  }
2051  }
2052 }
2053 
2060 {
2061  unsigned int nbpt = p.getNbPoint();
2062  if (nbpt > 0) {
2063  for (unsigned int i = 0; i < nbpt - 1; i++)
2064  addLine(p.p[i], p.p[i + 1], p.getIndex());
2065  addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
2066  }
2067 }
2068 
2081  bool &newvisibleline)
2082 {
2083  unsigned int n;
2084  bool changed = false;
2085 
2086  if (!useOgre) {
2087  // n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89),
2088  // changed);
2089  n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2090  } else {
2091 #ifdef VISP_HAVE_OGRE
2092  n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2093 #else
2094  n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2095 #endif
2096  }
2097 
2098  if (n > nbvisiblepolygone) {
2099  // cout << "une nouvelle face est visible " << endl;
2100  newvisibleline = true;
2101  } else
2102  newvisibleline = false;
2103 
2104  nbvisiblepolygone = n;
2105 }
2106 
2123 {
2124  unsigned int nbpt = polygon.getNbPoint();
2125  if (nbpt > 0) {
2126  for (unsigned int i = 0; i < nbpt - 1; i++)
2127  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2128  vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
2129  }
2130 }
2147 {
2148  unsigned int nbpt = polygon.getNbPoint();
2149  if (nbpt > 0) {
2150  for (unsigned int i = 0; i < nbpt - 1; i++)
2151  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2152  }
2153 }
2154 
2155 unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
2156  unsigned int &nberrors_circles)
2157 {
2158  unsigned int nbrow = 0;
2159  nberrors_lines = 0;
2160  nberrors_cylinders = 0;
2161  nberrors_circles = 0;
2162 
2163  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2164  ++it) {
2165 
2166  vpMbtDistanceLine *l = *it;
2167 
2168  if (l->isTracked()) {
2170  nbrow += l->nbFeatureTotal;
2171  nberrors_lines += l->nbFeatureTotal;
2172  }
2173  }
2174 
2175  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2176  it != cylinders[scaleLevel].end(); ++it) {
2177  vpMbtDistanceCylinder *cy = *it;
2178 
2179  if (cy->isTracked()) {
2181  nbrow += cy->nbFeature;
2182  nberrors_cylinders += cy->nbFeature;
2183  }
2184  }
2185 
2186  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2187  it != circles[scaleLevel].end(); ++it) {
2188  vpMbtDistanceCircle *ci = *it;
2189 
2190  if (ci->isTracked()) {
2192  nbrow += ci->nbFeature;
2193  nberrors_circles += ci->nbFeature;
2194  }
2195  }
2196 
2197  return nbrow;
2198 }
2199 
2211 void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
2212  const int idFace, const std::string &name)
2213 {
2214  addCircle(p1, p2, p3, radius, (int)idFace, name);
2215 }
2216 
2227 void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
2228  const std::string &name)
2229 {
2230  addCylinder(p1, p2, radius, (int)idFace, name);
2231 }
2232 
2239 {
2240  this->cMo.eye();
2241  vpMbtDistanceLine *l;
2243  vpMbtDistanceCircle *ci;
2244 
2245  for (unsigned int i = 0; i < scales.size(); i += 1) {
2246  if (scales[i]) {
2247  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2248  l = *it;
2249  if (l != NULL)
2250  delete l;
2251  l = NULL;
2252  }
2253 
2254  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2255  ++it) {
2256  cy = *it;
2257  if (cy != NULL)
2258  delete cy;
2259  cy = NULL;
2260  }
2261 
2262  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2263  ci = *it;
2264  if (ci != NULL)
2265  delete ci;
2266  ci = NULL;
2267  }
2268  lines[i].clear();
2269  cylinders[i].clear();
2270  circles[i].clear();
2271  }
2272  }
2273 
2274  faces.reset();
2275 
2276  useScanLine = false;
2277 
2278 #ifdef VISP_HAVE_OGRE
2279  useOgre = false;
2280 #endif
2281 
2282  m_computeInteraction = true;
2283  nline = 0;
2284  ncylinder = 0;
2285  m_lambda = 1.0;
2286  nbvisiblepolygone = 0;
2287  percentageGdPt = 0.4;
2288 
2289  angleAppears = vpMath::rad(89);
2292 
2294 
2295  // reinitialization of the scales.
2296  this->setScales(scales);
2297 }
2298 
2311 void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2312  const vpHomogeneousMatrix &cMo_, const bool verbose,
2313  const vpHomogeneousMatrix &T)
2314 {
2315  this->cMo.eye();
2316  vpMbtDistanceLine *l;
2318  vpMbtDistanceCircle *ci;
2319 
2320  for (unsigned int i = 0; i < scales.size(); i += 1) {
2321  if (scales[i]) {
2322  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2323  l = *it;
2324  if (l != NULL)
2325  delete l;
2326  l = NULL;
2327  }
2328 
2329  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2330  ++it) {
2331  cy = *it;
2332  if (cy != NULL)
2333  delete cy;
2334  cy = NULL;
2335  }
2336 
2337  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2338  ci = *it;
2339  if (ci != NULL)
2340  delete ci;
2341  ci = NULL;
2342  }
2343 
2344  lines[i].clear();
2345  cylinders[i].clear();
2346  circles[i].clear();
2347  }
2348  }
2349 
2350  faces.reset();
2351 
2352  // compute_interaction=1;
2353  nline = 0;
2354  ncylinder = 0;
2355  ncircle = 0;
2356  // lambda = 1;
2357  nbvisiblepolygone = 0;
2358 
2359  loadModel(cad_name, verbose, T);
2360  initFromPose(I, cMo_);
2361 }
2362 
2373 unsigned int vpMbEdgeTracker::getNbPoints(const unsigned int level) const
2374 {
2375  if ((level > scales.size()) || !scales[level]) {
2376  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2377  level);
2378  }
2379 
2380  unsigned int nbGoodPoints = 0;
2381  vpMbtDistanceLine *l;
2382  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2383  l = *it;
2384  if (l->isVisible() && l->isTracked()) {
2385  for (size_t a = 0; a < l->meline.size(); a++) {
2386  if (l->nbFeature[a] != 0)
2387  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2388  itme != l->meline[a]->getMeList().end(); ++itme) {
2389  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2390  nbGoodPoints++;
2391  }
2392  }
2393  }
2394  }
2395 
2397  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2398  ++it) {
2399  cy = *it;
2400  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
2401  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2402  itme1 != cy->meline1->getMeList().end(); ++itme1) {
2403  if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2404  nbGoodPoints++;
2405  }
2406  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2407  itme2 != cy->meline2->getMeList().end(); ++itme2) {
2408  if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2409  nbGoodPoints++;
2410  }
2411  }
2412  }
2413 
2414  vpMbtDistanceCircle *ci;
2415  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2416  ci = *it;
2417  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
2418  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2419  itme != ci->meEllipse->getMeList().end(); ++itme) {
2420  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2421  nbGoodPoints++;
2422  }
2423  }
2424  }
2425 
2426  return nbGoodPoints;
2427 }
2428 
2450 void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2451 {
2452  unsigned int nbActivatedLevels = 0;
2453  for (unsigned int i = 0; i < scale.size(); i++) {
2454  if (scale[i]) {
2455  nbActivatedLevels++;
2456  }
2457  }
2458 
2459  if (scale.empty() || (nbActivatedLevels == 0)) {
2460  vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2461  "tracking. Use the global one");
2462  this->scales.resize(0);
2463  this->scales.push_back(true);
2464 
2465  lines.resize(1);
2466  lines[0].clear();
2467 
2468  cylinders.resize(1);
2469  cylinders[0].clear();
2470 
2471  circles.resize(1);
2472  circles[0].clear();
2473  } else {
2474  this->scales = scale;
2475 
2476  lines.resize(scale.size());
2477  cylinders.resize(scale.size());
2478  circles.resize(scale.size());
2479 
2480  for (unsigned int i = 0; i < lines.size(); i++) {
2481  lines[i].clear();
2482  cylinders[i].clear();
2483  circles[i].clear();
2484  }
2485  }
2486 }
2487 
2494 {
2495  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2496  std::cerr << "Far clipping value cannot be inferior than near clipping "
2497  "value. Far clipping won't be considered."
2498  << std::endl;
2499  else if (dist < 0)
2500  std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2501  "won't be considered."
2502  << std::endl;
2503  else {
2505  vpMbtDistanceLine *l;
2506 
2507  for (unsigned int i = 0; i < scales.size(); i += 1) {
2508  if (scales[i]) {
2509  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2510  l = *it;
2512  }
2513  }
2514  }
2515  }
2516 }
2517 
2524 {
2525  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2526  std::cerr << "Near clipping value cannot be superior than far clipping "
2527  "value. Near clipping won't be considered."
2528  << std::endl;
2529  else if (dist < 0)
2530  std::cerr << "Near clipping value cannot be inferior than 0. Near "
2531  "clipping won't be considered."
2532  << std::endl;
2533  else {
2535  vpMbtDistanceLine *l;
2536 
2537  for (unsigned int i = 0; i < scales.size(); i += 1) {
2538  if (scales[i]) {
2539  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2540  l = *it;
2542  }
2543  }
2544  }
2545  }
2546 }
2547 
2555 void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2556 {
2557  vpMbTracker::setClipping(flags);
2558 
2559  vpMbtDistanceLine *l;
2560 
2561  for (unsigned int i = 0; i < scales.size(); i += 1) {
2562  if (scales[i]) {
2563  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2564  l = *it;
2566  }
2567  }
2568  }
2569 }
2570 
2587  std::vector<const vpImage<unsigned char> *> &_pyramid)
2588 {
2589  _pyramid.resize(scales.size());
2590 
2591  if (scales[0]) {
2592  _pyramid[0] = &_I;
2593  } else {
2594  _pyramid[0] = NULL;
2595  }
2596 
2597  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2598  if (scales[i]) {
2599  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2600  vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2601 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2602  IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
2603  vpI0->imageData = (char *)(_I.bitmap);
2604  IplImage *vpI =
2605  cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
2606  cvResize(vpI0, vpI, CV_INTER_NN);
2607  vpImageConvert::convert(vpI, *I);
2608  cvReleaseImage(&vpI);
2609  vpI0->imageData = NULL;
2610  cvReleaseImageHeader(&vpI0);
2611 #else
2612  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2613  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2614  (*I)[k][l] = _I[ii][jj];
2615  }
2616  }
2617 #endif
2618  _pyramid[i] = I;
2619  } else {
2620  _pyramid[i] = NULL;
2621  }
2622  }
2623 }
2624 
2631 void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2632 {
2633  if (_pyramid.size() > 0) {
2634  _pyramid[0] = NULL;
2635  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2636  if (_pyramid[i] != NULL) {
2637  delete _pyramid[i];
2638  _pyramid[i] = NULL;
2639  }
2640  }
2641  _pyramid.resize(0);
2642  }
2643 }
2644 
2655 void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
2656 {
2657  if (level > scales.size() || !scales[level]) {
2658  std::ostringstream oss;
2659  oss << level;
2660  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2661  throw vpException(vpException::dimensionError, errorMsg);
2662  }
2663 
2664  linesList = lines[level];
2665 }
2666 
2677 void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, const unsigned int level) const
2678 {
2679  if (level > scales.size() || !scales[level]) {
2680  std::ostringstream oss;
2681  oss << level;
2682  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2683  throw vpException(vpException::dimensionError, errorMsg);
2684  }
2685 
2686  cylindersList = cylinders[level];
2687 }
2688 
2699 void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
2700 {
2701  if (level > scales.size() || !scales[level]) {
2702  std::ostringstream oss;
2703  oss << level;
2704  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2705  throw vpException(vpException::dimensionError, errorMsg);
2706  }
2707 
2708  circlesList = circles[level];
2709 }
2710 
2717 void vpMbEdgeTracker::downScale(const unsigned int _scale)
2718 {
2719  const double ratio = pow(2., (int)_scale);
2720  scaleLevel = _scale;
2721 
2722  vpMatrix K = cam.get_K();
2723 
2724  K[0][0] /= ratio;
2725  K[1][1] /= ratio;
2726  K[0][2] /= ratio;
2727  K[1][2] /= ratio;
2728 
2730 }
2731 
2738 void vpMbEdgeTracker::upScale(const unsigned int _scale)
2739 {
2740  const double ratio = pow(2., (int)_scale);
2741  scaleLevel = 0;
2742 
2743  vpMatrix K = cam.get_K();
2744 
2745  K[0][0] *= ratio;
2746  K[1][1] *= ratio;
2747  K[0][2] *= ratio;
2748  K[1][2] *= ratio;
2749 
2751 }
2752 
2760 void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2761 {
2762  unsigned int scaleLevel_1 = scaleLevel;
2763  scaleLevel = _lvl;
2764 
2765  vpMbtDistanceLine *l;
2766  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2767  ++it) {
2768  if ((*it)->isTracked()) {
2769  l = *it;
2770  l->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2771  }
2772  }
2773 
2775  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2776  it != cylinders[scaleLevel].end(); ++it) {
2777  if ((*it)->isTracked()) {
2778  cy = *it;
2779  cy->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2780  }
2781  }
2782 
2783  vpMbtDistanceCircle *ci;
2784  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2785  it != circles[scaleLevel].end(); ++it) {
2786  if ((*it)->isTracked()) {
2787  ci = *it;
2788  ci->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2789  }
2790  }
2791 
2792  trackMovingEdge(*Ipyramid[_lvl]);
2793  updateMovingEdge(*Ipyramid[_lvl]);
2794  scaleLevel = scaleLevel_1;
2795 }
2796 
2804 void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2805 {
2806  for (unsigned int i = 0; i < scales.size(); i += 1) {
2807  if (scales[i]) {
2808  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2809  /*(*it)->setTracked(useEdgeTracking);
2810  for(std::list<int>::const_iterator
2811  itpoly=(*it)->Lindex_polygon.begin();
2812  itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2813  if(faces[(*itpoly)]->getName() != name){
2814  (*it)->setTracked(true);
2815  break;
2816  }
2817  }*/
2818 
2819  (*it)->setTracked(name, useEdgeTracking);
2820  }
2821 
2822  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2823  ++it) {
2824  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2825  (*it)->setTracked(useEdgeTracking);
2826  }
2827  }
2828 
2829  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2830  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2831  (*it)->setTracked(useEdgeTracking);
2832  }
2833  }
2834  }
2835  }
2836 }
bool m_computeInteraction
Definition: vpMbTracker.h:191
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
bool computeProjError
Definition: vpMbTracker.h:139
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:104
void displayMovingEdges(const vpImage< unsigned char > &I)
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:136
void initFromCalibrationMatrix(const vpMatrix &_K)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setMovingEdge(const vpMe &me)
void trackMovingEdge(const vpImage< unsigned char > &I)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:219
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:221
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:239
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void getCameraParameters(vpCameraParameters &_cam) const
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:149
bool Reinit
Indicates if the line has to be reinitialized.
void setMeanWeight2(const double wmean)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void init(const vpImage< unsigned char > &I)
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:133
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:171
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
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
vpColVector m_factor
Edge VVS variables.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:119
vpPoint * p1
The first extremity.
void displayMovingEdges(const vpImage< unsigned char > &I)
double getNearClippingDistance() const
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:129
void getMe(vpMe &_ecm) const
vpColVector m_w_edge
Robust weights.
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:161
bool getLodState() 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 displayFeaturesOnImage(const vpImage< unsigned char > &I, const unsigned int lvl)
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)
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
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:134
void loadConfigFile(const std::string &configFile)
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.
void downScale(const unsigned int _scale)
bool hasNearClippingDistance() const
Class that defines what is a point.
Definition: vpPoint.h:58
vpMatrix L
The interaction matrix.
vpMeSiteState getState() const
Definition: vpMeSite.h:188
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:117
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
virtual void setNearClippingDistance(const double &dist)
double getFarClippingDistance() const
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:157
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
double projectionError
Definition: vpMbTracker.h:142
bool hasFarClippingDistance() const
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:121
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.
double getAngleDisappear() const
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
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:164
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:108
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:183
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
void setAngleDisappear(const double &adisappear)
double getMinPolygonAreaThreshold() const
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.
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:215
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:199
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:193
virtual void setFarClippingDistance(const double &dist)
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:145
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:146
unsigned int getRows() const
Definition: vpArray2D.h:156
void setCameraParameters(const vpCameraParameters &_cam)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:151
vpColVector m_errorCylinders
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:174
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpColVector m_errorCircles
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:195
vpColVector m_wCylinders
vpRobust m_robustLines
double get_px() const
std::string getName() const
static double rad(double deg)
Definition: vpMath.h:102
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)
double getMinLineLengthThreshold() const
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:227
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="")
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:185
vpMatrix get_K() const
static double deg(double rad)
Definition: vpMath.h:95
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:144
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:162
bool applyLodSettingInConfig
Definition: vpMbTracker.h:181
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setAngleAppear(const double &aappear)
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:217
vpHomogeneousMatrix inverse() const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
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:153
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:368
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:178
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:1932
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.
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:159
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)
double getAngleAppear() const
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:262
vpColVector m_errorLines
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void removeLine(const std::string &name)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
bool getFovClipping() const
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:155
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:178
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)
void setMovingEdge(const vpMe &_ecm)
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:123
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:223
std::string getName() const
vpPoint * p1
The center of the circle.
bool useScanLine
Use scanline rendering.
void buildFrom(vpPoint &_p1, vpPoint &_p2)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
virtual void setLod(const bool useLod, const std::string &name="")
virtual void setNearClippingDistance(const double &dist)
void parse(const std::string &filename)
void computeFov(const unsigned int &w, const unsigned int &h)