Visual Servoing Platform  version 3.2.1 under development (2019-10-23) under development (2019-10-23)
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
unsigned int ncylinder
std::string getName() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
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
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
vpMeSiteState getState() const
Definition: vpMeSite.h:189
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)
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
int getIndex() const
Definition: vpMbtPolygon.h:101
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)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::string getName() const
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)
void getCameraParameters(vpCameraParameters &cam) 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::string getName() const
Definition: vpMbtPolygon.h:108
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.
bool isTracked() const
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
vpMatrix AtA() const
Definition: vpMatrix.cpp:693
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())
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.
virtual unsigned int getNbPoints(const unsigned int level=0) const
bool modelInitialised
Definition: vpMbTracker.h:123
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
unsigned int getRows() const
Definition: vpArray2D.h:289
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())
vpHomogeneousMatrix inverse() const
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
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)
vpMatrix get_K() const
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)
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.
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
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="")
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 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)
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)
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 cleanPyramid(std::vector< const vpImage< unsigned char > *> &_pyramid)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void setIndex(const unsigned int i)
std::string getName() const
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
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
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
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
vpColVector m_wCylinders
vpRobust m_robustLines
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 getLline(std::list< vpMbtDistanceLine *> &linesList, const unsigned int level=0) const
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)
void getLcircle(std::list< vpMbtDistanceCircle *> &circlesList, const unsigned int level=0) 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: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 getEdgeMe(vpMe &ecm) const
void setIndex(const unsigned int i)
bool isVisible() const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:101
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:186
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:364
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)
double getLodMinLineLengthThreshold() const
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
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)
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void computeVVSInit()
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
void setMeanWeight(const double w_mean)
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
vpColVector m_wLines
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 initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > *> &_pyramid)
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
unsigned int getWidth() const
Definition: vpImage.h:244
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
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:492
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
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 getLcylinder(std::list< vpMbtDistanceCylinder *> &cylindersList, const unsigned int level=0) const
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)
double getLodMinPolygonAreaThreshold() const
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)