Visual Servoing Platform  version 3.5.1 under development (2022-05-22)
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Make the complete tracking of an object by using its CAD model
33  *
34  * Authors:
35  * Nicolas Melchior
36  * Romain Tallonneau
37  * Eric Marchand
38  *
39  *****************************************************************************/
40 
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpExponentialMap.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMatrixException.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPolygon3D.h>
53 #include <visp3/core/vpTrackingException.h>
54 #include <visp3/core/vpVelocityTwistMatrix.h>
55 #include <visp3/mbt/vpMbEdgeTracker.h>
56 #include <visp3/mbt/vpMbtDistanceLine.h>
57 #include <visp3/mbt/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 =
1296  vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1297 
1298  for (size_t i = 0; i < models.size(); i++) {
1299  if (vpMath::equal(models[i][0], 0)) {
1300  vpImagePoint ip1(models[i][1], models[i][2]);
1301  vpImagePoint ip2(models[i][3], models[i][4]);
1302  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1303  } else if (vpMath::equal(models[i][0], 1)) {
1304  vpImagePoint center(models[i][1], models[i][2]);
1305  double n20 = models[i][3];
1306  double n11 = models[i][4];
1307  double n02 = models[i][5];
1308  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1309  }
1310  }
1311 
1312 #ifdef VISP_HAVE_OGRE
1313  if (useOgre)
1314  faces.displayOgre(cMo);
1315 #endif
1316 }
1317 
1330  const vpColor &col, unsigned int thickness, bool displayFullModel)
1331 {
1332  // Display first the Moving-Edges
1333  if (displayFeatures) {
1335  }
1336 
1337  std::vector<std::vector<double> > models =
1338  vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1339 
1340  for (size_t i = 0; i < models.size(); i++) {
1341  if (vpMath::equal(models[i][0], 0)) {
1342  vpImagePoint ip1(models[i][1], models[i][2]);
1343  vpImagePoint ip2(models[i][3], models[i][4]);
1344  vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1345  } else if (vpMath::equal(models[i][0], 1)) {
1346  vpImagePoint center(models[i][1], models[i][2]);
1347  double n20 = models[i][3];
1348  double n11 = models[i][4];
1349  double n02 = models[i][5];
1350  vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1351  }
1352  }
1353 
1354 #ifdef VISP_HAVE_OGRE
1355  if (useOgre)
1356  faces.displayOgre(cMo);
1357 #endif
1358 }
1359 
1360 std::vector<std::vector<double> > vpMbEdgeTracker::getFeaturesForDisplayEdge()
1361 {
1362  std::vector<std::vector<double> > features;
1363 
1364  const unsigned int lvl = 0;
1365  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1366  vpMbtDistanceLine *l = *it;
1367  if (l->isVisible() && l->isTracked()) {
1368  std::vector<std::vector<double> > currentFeatures = l->getFeaturesForDisplay();
1369  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1370  }
1371  }
1372 
1373  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1374  ++it) {
1375  vpMbtDistanceCylinder *cy = *it;
1376  if (cy->isVisible() && cy->isTracked()) {
1377  std::vector<std::vector<double> > currentFeatures = cy->getFeaturesForDisplay();
1378  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1379  }
1380  }
1381 
1382  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1383  vpMbtDistanceCircle *ci = *it;
1384  if (ci->isVisible() && ci->isTracked()) {
1385  std::vector<std::vector<double> > currentFeatures = ci->getFeaturesForDisplay();
1386  features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1387  }
1388  }
1389 
1390  return features;
1391 }
1392 
1408 std::vector<std::vector<double> > vpMbEdgeTracker::getModelForDisplay(unsigned int width, unsigned int height,
1409  const vpHomogeneousMatrix &cMo,
1410  const vpCameraParameters &cam,
1411  bool displayFullModel)
1412 {
1413  std::vector<std::vector<double> > models;
1414 
1415  for (unsigned int i = 0; i < scales.size(); i += 1) {
1416  if (scales[i]) {
1417  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1418  ++it) {
1419  std::vector<std::vector<double> > currentModel =
1420  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1421  models.insert(models.end(), currentModel.begin(), currentModel.end());
1422  }
1423 
1424  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1425  it != cylinders[scaleLevel].end(); ++it) {
1426  std::vector<std::vector<double> > currentModel =
1427  (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1428  models.insert(models.end(), currentModel.begin(), currentModel.end());
1429  }
1430 
1431  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1432  it != circles[scaleLevel].end(); ++it) {
1433  std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1434  if (!paramsCircle.empty()) {
1435  models.push_back(paramsCircle);
1436  }
1437  }
1438  break; // displaying model on one scale only
1439  }
1440  }
1441 
1442  return models;
1443 }
1444 
1446 {
1447  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1450  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1451 
1452  switch (state) {
1455  break;
1456 
1457  case vpMeSite::CONSTRAST:
1458  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1459  break;
1460 
1461  case vpMeSite::THRESHOLD:
1463  break;
1464 
1465  case vpMeSite::M_ESTIMATOR:
1466  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1467  break;
1468 
1469  case vpMeSite::TOO_NEAR:
1470  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1471  break;
1472 
1473  default:
1475  }
1476  }
1477  }
1478 }
1479 
1481 {
1482  for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1485  int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1486 
1487  switch (state) {
1490  break;
1491 
1492  case vpMeSite::CONSTRAST:
1493  vpDisplay::displayCross(I, ip, 3, vpColor::blue, 1);
1494  break;
1495 
1496  case vpMeSite::THRESHOLD:
1498  break;
1499 
1500  case vpMeSite::M_ESTIMATOR:
1501  vpDisplay::displayCross(I, ip, 3, vpColor::red, 1);
1502  break;
1503 
1504  case vpMeSite::TOO_NEAR:
1505  vpDisplay::displayCross(I, ip, 3, vpColor::cyan, 1);
1506  break;
1507 
1508  default:
1510  }
1511  }
1512  }
1513 }
1514 
1524 {
1525  const bool doNotTrack = false;
1526 
1527  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1528  ++it) {
1529  vpMbtDistanceLine *l = *it;
1530  bool isvisible = false;
1531 
1532  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1533  ++itindex) {
1534  int index = *itindex;
1535  if (index == -1)
1536  isvisible = true;
1537  else {
1538  if (l->hiddenface->isVisible((unsigned int)index))
1539  isvisible = true;
1540  }
1541  }
1542 
1543  // Si la ligne n'appartient a aucune face elle est tout le temps visible
1544  if (l->Lindex_polygon.empty())
1545  isvisible = true; // Not sure that this can occur
1546 
1547  if (isvisible) {
1548  l->setVisible(true);
1549  l->updateTracked();
1550  if (l->meline.empty() && l->isTracked())
1551  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1552  } else {
1553  l->setVisible(false);
1554  for (size_t a = 0; a < l->meline.size(); a++) {
1555  if (l->meline[a] != NULL)
1556  delete l->meline[a];
1557  if (a < l->nbFeature.size())
1558  l->nbFeature[a] = 0;
1559  }
1560  l->nbFeatureTotal = 0;
1561  l->meline.clear();
1562  l->nbFeature.clear();
1563  }
1564  }
1565 
1566  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1567  it != cylinders[scaleLevel].end(); ++it) {
1568  vpMbtDistanceCylinder *cy = *it;
1569 
1570  bool isvisible = false;
1571 
1572  int index = cy->index_polygon;
1573  if (index == -1)
1574  isvisible = true;
1575  else {
1576  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1577  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1578  isvisible = true;
1579  }
1580  // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1581 
1582  if (isvisible) {
1583  cy->setVisible(true);
1584  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1585  if (cy->isTracked())
1586  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1587  }
1588  } else {
1589  cy->setVisible(false);
1590  if (cy->meline1 != NULL)
1591  delete cy->meline1;
1592  if (cy->meline2 != NULL)
1593  delete cy->meline2;
1594  cy->meline1 = NULL;
1595  cy->meline2 = NULL;
1596  cy->nbFeature = 0;
1597  cy->nbFeaturel1 = 0;
1598  cy->nbFeaturel2 = 0;
1599  }
1600  }
1601 
1602  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1603  it != circles[scaleLevel].end(); ++it) {
1604  vpMbtDistanceCircle *ci = *it;
1605  bool isvisible = false;
1606 
1607  int index = ci->index_polygon;
1608  if (index == -1)
1609  isvisible = true;
1610  else {
1611  if (ci->hiddenface->isVisible((unsigned int)index))
1612  isvisible = true;
1613  }
1614 
1615  if (isvisible) {
1616  ci->setVisible(true);
1617  if (ci->meEllipse == NULL) {
1618  if (ci->isTracked())
1619  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1620  }
1621  } else {
1622  ci->setVisible(false);
1623  if (ci->meEllipse != NULL)
1624  delete ci->meEllipse;
1625  ci->meEllipse = NULL;
1626  ci->nbFeature = 0;
1627  }
1628  }
1629 }
1630 
1637 {
1638  const bool doNotTrack = false;
1639 
1640  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1641  ++it) {
1642  vpMbtDistanceLine *l = *it;
1643  if (l->isVisible() && l->isTracked()) {
1644  if (l->meline.empty()) {
1645  l->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1646  }
1647  l->trackMovingEdge(I);
1648  }
1649  }
1650 
1651  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1652  it != cylinders[scaleLevel].end(); ++it) {
1653  vpMbtDistanceCylinder *cy = *it;
1654  if (cy->isVisible() && cy->isTracked()) {
1655  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1656  cy->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1657  }
1658  cy->trackMovingEdge(I, m_cMo);
1659  }
1660  }
1661 
1662  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1663  it != circles[scaleLevel].end(); ++it) {
1664  vpMbtDistanceCircle *ci = *it;
1665  if (ci->isVisible() && ci->isTracked()) {
1666  if (ci->meEllipse == NULL) {
1667  ci->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1668  }
1669  ci->trackMovingEdge(I, m_cMo);
1670  }
1671  }
1672 }
1673 
1680 {
1681  vpMbtDistanceLine *l;
1682  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1683  ++it) {
1684  if ((*it)->isTracked()) {
1685  l = *it;
1686  l->updateMovingEdge(I, m_cMo);
1687  if (l->nbFeatureTotal == 0 && l->isVisible()) {
1688  l->Reinit = true;
1689  }
1690  }
1691  }
1692 
1694  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1695  it != cylinders[scaleLevel].end(); ++it) {
1696  if ((*it)->isTracked()) {
1697  cy = *it;
1698  cy->updateMovingEdge(I, m_cMo);
1699  if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1700  cy->Reinit = true;
1701  }
1702  }
1703  }
1704 
1705  vpMbtDistanceCircle *ci;
1706  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1707  it != circles[scaleLevel].end(); ++it) {
1708  if ((*it)->isTracked()) {
1709  ci = *it;
1710  ci->updateMovingEdge(I, m_cMo);
1711  if (ci->nbFeature == 0 && ci->isVisible()) {
1712  ci->Reinit = true;
1713  }
1714  }
1715  }
1716 }
1717 
1719 {
1720  unsigned int n = 0;
1721 
1722  vpMbtDistanceLine *l;
1723  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1724  ++it) {
1725  if ((*it)->isTracked()) {
1726  l = *it;
1727  unsigned int indexLine = 0;
1728  double wmean = 0;
1729  for (size_t a = 0; a < l->meline.size(); a++) {
1730  if (l->nbFeature[a] > 0) {
1731  std::list<vpMeSite>::iterator itListLine;
1732  itListLine = l->meline[a]->getMeList().begin();
1733 
1734  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1735  wmean += m_w_edge[n + indexLine];
1736  vpMeSite p = *itListLine;
1737  if (m_w_edge[n + indexLine] < 0.5) {
1739 
1740  *itListLine = p;
1741  }
1742 
1743  ++itListLine;
1744  indexLine++;
1745  }
1746  }
1747  }
1748  n += l->nbFeatureTotal;
1749 
1750  if (l->nbFeatureTotal != 0)
1751  wmean /= l->nbFeatureTotal;
1752  else
1753  wmean = 1;
1754 
1755  l->setMeanWeight(wmean);
1756 
1757  if (wmean < 0.8)
1758  l->Reinit = true;
1759  }
1760  }
1761 
1762  // Same thing with cylinders as with lines
1764  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1765  it != cylinders[scaleLevel].end(); ++it) {
1766  if ((*it)->isTracked()) {
1767  cy = *it;
1768  double wmean = 0;
1769  std::list<vpMeSite>::iterator itListCyl1;
1770  std::list<vpMeSite>::iterator itListCyl2;
1771 
1772  if (cy->nbFeature > 0) {
1773  itListCyl1 = cy->meline1->getMeList().begin();
1774  itListCyl2 = cy->meline2->getMeList().begin();
1775 
1776  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1777  wmean += m_w_edge[n + i];
1778  vpMeSite p = *itListCyl1;
1779  if (m_w_edge[n + i] < 0.5) {
1781 
1782  *itListCyl1 = p;
1783  }
1784 
1785  ++itListCyl1;
1786  }
1787  }
1788 
1789  if (cy->nbFeaturel1 != 0)
1790  wmean /= cy->nbFeaturel1;
1791  else
1792  wmean = 1;
1793 
1794  cy->setMeanWeight1(wmean);
1795 
1796  if (wmean < 0.8) {
1797  cy->Reinit = true;
1798  }
1799 
1800  wmean = 0;
1801  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1802  wmean += m_w_edge[n + i];
1803  vpMeSite p = *itListCyl2;
1804  if (m_w_edge[n + i] < 0.5) {
1806 
1807  *itListCyl2 = p;
1808  }
1809 
1810  ++itListCyl2;
1811  }
1812 
1813  if (cy->nbFeaturel2 != 0)
1814  wmean /= cy->nbFeaturel2;
1815  else
1816  wmean = 1;
1817 
1818  cy->setMeanWeight2(wmean);
1819 
1820  if (wmean < 0.8) {
1821  cy->Reinit = true;
1822  }
1823 
1824  n += cy->nbFeature;
1825  }
1826  }
1827 
1828  // Same thing with circles as with lines
1829  vpMbtDistanceCircle *ci;
1830  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1831  it != circles[scaleLevel].end(); ++it) {
1832  if ((*it)->isTracked()) {
1833  ci = *it;
1834  double wmean = 0;
1835  std::list<vpMeSite>::iterator itListCir;
1836 
1837  if (ci->nbFeature > 0) {
1838  itListCir = ci->meEllipse->getMeList().begin();
1839  }
1840 
1841  wmean = 0;
1842  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1843  wmean += m_w_edge[n + i];
1844  vpMeSite p = *itListCir;
1845  if (m_w_edge[n + i] < 0.5) {
1847 
1848  *itListCir = p;
1849  }
1850 
1851  ++itListCir;
1852  }
1853 
1854  if (ci->nbFeature != 0)
1855  wmean /= ci->nbFeature;
1856  else
1857  wmean = 1;
1858 
1859  ci->setMeanWeight(wmean);
1860 
1861  if (wmean < 0.8) {
1862  ci->Reinit = true;
1863  }
1864 
1865  n += ci->nbFeature;
1866  }
1867  }
1868 }
1869 
1880 {
1881  vpMbtDistanceLine *l;
1882  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1883  ++it) {
1884  if ((*it)->isTracked()) {
1885  l = *it;
1886  if (l->Reinit && l->isVisible())
1887  l->reinitMovingEdge(I, _cMo, m_mask);
1888  }
1889  }
1890 
1892  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1893  it != cylinders[scaleLevel].end(); ++it) {
1894  if ((*it)->isTracked()) {
1895  cy = *it;
1896  if (cy->Reinit && cy->isVisible())
1897  cy->reinitMovingEdge(I, _cMo, m_mask);
1898  }
1899  }
1900 
1901  vpMbtDistanceCircle *ci;
1902  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1903  it != circles[scaleLevel].end(); ++it) {
1904  if ((*it)->isTracked()) {
1905  ci = *it;
1906  if (ci->Reinit && ci->isVisible())
1907  ci->reinitMovingEdge(I, _cMo, m_mask);
1908  }
1909  }
1910 }
1911 
1913 {
1914  // Clear ME to be displayed
1916 
1917  for (unsigned int i = 0; i < scales.size(); i += 1) {
1918  if (scales[i]) {
1919  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1920  for (size_t a = 0; a < (*it)->meline.size(); a++) {
1921  if ((*it)->meline[a] != NULL) {
1922  delete (*it)->meline[a];
1923  (*it)->meline[a] = NULL;
1924  }
1925  }
1926 
1927  (*it)->meline.clear();
1928  (*it)->nbFeature.clear();
1929  (*it)->nbFeatureTotal = 0;
1930  }
1931 
1932  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1933  ++it) {
1934  if ((*it)->meline1 != NULL) {
1935  delete (*it)->meline1;
1936  (*it)->meline1 = NULL;
1937  }
1938  if ((*it)->meline2 != NULL) {
1939  delete (*it)->meline2;
1940  (*it)->meline2 = NULL;
1941  }
1942 
1943  (*it)->nbFeature = 0;
1944  (*it)->nbFeaturel1 = 0;
1945  (*it)->nbFeaturel2 = 0;
1946  }
1947 
1948  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1949  if ((*it)->meEllipse != NULL) {
1950  delete (*it)->meEllipse;
1951  (*it)->meEllipse = NULL;
1952  }
1953  (*it)->nbFeature = 0;
1954  }
1955  }
1956  }
1957 }
1958 
1971 void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1972 {
1973  {
1974  // suppress line already in the model
1975  bool already_here = false;
1976  vpMbtDistanceLine *l;
1977 
1978  for (unsigned int i = 0; i < scales.size(); i += 1) {
1979  if (scales[i]) {
1980  downScale(i);
1981  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1982  l = *it;
1983  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
1984  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
1985  already_here = true;
1986  l->addPolygon(polygon);
1987  l->hiddenface = &faces;
1988  }
1989  }
1990 
1991  if (!already_here) {
1992  l = new vpMbtDistanceLine;
1993 
1995  l->buildFrom(P1, P2, m_rand);
1996  l->addPolygon(polygon);
1997  l->setMovingEdge(&me);
1998  l->hiddenface = &faces;
1999  l->useScanLine = useScanLine;
2000 
2001  l->setIndex(nline);
2002  l->setName(name);
2003 
2006 
2007  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
2009 
2010  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
2012 
2013  nline += 1;
2014  lines[i].push_back(l);
2015  }
2016  upScale(i);
2017  }
2018  }
2019  }
2020 }
2021 
2027 void vpMbEdgeTracker::removeLine(const std::string &name)
2028 {
2029  vpMbtDistanceLine *l;
2030 
2031  for (unsigned int i = 0; i < scales.size(); i++) {
2032  if (scales[i]) {
2033  for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2034  l = *it;
2035  if (name.compare(l->getName()) == 0) {
2036  lines[i].erase(it);
2037  break;
2038  }
2039  }
2040  }
2041  }
2042 }
2043 
2054 void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
2055  const std::string &name)
2056 {
2057  {
2058  bool already_here = false;
2059  vpMbtDistanceCircle *ci;
2060 
2061  for (unsigned int i = 0; i < scales.size(); i += 1) {
2062  if (scales[i]) {
2063  downScale(i);
2064  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2065  ci = *it;
2066  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
2067  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
2068  already_here =
2069  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
2070  }
2071  }
2072 
2073  if (!already_here) {
2074  ci = new vpMbtDistanceCircle;
2075 
2077  ci->buildFrom(P1, P2, P3, r);
2078  ci->setMovingEdge(&me);
2079  ci->setIndex(ncircle);
2080  ci->setName(name);
2081  ci->index_polygon = idFace;
2082  ci->hiddenface = &faces;
2083 
2084  // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
2085  // ci->getPolygon().setClipping(clippingFlag);
2086 
2087  // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
2088  // vpPolygon3D::NEAR_CLIPPING)
2089  // ci->getPolygon().setNearClippingDistance(distNearClip);
2090 
2091  // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
2092  // vpPolygon3D::FAR_CLIPPING)
2093  // ci->getPolygon().setFarClippingDistance(distFarClip);
2094 
2095  ncircle += 1;
2096  circles[i].push_back(ci);
2097  }
2098  upScale(i);
2099  }
2100  }
2101  }
2102 }
2103 
2113 void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace, const std::string &name)
2114 {
2115  {
2116  bool already_here = false;
2118 
2119  for (unsigned int i = 0; i < scales.size(); i += 1) {
2120  if (scales[i]) {
2121  downScale(i);
2122  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2123  ++it) {
2124  cy = *it;
2125  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
2126  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
2127  already_here =
2128  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
2129  }
2130  }
2131 
2132  if (!already_here) {
2133  cy = new vpMbtDistanceCylinder;
2134 
2136  cy->buildFrom(P1, P2, r);
2137  cy->setMovingEdge(&me);
2138  cy->setIndex(ncylinder);
2139  cy->setName(name);
2140  cy->index_polygon = idFace;
2141  cy->hiddenface = &faces;
2142  ncylinder += 1;
2143  cylinders[i].push_back(cy);
2144  }
2145  upScale(i);
2146  }
2147  }
2148  }
2149 }
2150 
2156 void vpMbEdgeTracker::removeCylinder(const std::string &name)
2157 {
2159 
2160  for (unsigned int i = 0; i < scales.size(); i++) {
2161  if (scales[i]) {
2162  for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
2163  cy = *it;
2164  if (name.compare(cy->getName()) == 0) {
2165  cylinders[i].erase(it);
2166  break;
2167  }
2168  }
2169  }
2170  }
2171 }
2172 
2178 void vpMbEdgeTracker::removeCircle(const std::string &name)
2179 {
2180  vpMbtDistanceCircle *ci;
2181 
2182  for (unsigned int i = 0; i < scales.size(); i++) {
2183  if (scales[i]) {
2184  for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2185  ci = *it;
2186  if (name.compare(ci->getName()) == 0) {
2187  circles[i].erase(it);
2188  break;
2189  }
2190  }
2191  }
2192  }
2193 }
2194 
2201 {
2202  unsigned int nbpt = p.getNbPoint();
2203  if (nbpt > 0) {
2204  for (unsigned int i = 0; i < nbpt - 1; i++)
2205  addLine(p.p[i], p.p[i + 1], p.getIndex());
2206  addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
2207  }
2208 }
2209 
2221 void vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, 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, int idFace,
2352  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, const vpHomogeneousMatrix &T)
2453 {
2454  m_cMo.eye();
2455  vpMbtDistanceLine *l;
2457  vpMbtDistanceCircle *ci;
2458 
2459  for (unsigned int i = 0; i < scales.size(); i += 1) {
2460  if (scales[i]) {
2461  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2462  l = *it;
2463  if (l != NULL)
2464  delete l;
2465  l = NULL;
2466  }
2467 
2468  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2469  ++it) {
2470  cy = *it;
2471  if (cy != NULL)
2472  delete cy;
2473  cy = NULL;
2474  }
2475 
2476  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2477  ci = *it;
2478  if (ci != NULL)
2479  delete ci;
2480  ci = NULL;
2481  }
2482 
2483  lines[i].clear();
2484  cylinders[i].clear();
2485  circles[i].clear();
2486  }
2487  }
2488 
2489  faces.reset();
2490 
2491  // compute_interaction=1;
2492  nline = 0;
2493  ncylinder = 0;
2494  ncircle = 0;
2495  // lambda = 1;
2496  nbvisiblepolygone = 0;
2497 
2498  loadModel(cad_name, verbose, T);
2499  initFromPose(I, cMo);
2500 }
2501 
2512 unsigned int vpMbEdgeTracker::getNbPoints(unsigned int level) const
2513 {
2514  if ((level > scales.size()) || !scales[level]) {
2515  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2516  level);
2517  }
2518 
2519  unsigned int nbGoodPoints = 0;
2520  vpMbtDistanceLine *l;
2521  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2522  l = *it;
2523  if (l->isVisible() && l->isTracked()) {
2524  for (size_t a = 0; a < l->meline.size(); a++) {
2525  if (l->nbFeature[a] != 0)
2526  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2527  itme != l->meline[a]->getMeList().end(); ++itme) {
2528  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2529  nbGoodPoints++;
2530  }
2531  }
2532  }
2533  }
2534 
2536  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2537  ++it) {
2538  cy = *it;
2539  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
2540  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2541  itme1 != cy->meline1->getMeList().end(); ++itme1) {
2542  if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2543  nbGoodPoints++;
2544  }
2545  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2546  itme2 != cy->meline2->getMeList().end(); ++itme2) {
2547  if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2548  nbGoodPoints++;
2549  }
2550  }
2551  }
2552 
2553  vpMbtDistanceCircle *ci;
2554  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2555  ci = *it;
2556  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
2557  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2558  itme != ci->meEllipse->getMeList().end(); ++itme) {
2559  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2560  nbGoodPoints++;
2561  }
2562  }
2563  }
2564 
2565  return nbGoodPoints;
2566 }
2567 
2589 void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2590 {
2591  unsigned int nbActivatedLevels = 0;
2592  for (unsigned int i = 0; i < scale.size(); i++) {
2593  if (scale[i]) {
2594  nbActivatedLevels++;
2595  }
2596  }
2597 
2598  if (scale.empty() || (nbActivatedLevels == 0)) {
2599  vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2600  "tracking. Use the global one");
2601  this->scales.resize(0);
2602  this->scales.push_back(true);
2603 
2604  lines.resize(1);
2605  lines[0].clear();
2606 
2607  cylinders.resize(1);
2608  cylinders[0].clear();
2609 
2610  circles.resize(1);
2611  circles[0].clear();
2612  } else {
2613  this->scales = scale;
2614 
2615  lines.resize(scale.size());
2616  cylinders.resize(scale.size());
2617  circles.resize(scale.size());
2618 
2619  for (unsigned int i = 0; i < lines.size(); i++) {
2620  lines[i].clear();
2621  cylinders[i].clear();
2622  circles[i].clear();
2623  }
2624  }
2625 }
2626 
2633 {
2634  if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
2635  std::cerr << "Far clipping value cannot be inferior than near clipping "
2636  "value. Far clipping won't be considered."
2637  << std::endl;
2638  else if (dist < 0)
2639  std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2640  "won't be considered."
2641  << std::endl;
2642  else {
2644  vpMbtDistanceLine *l;
2645 
2646  for (unsigned int i = 0; i < scales.size(); i += 1) {
2647  if (scales[i]) {
2648  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2649  l = *it;
2651  }
2652  }
2653  }
2654  }
2655 }
2656 
2663 {
2664  if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
2665  std::cerr << "Near clipping value cannot be superior than far clipping "
2666  "value. Near clipping won't be considered."
2667  << std::endl;
2668  else if (dist < 0)
2669  std::cerr << "Near clipping value cannot be inferior than 0. Near "
2670  "clipping won't be considered."
2671  << std::endl;
2672  else {
2674  vpMbtDistanceLine *l;
2675 
2676  for (unsigned int i = 0; i < scales.size(); i += 1) {
2677  if (scales[i]) {
2678  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2679  l = *it;
2681  }
2682  }
2683  }
2684  }
2685 }
2686 
2694 void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2695 {
2696  vpMbTracker::setClipping(flags);
2697 
2698  vpMbtDistanceLine *l;
2699 
2700  for (unsigned int i = 0; i < scales.size(); i += 1) {
2701  if (scales[i]) {
2702  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2703  l = *it;
2705  }
2706  }
2707  }
2708 }
2709 
2726  std::vector<const vpImage<unsigned char> *> &_pyramid)
2727 {
2728  _pyramid.resize(scales.size());
2729 
2730  if (scales[0]) {
2731  _pyramid[0] = &_I;
2732  } else {
2733  _pyramid[0] = NULL;
2734  }
2735 
2736  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2737  if (scales[i]) {
2738  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2739  vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2740 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2741  IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
2742  vpI0->imageData = (char *)(_I.bitmap);
2743  IplImage *vpI =
2744  cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
2745  cvResize(vpI0, vpI, CV_INTER_NN);
2746  vpImageConvert::convert(vpI, *I);
2747  cvReleaseImage(&vpI);
2748  vpI0->imageData = NULL;
2749  cvReleaseImageHeader(&vpI0);
2750 #else
2751  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2752  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2753  (*I)[k][l] = _I[ii][jj];
2754  }
2755  }
2756 #endif
2757  _pyramid[i] = I;
2758  } else {
2759  _pyramid[i] = NULL;
2760  }
2761  }
2762 }
2763 
2770 void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2771 {
2772  if (_pyramid.size() > 0) {
2773  _pyramid[0] = NULL;
2774  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2775  if (_pyramid[i] != NULL) {
2776  delete _pyramid[i];
2777  _pyramid[i] = NULL;
2778  }
2779  }
2780  _pyramid.resize(0);
2781  }
2782 }
2783 
2794 void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level) const
2795 {
2796  if (level > scales.size() || !scales[level]) {
2797  std::ostringstream oss;
2798  oss << level;
2799  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2800  throw vpException(vpException::dimensionError, errorMsg);
2801  }
2802 
2803  linesList = lines[level];
2804 }
2805 
2816 void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level) const
2817 {
2818  if (level > scales.size() || !scales[level]) {
2819  std::ostringstream oss;
2820  oss << level;
2821  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2822  throw vpException(vpException::dimensionError, errorMsg);
2823  }
2824 
2825  cylindersList = cylinders[level];
2826 }
2827 
2838 void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level) const
2839 {
2840  if (level > scales.size() || !scales[level]) {
2841  std::ostringstream oss;
2842  oss << level;
2843  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2844  throw vpException(vpException::dimensionError, errorMsg);
2845  }
2846 
2847  circlesList = circles[level];
2848 }
2849 
2856 void vpMbEdgeTracker::downScale(const unsigned int _scale)
2857 {
2858  const double ratio = pow(2., (int)_scale);
2859  scaleLevel = _scale;
2860 
2861  vpMatrix K = m_cam.get_K();
2862 
2863  K[0][0] /= ratio;
2864  K[1][1] /= ratio;
2865  K[0][2] /= ratio;
2866  K[1][2] /= ratio;
2867 
2869 }
2870 
2877 void vpMbEdgeTracker::upScale(const unsigned int _scale)
2878 {
2879  const double ratio = pow(2., (int)_scale);
2880  scaleLevel = 0;
2881 
2882  vpMatrix K = m_cam.get_K();
2883 
2884  K[0][0] *= ratio;
2885  K[1][1] *= ratio;
2886  K[0][2] *= ratio;
2887  K[1][2] *= ratio;
2888 
2890 }
2891 
2899 void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2900 {
2901  unsigned int scaleLevel_1 = scaleLevel;
2902  scaleLevel = _lvl;
2903 
2904  vpMbtDistanceLine *l;
2905  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2906  ++it) {
2907  if ((*it)->isTracked()) {
2908  l = *it;
2909  l->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2910  }
2911  }
2912 
2914  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2915  it != cylinders[scaleLevel].end(); ++it) {
2916  if ((*it)->isTracked()) {
2917  cy = *it;
2918  cy->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2919  }
2920  }
2921 
2922  vpMbtDistanceCircle *ci;
2923  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2924  it != circles[scaleLevel].end(); ++it) {
2925  if ((*it)->isTracked()) {
2926  ci = *it;
2927  ci->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2928  }
2929  }
2930 
2931  trackMovingEdge(*Ipyramid[_lvl]);
2932  updateMovingEdge(*Ipyramid[_lvl]);
2933  scaleLevel = scaleLevel_1;
2934 }
2935 
2943 void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2944 {
2945  for (unsigned int i = 0; i < scales.size(); i += 1) {
2946  if (scales[i]) {
2947  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2948  /*(*it)->setTracked(useEdgeTracking);
2949  for(std::list<int>::const_iterator
2950  itpoly=(*it)->Lindex_polygon.begin();
2951  itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2952  if(faces[(*itpoly)]->getName() != name){
2953  (*it)->setTracked(true);
2954  break;
2955  }
2956  }*/
2957 
2958  (*it)->setTracked(name, useEdgeTracking);
2959  }
2960 
2961  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2962  ++it) {
2963  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2964  (*it)->setTracked(useEdgeTracking);
2965  }
2966  }
2967 
2968  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2969  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2970  (*it)->setTracked(useEdgeTracking);
2971  }
2972  }
2973  }
2974  }
2975 }
void computeVVSFirstPhase(const vpImage< unsigned char > &I, unsigned int iter, double &count, unsigned int lvl=0)
bool m_computeInteraction
Definition: vpMbTracker.h:185
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
bool computeProjError
Definition: vpMbTracker.h:133
unsigned int ncylinder
std::string getName() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
void initFromCalibrationMatrix(const vpMatrix &_K)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setMovingEdge(const vpMe &me)
void trackMovingEdge(const vpImage< unsigned char > &I)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
vpMeSiteState getState() const
Definition: vpMeSite.h:190
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int nbFeature
The number of moving edges.
unsigned int nbFeatureTotal
The number of moving edges.
void setVisible(bool _isvisible)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
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 isVisible(unsigned int i)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setCameraParameters(const vpCameraParameters &camera)
vpColVector m_error_edge
(s - s*)
void upScale(const unsigned int _scale)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpColVector m_weightedError_edge
Weighted error.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
int getIndex() const
Definition: vpMbtPolygon.h:101
vpPoint * p3
An other point on the plane containing the circle.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
void setIndex(unsigned int i)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
std::string getName() const
void parse(const std::string &filename)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool Reinit
Indicates if the line has to be reinitialized.
void getCameraParameters(vpCameraParameters &cam) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
virtual void init(const vpImage< unsigned char > &I)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
virtual void track(const vpImage< unsigned char > &I)
std::string getName() const
Definition: vpMbtPolygon.h:108
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
unsigned int scaleLevel
std::list< int > Lindex_polygon
Index of the faces which contain the line.
Point removed because too near image borders.
Definition: vpMeSite.h:82
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
vpMatrix L
The interaction matrix.
bool isTracked() const
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
void setCameraParameters(const vpCameraParameters &camera)
vpMatrix AtA() const
Definition: vpMatrix.cpp:626
Performs search in a given direction(normal) for a given distance(pixels) for a given &#39;site&#39;...
Definition: vpMeSite.h:71
#define vpERROR_TRACE
Definition: vpDebug.h:393
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:367
Point removed due to a threshold problem.
Definition: vpMeSite.h:80
vpColVector m_factor
Edge VVS variables.
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
vpPoint * p1
The first extremity.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
virtual ~vpMbEdgeTracker()
unsigned int ncircle
std::vector< const vpImage< unsigned char > * > Ipyramid
vpColVector error
The error vector.
bool modelInitialised
Definition: vpMbTracker.h:123
vpColVector m_w_edge
Robust weights.
vpColVector m_wCircles
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void reInitLevel(const unsigned int _lvl)
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
unsigned int getRows() const
Definition: vpArray2D.h:291
Definition: vpMe.h:60
vpHomogeneousMatrix inverse() const
Manage the line of a polygon used in the model-based tracker.
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
unsigned int nbFeature
The number of moving edges.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void removeCircle(const std::string &name)
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
int index_polygon
Index of the faces which contain the line.
vpPoint * p2
A point on the plane containing the circle.
vpMatrix get_K() const
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
void setEdgeMe(const vpMe &ecm)
void removeCylinder(const std::string &name)
vpMbtPolygon & getPolygon()
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
static const vpColor green
Definition: vpColor.h:220
vpMe me
The moving edges parameters.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
void updateMovingEdge(const vpImage< unsigned char > &I)
vpPoint * p1
The first extremity on the axe.
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
virtual void setLod(bool useLod, const std::string &name="")
void downScale(const unsigned int _scale)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_)
static const vpColor red
Definition: vpColor.h:217
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
vpMatrix L
The interaction matrix.
virtual void computeVVSWeights()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:171
virtual void setNearClippingDistance(const double &dist)
void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
Parse an Xml file to extract configuration parameters of a mbtConfig object.Data parser for the model...
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int nbFeaturel1
The number of moving edges on line 1.
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
double projectionError
Definition: vpMbTracker.h:136
vpAROgre * getOgreContext()
int index_polygon
Index of the face which contains the cylinder.
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
Manage a circle used in the model-based tracker.
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
bool Reinit
Indicates if the circle has to be reinitialized.
Error that can be emited by the vpTracker class and its derivates.
void getLline(std::list< vpMbtDistanceLine *> &linesList, unsigned int level=0) const
std::vector< std::vector< double > > getFeaturesForDisplay()
Point used by the tracker.
Definition: vpMeSite.h:78
static const vpColor cyan
Definition: vpColor.h:226
void setMeanWeight(double w_mean)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:66
void setAngleDisappear(const double &adisappear)
bool isAppearing(unsigned int i)
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setName(const std::string &circle_name)
vpPoint * p2
The second extremity.
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
static double sqr(double x)
Definition: vpMath.h:123
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
Point removed due to a contrast problem.
Definition: vpMeSite.h:79
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
virtual void testTracking()
void setMeanWeight1(double wmean)
vpColVector error
The error vector.
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
Generic class defining intrinsic camera parameters.
std::vector< std::vector< double > > getFeaturesForDisplay()
void cleanPyramid(std::vector< const vpImage< unsigned char > *> &_pyramid)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void setOgreShowConfigDialog(bool showConfigDialog)
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
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)
std::string getName() const
void setMeanWeight(double _wmean)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
virtual void computeVVSInteractionMatrixAndResidu()
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
void setVisible(bool _isvisible)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
void setMeanWeight2(double wmean)
virtual void setFarClippingDistance(const double &dist)
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:179
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
vpColVector m_errorCylinders
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpColVector m_errorCircles
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
vpColVector m_wCylinders
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
vpRobust m_robustLines
void setAngleAppear(const double &aappear)
static double rad(double deg)
Definition: vpMath.h:117
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
void computeProjectionError(const vpImage< unsigned char > &_I)
void setVisible(bool _isvisible)
void insert(unsigned int i, const vpColVector &v)
vpRobust m_robustCylinders
virtual unsigned int getNbPoints(unsigned int level=0) const
virtual void initFaceFromLines(vpMbtPolygon &polygon)
vpRobust m_robustCircles
void setName(const std::string &line_name)
void setScales(const std::vector< bool > &_scales)
vpPoint * p2
The second extremity on the axe.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
void setCameraParameters(const vpCameraParameters &camera)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void getEdgeMe(vpMe &ecm) const
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)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
bool isVisible() const
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
static double deg(double rad)
Definition: vpMath.h:110
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
unsigned int getHeight() const
Definition: vpImage.h:188
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
void setIndex(unsigned int i)
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void setMovingEdge(vpMe *Me)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
double getLodMinLineLengthThreshold() const
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
void getLcircle(std::list< vpMbtDistanceCircle *> &circlesList, unsigned int level=0) const
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
static vpHomogeneousMatrix direct(const vpColVector &v)
std::vector< std::vector< double > > getFeaturesForDisplay()
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
void computeVVS(const vpImage< unsigned char > &_I, unsigned int lvl)
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
Tukey influence function.
Definition: vpRobust.h:93
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
virtual void computeVVSInit()
void getLcylinder(std::list< vpMbtDistanceCylinder *> &cylindersList, unsigned int level=0) const
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
void addPolygon(const int &index)
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
vpColVector error
The error vector.
std::vector< std::list< vpMbtDistanceLine * > > lines
vpColVector m_wLines
virtual void setClipping(const unsigned int &flags)
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
Definition: vpMbTracker.h:227
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
bool Reinit
Indicates if the line has to be reinitialized.
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double radius
The radius of the cylinder.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
std::vector< vpMbtMeLine * > meline
The moving edge container.
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
vpMatrix m_L_edge
Interaction matrix.
unsigned int nline
void addPolygon(vpMbtPolygon &p)
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > *> &_pyramid)
void trackMovingEdge(const vpImage< unsigned char > &I)
void displayOgre(const vpHomogeneousMatrix &cMo)
virtual void setClipping(const unsigned int &flags)
unsigned int nbFeaturel2
The number of moving edges on line 2.
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
double radius
The radius of the circle.
virtual void setFarClippingDistance(const double &dist)
void setName(const std::string &cyl_name)
vpColVector m_errorLines
static const vpColor yellow
Definition: vpColor.h:225
void setCameraParameters(const vpCameraParameters &cam)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void removeLine(const std::string &name)
static const vpColor purple
Definition: vpColor.h:228
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int getWidth() const
Definition: vpImage.h:246
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
static int() sign(double x)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpMatrix L
The interaction matrix.
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
std::vector< unsigned int > nbFeature
The number of moving edges.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void eye()
Definition: vpMatrix.cpp:450
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
void displayFeaturesOnImage(const vpImage< unsigned char > &I)
vpPoint * p1
The center of the circle.
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
bool useScanLine
Use scanline rendering.
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setIndex(unsigned int i)
double getLodMinPolygonAreaThreshold() const
static const vpColor blue
Definition: vpColor.h:223
virtual void setNearClippingDistance(const double &dist)
void computeFov(const unsigned int &w, const unsigned int &h)