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