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