Visual Servoing Platform  version 3.5.1 under development (2022-09-30)
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 
2009 
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 {
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 {
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 setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:306
unsigned int getRows() const
Definition: vpArray2D.h:291
Generic class defining intrinsic camera parameters.
vpMatrix get_K() const
void computeFov(const unsigned int &w, const unsigned int &h)
void initFromCalibrationMatrix(const vpMatrix &_K)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:314
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor yellow
Definition: vpColor.h:225
static const vpColor green
Definition: vpColor.h:220
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
unsigned int getWidth() const
Definition: vpImage.h:246
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
static int() sign(double x)
static double rad(double deg)
Definition: vpMath.h:117
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:171
static double sqr(double x)
Definition: vpMath.h:123
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:364
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:179
static double deg(double rad)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void eye()
Definition: vpMatrix.cpp:450
vpMatrix AtA() const
Definition: vpMatrix.cpp:626
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2235
void removeCircle(const std::string &name)
void computeVVS(const vpImage< unsigned char > &_I, unsigned int lvl)
vpColVector m_errorCircles
vpColVector m_w_edge
Robust weights.
void upScale(const unsigned int _scale)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void setNearClippingDistance(const double &dist)
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
virtual void setFarClippingDistance(const double &dist)
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMe me
The moving edges parameters.
void displayFeaturesOnImage(const vpImage< unsigned char > &I)
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, unsigned int level=0) const
vpColVector m_wLines
void computeProjectionError(const vpImage< unsigned char > &_I)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void track(const vpImage< unsigned char > &I)
virtual void computeVVSWeights()
vpColVector m_error_edge
(s - s*)
unsigned int ncylinder
void downScale(const unsigned int _scale)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
void removeCylinder(const std::string &name)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void computeVVSFirstPhase(const vpImage< unsigned char > &I, unsigned int iter, double &count, unsigned int lvl=0)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace=-1, const std::string &name="")
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
std::vector< std::vector< double > > m_featuresToBeDisplayedEdge
Display features.
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
void addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace=-1, const std::string &name="")
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity_)
virtual void setClipping(const unsigned int &flags)
vpRobust m_robustCircles
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, unsigned int level=0) const
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int scaleLevel
void removeLine(const std::string &name)
virtual std::vector< std::vector< double > > getFeaturesForDisplayEdge()
void setScales(const std::vector< bool > &_scales)
void trackMovingEdge(const vpImage< unsigned char > &I)
std::vector< const vpImage< unsigned char > * > Ipyramid
virtual unsigned int getNbPoints(unsigned int level=0) const
vpColVector m_weightedError_edge
Weighted error.
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace=0, const std::string &name="")
unsigned int ncircle
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace=0, const std::string &name="")
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, unsigned int lvl=0)
void updateMovingEdge(const vpImage< unsigned char > &I)
vpMatrix m_L_edge
Interaction matrix.
vpRobust m_robustCylinders
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
void reInitLevel(const unsigned int _lvl)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
vpRobust m_robustLines
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
void setMovingEdge(const vpMe &me)
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
void getLline(std::list< vpMbtDistanceLine * > &linesList, unsigned int level=0) const
vpColVector m_wCylinders
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
vpColVector m_factor
Edge VVS variables.
vpColVector m_errorLines
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
vpColVector m_wCircles
void addPolygon(vpMbtPolygon &p)
virtual ~vpMbEdgeTracker()
virtual void init(const vpImage< unsigned char > &I)
unsigned int nline
virtual void testTracking()
vpColVector m_errorCylinders
vpAROgre * getOgreContext()
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool isAppearing(unsigned int i)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
bool isVisible(unsigned int i)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
void displayOgre(const vpHomogeneousMatrix &cMo)
void setOgreShowConfigDialog(bool showConfigDialog)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
bool modelInitialised
Definition: vpMbTracker.h:123
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:213
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:215
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
Definition: vpMbTracker.h:227
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:130
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
bool computeProjError
Definition: vpMbTracker.h:133
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:209
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
double projectionError
Definition: vpMbTracker.h:136
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
virtual void setLod(bool useLod, const std::string &name="")
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:217
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:151
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:211
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:149
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setVisible(bool _isvisible)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getFeaturesForDisplay()
void setCameraParameters(const vpCameraParameters &camera)
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
vpPoint * p1
The center of the circle.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void setIndex(unsigned int i)
std::string getName() const
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
vpPoint * p2
A point on the plane containing the circle.
void setMeanWeight(double _wmean)
bool Reinit
Indicates if the circle has to be reinitialized.
double radius
The radius of the circle.
int index_polygon
Index of the faces which contain the line.
vpPoint * p3
An other point on the plane containing the circle.
vpMbtMeEllipse * meEllipse
The moving edge containers.
void setName(const std::string &circle_name)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
Manage a cylinder used in the model-based tracker.
void setMeanWeight1(double wmean)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, double r)
void setCameraParameters(const vpCameraParameters &camera)
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
void setName(const std::string &cyl_name)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
void setVisible(bool _isvisible)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
vpMatrix L
The interaction matrix.
unsigned int nbFeaturel2
The number of moving edges on line 2.
bool Reinit
Indicates if the line has to be reinitialized.
vpPoint * p2
The second extremity on the axe.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void setMeanWeight2(double wmean)
double radius
The radius of the cylinder.
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
std::string getName() const
std::vector< std::vector< double > > getFeaturesForDisplay()
unsigned int nbFeature
The number of moving edges.
int index_polygon
Index of the face which contains the cylinder.
void setIndex(unsigned int i)
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
vpPoint * p1
The first extremity on the axe.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Manage the line of a polygon used in the model-based tracker.
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
void setIndex(unsigned int i)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpPoint * p2
The second extremity.
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
std::string getName() const
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getFeaturesForDisplay()
bool isTracked() const
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setCameraParameters(const vpCameraParameters &camera)
void setName(const std::string &line_name)
void setMeanWeight(double w_mean)
void setVisible(bool _isvisible)
void addPolygon(const int &index)
void trackMovingEdge(const vpImage< unsigned char > &I)
vpMbtPolygon & getPolygon()
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
std::string getName() const
Definition: vpMbtPolygon.h:108
int getIndex() const
Definition: vpMbtPolygon.h:101
Parse an Xml file to extract configuration parameters of a mbtConfig object.
void getCameraParameters(vpCameraParameters &cam) const
void setEdgeMe(const vpMe &ecm)
void getEdgeMe(vpMe &ecm) const
double getLodMinLineLengthThreshold() const
void setAngleDisappear(const double &adisappear)
void setAngleAppear(const double &aappear)
void parse(const std::string &filename)
void setCameraParameters(const vpCameraParameters &cam)
double getLodMinPolygonAreaThreshold() const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
@ CONSTRAST
Point removed due to a contrast problem.
Definition: vpMeSite.h:79
@ TOO_NEAR
Point removed because too near image borders.
Definition: vpMeSite.h:82
@ THRESHOLD
Point removed due to a threshold problem.
Definition: vpMeSite.h:80
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition: vpMeSite.h:81
@ NO_SUPPRESSION
Point used by the tracker.
Definition: vpMeSite.h:78
vpMeSiteState getState() const
Definition: vpMeSite.h:190
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:176
Definition: vpMe.h:61
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:194
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:207
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:187
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393