Visual Servoing Platform  version 3.2.0
vpMbEdgeTracker.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Make the complete tracking of an object by using its CAD model
33  *
34  * Authors:
35  * Nicolas Melchior
36  * Romain Tallonneau
37  * Eric Marchand
38  *
39  *****************************************************************************/
40 
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpExponentialMap.h>
49 #include <visp3/core/vpMath.h>
50 #include <visp3/core/vpMatrixException.h>
51 #include <visp3/core/vpPixelMeterConversion.h>
52 #include <visp3/core/vpPolygon3D.h>
53 #include <visp3/core/vpTrackingException.h>
54 #include <visp3/core/vpVelocityTwistMatrix.h>
55 #include <visp3/mbt/vpMbEdgeTracker.h>
56 #include <visp3/mbt/vpMbtDistanceLine.h>
57 #include <visp3/mbt/vpMbtXmlParser.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()
75 {
78 
79  scales[0] = true;
80 
81 #ifdef VISP_HAVE_OGRE
82  faces.getOgreContext()->setWindowName("MBT Edge");
83 #endif
84 }
85 
90 {
94 
95  for (unsigned int i = 0; i < scales.size(); i += 1) {
96  if (scales[i]) {
97  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
98  l = *it;
99  if (l != NULL) {
100  delete l;
101  }
102  l = NULL;
103  }
104 
105  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
106  ++it) {
107  cy = *it;
108  if (cy != NULL) {
109  delete cy;
110  }
111  cy = NULL;
112  }
113 
114  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
115  ci = *it;
116  if (ci != NULL) {
117  delete ci;
118  }
119  ci = NULL;
120  }
121 
122  lines[i].clear();
123  cylinders[i].clear();
124  circles[i].clear();
125  }
126  }
127 
129 }
130 
136 void vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
137 {
138  this->me = p_me;
139 
140  for (unsigned int i = 0; i < scales.size(); i += 1) {
141  if (scales[i]) {
142  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
143  vpMbtDistanceLine *l = *it;
144  l->setMovingEdge(&(this->me));
145  }
146 
147  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
148  ++it) {
149  vpMbtDistanceCylinder *cy = *it;
150  cy->setMovingEdge(&(this->me));
151  }
152 
153  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
154  vpMbtDistanceCircle *ci = *it;
155  ci->setMovingEdge(&(this->me));
156  }
157  }
158  }
159 }
160 
170 void vpMbEdgeTracker::computeVVS(const vpImage<unsigned char> &_I, const unsigned int lvl)
171 {
172  double residu_1 = 1e3;
173  double r = 1e3 - 1;
174 
175  unsigned int iter = 0;
176 
177  computeVVSInit();
178  unsigned int nbrow = m_error_edge.getRows();
179 
180  bool reloop = true;
181 
182  bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
183  if (isoJoIdentity_)
184  oJo.eye();
185 
186  /*** First phase ***/
187 
188  while (reloop == true && iter < 10) {
189  double count = 0;
190 
191  computeVVSFirstPhase(_I, iter, count, lvl);
192 
193  count = count / (double)nbrow;
194  if (count >= 0.85) {
195  reloop = false;
196  }
197 
198  computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);
199 
200  iter++;
201  }
202 
203  // std::cout << "\t First minimization in " << iter << " iteration give as
204  // initial cMo: \n" << cMo << std::endl;
205 
206  /*** Second phase ***/
207  vpHomogeneousMatrix cMoPrev;
208  vpColVector W_true(nbrow);
209  vpMatrix L_true;
210  vpMatrix LVJ_true;
211 
212  double mu = m_initialMu;
213  vpColVector m_error_prev;
214  vpColVector m_w_prev;
215 
216  // To avoid to create these matrices each iteration
217  vpMatrix LTL;
218  vpColVector LTR;
219  vpColVector v;
220 
221  iter = 0;
222  m_w_edge = 1;
223 
224  // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
225  while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
227 
228  bool reStartFromLastIncrement = false;
229  computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
230  &m_w_edge, &m_w_prev);
231 
232  if (!reStartFromLastIncrement) {
234 
235  L_true = m_L_edge;
237 
238  if (computeCovariance) {
239  L_true = m_L_edge;
240  if (!isoJoIdentity_) {
241  cVo.buildFrom(cMo);
242  LVJ_true = (m_L_edge * cVo * oJo);
243  }
244  }
245 
246  double wi = 0.0, eri = 0.0;
247  double num = 0.0, den = 0.0;
248  if ((iter == 0) || m_computeInteraction) {
249  for (unsigned int i = 0; i < nbrow; i++) {
250  wi = m_w_edge[i] * m_factor[i];
251  W_true[i] = wi;
252  eri = m_error_edge[i];
253  num += wi * vpMath::sqr(eri);
254  den += wi;
255 
256  m_weightedError_edge[i] = wi * eri;
257 
258  for (unsigned int j = 0; j < 6; j++) {
259  m_L_edge[i][j] = wi * m_L_edge[i][j];
260  }
261  }
262  } else {
263  for (unsigned int i = 0; i < nbrow; i++) {
264  wi = m_w_edge[i] * m_factor[i];
265  W_true[i] = wi;
266  eri = m_error_edge[i];
267  num += wi * vpMath::sqr(eri);
268  den += wi;
269 
270  m_weightedError_edge[i] = wi * eri;
271  }
272  }
273 
274  residu_1 = r;
275  r = sqrt(num / den); // Le critere d'arret prend en compte le poids
276 
277  computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev,
278  LTR, mu, v, &m_w_edge, &m_w_prev);
279 
280  cMoPrev = cMo;
282 
283  } // endif(!restartFromLast)
284 
285  iter++;
286  }
287 
288  computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);
289 
291 }
292 
293 void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_I, const unsigned int iter, double &count,
294  const unsigned int lvl)
295 {
299 
300  double limite = 3; // Une limite de 3 pixels
301  limite = limite / cam.get_px(); // Transformation limite pixel en limite metre.
302 
303  unsigned int n = 0;
304 
305  // Parametre pour la premiere phase d'asservissement
306  double e_prev = 0, e_cur, e_next;
307 
308  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
309  if ((*it)->isTracked()) {
310  l = *it;
312 
313  double fac = 1;
314  if (iter == 0) {
315  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
316  ++itindex) {
317  int index = *itindex;
318  if (l->hiddenface->isAppearing((unsigned int)index)) {
319  fac = 0.2;
320  break;
321  }
322  if (l->closeToImageBorder(_I, 10)) {
323  fac = 0.1;
324  break;
325  }
326  }
327  }
328 
329  std::list<vpMeSite>::const_iterator itListLine;
330 
331  unsigned int indexFeature = 0;
332 
333  for (size_t a = 0; a < l->meline.size(); a++) {
334  if (iter == 0 && l->meline[a] != NULL)
335  itListLine = l->meline[a]->getMeList().begin();
336 
337  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
338  for (unsigned int j = 0; j < 6; j++) {
339  m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale
340  }
341  m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur
342 
343  if (m_error_edge[n + i] <= limite)
344  count = count + 1.0; // Si erreur proche de 0 on incremente cur
345 
346  m_w_edge[n + i] = 0;
347 
348  if (iter == 0) {
349  m_factor[n + i] = fac;
350  vpMeSite site = *itListLine;
351  if (site.getState() != vpMeSite::NO_SUPPRESSION)
352  m_factor[n + i] = 0.2;
353  ++itListLine;
354  }
355 
356  // If pour la premiere extremite des moving edges
357  if (indexFeature == 0) {
358  e_cur = l->error[0];
359  if (l->nbFeature[a] > 1) {
360  e_next = l->error[1];
361  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
362  m_w_edge[n + i] = 1 /*0.5*/;
363  }
364  e_prev = e_cur;
365  } else
366  m_w_edge[n + i] = 1;
367  }
368 
369  // If pour la derniere extremite des moving edges
370  else if (indexFeature == l->nbFeatureTotal - 1) {
371  e_cur = l->error[indexFeature];
372  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
373  m_w_edge[n + i] += 1 /*0.5*/;
374  }
375  }
376 
377  else {
378  e_cur = l->error[indexFeature];
379  e_next = l->error[indexFeature + 1];
380  if (fabs(e_cur - e_prev) < limite) {
381  m_w_edge[n + i] += 0.5;
382  }
383  if (fabs(e_cur - e_next) < limite) {
384  m_w_edge[n + i] += 0.5;
385  }
386  e_prev = e_cur;
387  }
388  indexFeature++;
389  }
390  n += l->nbFeature[a];
391  }
392  }
393  }
394 
395  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
396  ++it) {
397  if ((*it)->isTracked()) {
398  cy = *it;
400  double fac = 1.0;
401 
402  std::list<vpMeSite>::const_iterator itCyl1;
403  std::list<vpMeSite>::const_iterator itCyl2;
404  if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)) {
405  itCyl1 = cy->meline1->getMeList().begin();
406  itCyl2 = cy->meline2->getMeList().begin();
407  }
408 
409  for (unsigned int i = 0; i < cy->nbFeature; i++) {
410  for (unsigned int j = 0; j < 6; j++) {
411  m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale
412  }
413  m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur
414 
415  if (m_error_edge[n + i] <= limite)
416  count = count + 1.0; // Si erreur proche de 0 on incremente cur
417 
418  m_w_edge[n + i] = 0;
419 
420  if (iter == 0) {
421  m_factor[n + i] = fac;
422  vpMeSite site;
423  if (i < cy->nbFeaturel1) {
424  site = *itCyl1;
425  ++itCyl1;
426  } else {
427  site = *itCyl2;
428  ++itCyl2;
429  }
430  if (site.getState() != vpMeSite::NO_SUPPRESSION)
431  m_factor[n + i] = 0.2;
432  }
433 
434  // If pour la premiere extremite des moving edges
435  if (i == 0) {
436  e_cur = cy->error[0];
437  if (cy->nbFeature > 1) {
438  e_next = cy->error[1];
439  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
440  m_w_edge[n + i] = 1 /*0.5*/;
441  }
442  e_prev = e_cur;
443  } else
444  m_w_edge[n + i] = 1;
445  }
446  if (i == cy->nbFeaturel1) {
447  e_cur = cy->error[i];
448  if (cy->nbFeaturel2 > 1) {
449  e_next = cy->error[i + 1];
450  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
451  m_w_edge[n + i] = 1 /*0.5*/;
452  }
453  e_prev = e_cur;
454  } else
455  m_w_edge[n + i] = 1;
456  }
457 
458  // If pour la derniere extremite des moving edges
459  else if (i == cy->nbFeaturel1 - 1) {
460  e_cur = cy->error[i];
461  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
462  m_w_edge[n + i] += 1 /*0.5*/;
463  }
464  }
465  // If pour la derniere extremite des moving edges
466  else if (i == cy->nbFeature - 1) {
467  e_cur = cy->error[i];
468  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
469  m_w_edge[n + i] += 1 /*0.5*/;
470  }
471  }
472 
473  else {
474  e_cur = cy->error[i];
475  e_next = cy->error[i + 1];
476  if (fabs(e_cur - e_prev) < limite) {
477  m_w_edge[n + i] += 0.5;
478  }
479  if (fabs(e_cur - e_next) < limite) {
480  m_w_edge[n + i] += 0.5;
481  }
482  e_prev = e_cur;
483  }
484  }
485 
486  n += cy->nbFeature;
487  }
488  }
489 
490  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
491  if ((*it)->isTracked()) {
492  ci = *it;
494  double fac = 1.0;
495 
496  std::list<vpMeSite>::const_iterator itCir;
497  if (iter == 0 && (ci->meEllipse != NULL)) {
498  itCir = ci->meEllipse->getMeList().begin();
499  }
500 
501  for (unsigned int i = 0; i < ci->nbFeature; i++) {
502  for (unsigned int j = 0; j < 6; j++) {
503  m_L_edge[n + i][j] = ci->L[i][j]; // On remplit la matrice d'interaction globale
504  }
505  m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur
506 
507  if (m_error_edge[n + i] <= limite)
508  count = count + 1.0; // Si erreur proche de 0 on incremente cur
509 
510  m_w_edge[n + i] = 0;
511 
512  if (iter == 0) {
513  m_factor[n + i] = fac;
514  vpMeSite site = *itCir;
515  if (site.getState() != vpMeSite::NO_SUPPRESSION)
516  m_factor[n + i] = 0.2;
517  ++itCir;
518  }
519 
520  // If pour la premiere extremite des moving edges
521  if (i == 0) {
522  e_cur = ci->error[0];
523  if (ci->nbFeature > 1) {
524  e_next = ci->error[1];
525  if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
526  m_w_edge[n + i] = 1 /*0.5*/;
527  }
528  e_prev = e_cur;
529  } else
530  m_w_edge[n + i] = 1;
531  }
532 
533  // If pour la derniere extremite des moving edges
534  else if (i == ci->nbFeature - 1) {
535  e_cur = ci->error[i];
536  if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
537  m_w_edge[n + i] += 1 /*0.5*/;
538  }
539  }
540 
541  else {
542  e_cur = ci->error[i];
543  e_next = ci->error[i + 1];
544  if (fabs(e_cur - e_prev) < limite) {
545  m_w_edge[n + i] += 0.5;
546  }
547  if (fabs(e_cur - e_next) < limite) {
548  m_w_edge[n + i] += 0.5;
549  }
550  e_prev = e_cur;
551  }
552  }
553 
554  n += ci->nbFeature;
555  }
556  }
557 }
558 
559 void vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage<unsigned char> &I, const unsigned int lvl)
560 {
564 
565  unsigned int n = 0;
566  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
567  if ((*it)->isTracked()) {
568  l = *it;
570 
571  double fac = 1;
572  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
573  ++itindex) {
574  int index = *itindex;
575  if (l->hiddenface->isAppearing((unsigned int)index)) {
576  fac = 0.2;
577  break;
578  }
579  if (l->closeToImageBorder(I, 10)) {
580  fac = 0.1;
581  break;
582  }
583  }
584 
585  unsigned int indexFeature = 0;
586  for (size_t a = 0; a < l->meline.size(); a++) {
587  std::list<vpMeSite>::const_iterator itListLine;
588  if (l->meline[a] != NULL) {
589  itListLine = l->meline[a]->getMeList().begin();
590 
591  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
592  m_factor[n + i] = fac;
593  vpMeSite site = *itListLine;
594  if (site.getState() != vpMeSite::NO_SUPPRESSION)
595  m_factor[n + i] = 0.2;
596  ++itListLine;
597  indexFeature++;
598  }
599  n += l->nbFeature[a];
600  }
601  }
602  }
603  }
604 
605  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
606  ++it) {
607  if ((*it)->isTracked()) {
608  cy = *it;
610 
611  std::list<vpMeSite>::const_iterator itCyl1;
612  std::list<vpMeSite>::const_iterator itCyl2;
613  if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
614  itCyl1 = cy->meline1->getMeList().begin();
615  itCyl2 = cy->meline2->getMeList().begin();
616 
617  double fac = 1.0;
618  for (unsigned int i = 0; i < cy->nbFeature; i++) {
619  m_factor[n + i] = fac;
620  vpMeSite site;
621  if (i < cy->nbFeaturel1) {
622  site = *itCyl1;
623  ++itCyl1;
624  } else {
625  site = *itCyl2;
626  ++itCyl2;
627  }
628  if (site.getState() != vpMeSite::NO_SUPPRESSION)
629  m_factor[n + i] = 0.2;
630  }
631  n += cy->nbFeature;
632  }
633  }
634  }
635 
636  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
637  if ((*it)->isTracked()) {
638  ci = *it;
640 
641  std::list<vpMeSite>::const_iterator itCir;
642  if (ci->meEllipse != NULL) {
643  itCir = ci->meEllipse->getMeList().begin();
644  double fac = 1.0;
645 
646  for (unsigned int i = 0; i < ci->nbFeature; i++) {
647  m_factor[n + i] = fac;
648  vpMeSite site = *itCir;
649  if (site.getState() != vpMeSite::NO_SUPPRESSION)
650  m_factor[n + i] = 0.2;
651  ++itCir;
652  }
653  n += ci->nbFeature;
654  }
655  }
656  }
657 }
658 
659 void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
660 {
661  unsigned int nerror = m_weightedError_edge.getRows();
662 
663  double wi, eri;
664  if ((iter == 0) || m_computeInteraction) {
665  for (unsigned int i = 0; i < nerror; i++) {
666  wi = m_w_edge[i] * m_factor[i];
667  eri = m_error_edge[i];
668 
669  m_weightedError_edge[i] = wi * eri;
670 
671  for (unsigned int j = 0; j < 6; j++) {
672  m_L_edge[i][j] = wi * m_L_edge[i][j];
673  }
674  }
675  } else {
676  for (unsigned int i = 0; i < nerror; i++) {
677  wi = m_w_edge[i] * m_factor[i];
678  eri = m_error_edge[i];
679 
680  m_weightedError_edge[i] = wi * eri;
681  }
682  }
683 
685 
686  // If all the 6 dof should be estimated, we check if the interaction matrix
687  // is full rank. If not we remove automatically the dof that cannot be
688  // estimated This is particularly useful when consering circles (rank 5) and
689  // cylinders (rank 4)
690  if (isoJoIdentity_) {
691  cVo.buildFrom(cMo);
692 
693  vpMatrix K; // kernel
694  unsigned int rank = (m_L_edge * cVo).kernel(K);
695  if (rank == 0) {
696  throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
697  }
698  if (rank != 6) {
699  vpMatrix I; // Identity
700  I.eye(6);
701  oJo = I - K.AtA();
702 
703  isoJoIdentity_ = false;
704  }
705  }
706 
707  vpColVector v;
708  vpMatrix LTL;
709  vpColVector LTR;
710 
711  if (isoJoIdentity_) {
712  LTL = m_L_edge.AtA();
714  v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
715  } else {
716  cVo.buildFrom(cMo);
717  vpMatrix LVJ = (m_L_edge * cVo * oJo);
718  vpMatrix LVJTLVJ = (LVJ).AtA();
719  vpColVector LVJTR;
720  computeJTR(LVJ, m_weightedError_edge, LVJTR);
721  v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
722  v = cVo * v;
723  }
724 
726 }
727 
729 {
730  // Nombre de moving edges
731  unsigned int nbrow = 0;
732  unsigned int nberrors_lines = 0;
733  unsigned int nberrors_cylinders = 0;
734  unsigned int nberrors_circles = 0;
735 
736  nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
737 
738  if (nbrow == 0) {
740  "No data found to compute the interaction matrix...");
741  }
742 
743  m_L_edge.resize(nbrow, 6, false, false);
744  m_error_edge.resize(nbrow, false);
745 
746  m_weightedError_edge.resize(nbrow, false);
747  m_w_edge.resize(nbrow, false);
748  m_w_edge = 1;
749  m_factor.resize(nbrow, false);
750  m_factor = 1;
751 
752  m_robustLines.resize(nberrors_lines);
753  m_robustCylinders.resize(nberrors_cylinders);
754  m_robustCircles.resize(nberrors_circles);
758 
759  m_wLines.resize(nberrors_lines, false);
760  m_wLines = 1;
761  m_wCylinders.resize(nberrors_cylinders, false);
762  m_wCylinders = 1;
763  m_wCircles.resize(nberrors_circles, false);
764  m_wCircles = 1;
765 
766  m_errorLines.resize(nberrors_lines, false);
767  m_errorCylinders.resize(nberrors_cylinders, false);
768  m_errorCircles.resize(nberrors_circles, false);
769 }
770 
772 {
773  throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
774  "computeVVSInteractionMatrixAndR"
775  "esidu() should not be called!");
776 }
777 
779 {
783 
784  unsigned int n = 0;
785  unsigned int nlines = 0;
786  unsigned int ncylinders = 0;
787  unsigned int ncircles = 0;
788 
789  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
790  ++it) {
791  if ((*it)->isTracked()) {
792  l = *it;
794  for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
795  for (unsigned int j = 0; j < 6; j++) {
796  m_L_edge[n + i][j] = l->L[i][j];
797  m_error_edge[n + i] = l->error[i];
798  m_errorLines[nlines + i] = m_error_edge[n + i];
799  }
800  }
801  n += l->nbFeatureTotal;
802  nlines += l->nbFeatureTotal;
803  }
804  }
805 
806  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
807  it != cylinders[scaleLevel].end(); ++it) {
808  if ((*it)->isTracked()) {
809  cy = *it;
811  for (unsigned int i = 0; i < cy->nbFeature; i++) {
812  for (unsigned int j = 0; j < 6; j++) {
813  m_L_edge[n + i][j] = cy->L[i][j];
814  m_error_edge[n + i] = cy->error[i];
815  m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
816  }
817  }
818 
819  n += cy->nbFeature;
820  ncylinders += cy->nbFeature;
821  }
822  }
823 
824  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
825  it != circles[scaleLevel].end(); ++it) {
826  if ((*it)->isTracked()) {
827  ci = *it;
829  for (unsigned int i = 0; i < ci->nbFeature; i++) {
830  for (unsigned int j = 0; j < 6; j++) {
831  m_L_edge[n + i][j] = ci->L[i][j];
832  m_error_edge[n + i] = ci->error[i];
833  m_errorCircles[ncircles + i] = m_error_edge[n + i];
834  }
835  }
836 
837  n += ci->nbFeature;
838  ncircles += ci->nbFeature;
839  }
840  }
841 }
842 
844 {
845  unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
846  nberrors_circles = m_errorCircles.getRows();
847 
848  if (nberrors_lines > 0)
850  if (nberrors_cylinders > 0)
852  if (nberrors_circles > 0)
854 
858 }
859 
869 {
870  projectionError = 0.0;
871  unsigned int nbFeatures = 0;
872  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
873  ++it) {
874  vpMbtDistanceLine *l = *it;
875  if (l->isVisible() && l->isTracked()) {
876  for (size_t a = 0; a < l->meline.size(); a++) {
877  if (l->meline[a] != NULL) {
878  double lineNormGradient;
879  unsigned int lineNbFeatures;
880  l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
883  projectionError += lineNormGradient;
884  nbFeatures += lineNbFeatures;
885  }
886  }
887  }
888  }
889 
890  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
891  it != cylinders[scaleLevel].end(); ++it) {
892  vpMbtDistanceCylinder *cy = *it;
893  if (cy->isVisible() && cy->isTracked()) {
894  if (cy->meline1 != NULL) {
895  double cylinderNormGradient = 0;
896  unsigned int cylinderNbFeatures = 0;
897  cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
900  projectionError += cylinderNormGradient;
901  nbFeatures += cylinderNbFeatures;
902  }
903 
904  if (cy->meline2 != NULL) {
905  double cylinderNormGradient = 0;
906  unsigned int cylinderNbFeatures = 0;
907  cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
910  projectionError += cylinderNormGradient;
911  nbFeatures += cylinderNbFeatures;
912  }
913  }
914  }
915 
916  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
917  it != circles[scaleLevel].end(); ++it) {
918  vpMbtDistanceCircle *c = *it;
919  if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
920  double circleNormGradient = 0;
921  unsigned int circleNbFeatures = 0;
922  c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
925  projectionError += circleNormGradient;
926  nbFeatures += circleNbFeatures;
927  }
928  }
929 
930  if (nbFeatures > 0) {
931  projectionError = vpMath::deg(projectionError / (double)nbFeatures);
932  } else {
933  projectionError = 90.0;
934  }
935 
937  // std::cout << "Norm Gradient = " << errorGradient << std::endl;
938 }
939 
946 {
947  int nbExpectedPoint = 0;
948  int nbGoodPoint = 0;
949  int nbBadPoint = 0;
950 
951  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
952  ++it) {
953  vpMbtDistanceLine *l = *it;
954  if (l->isVisible() && l->isTracked()) {
955  for (size_t a = 0; a < l->meline.size(); a++) {
956  if (l->meline[a] != NULL) {
957  nbExpectedPoint += (int)l->meline[a]->expecteddensity;
958  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
959  itme != l->meline[a]->getMeList().end(); ++itme) {
960  vpMeSite pix = *itme;
961  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
962  nbGoodPoint++;
963  else
964  nbBadPoint++;
965  }
966  }
967  }
968  }
969  }
970 
971  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
972  it != cylinders[scaleLevel].end(); ++it) {
973  vpMbtDistanceCylinder *cy = *it;
974  if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) {
975  nbExpectedPoint += (int)cy->meline1->expecteddensity;
976  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
977  itme1 != cy->meline1->getMeList().end(); ++itme1) {
978  vpMeSite pix = *itme1;
979  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
980  nbGoodPoint++;
981  else
982  nbBadPoint++;
983  }
984  nbExpectedPoint += (int)cy->meline2->expecteddensity;
985  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
986  itme2 != cy->meline2->getMeList().end(); ++itme2) {
987  vpMeSite pix = *itme2;
988  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
989  nbGoodPoint++;
990  else
991  nbBadPoint++;
992  }
993  }
994  }
995 
996  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
997  it != circles[scaleLevel].end(); ++it) {
998  vpMbtDistanceCircle *ci = *it;
999  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
1000  nbExpectedPoint += ci->meEllipse->getExpectedDensity();
1001  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
1002  itme != ci->meEllipse->getMeList().end(); ++itme) {
1003  vpMeSite pix = *itme;
1004  if (pix.getState() == vpMeSite::NO_SUPPRESSION)
1005  nbGoodPoint++;
1006  else
1007  nbBadPoint++;
1008  }
1009  }
1010  }
1011 
1012  // Compare the number of good points with the min between the number of
1013  // expected points and number of points that are tracked
1014  int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
1015  // int nb_min = (std::min)(val1, val2);
1016  if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1017  std::ostringstream oss;
1018  oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
1019  << ". Try to reduce the threshold=" << percentageGdPt
1020  << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1022  }
1023 }
1024 
1033 {
1034  initPyramid(I, Ipyramid);
1035 
1036  // for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
1037  unsigned int lvl = (unsigned int)scales.size();
1038  do {
1039  lvl--;
1040 
1041  projectionError = 90.0;
1042 
1043  if (scales[lvl]) {
1044  vpHomogeneousMatrix cMo_1 = cMo;
1045  try {
1046  downScale(lvl);
1047 
1048  try {
1049  trackMovingEdge(*Ipyramid[lvl]);
1050  } catch (...) {
1051  vpTRACE("Error in moving edge tracking");
1052  throw;
1053  }
1054 
1055  // initialize the vector that contains the error and the matrix that
1056  // contains the interaction matrix AY: Useless as it is done in
1057  // coputeVVS()
1058  /*
1059  for(std::list<vpMbtDistanceLine*>::const_iterator
1060  it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
1061  (l->isVisible()){ l->initInteractionMatrixError();
1062  }
1063  }
1064 
1065  for(std::list<vpMbtDistanceCylinder*>::const_iterator
1066  it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
1067  if(cy->isVisible()) {
1068  cy->initInteractionMatrixError();
1069  }
1070  }
1071 
1072  for(std::list<vpMbtDistanceCircle*>::const_iterator
1073  it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
1074  (ci->isVisible()){ ci->initInteractionMatrixError();
1075  }
1076  }
1077  */
1078 
1079  try {
1080  computeVVS(*Ipyramid[lvl], lvl);
1081  } catch (...) {
1082  covarianceMatrix = -1;
1083  throw; // throw the original exception
1084  }
1085 
1086  testTracking();
1087 
1088  if (displayFeatures) {
1089  displayFeaturesOnImage(I, lvl);
1090  }
1091 
1092  // Looking for new visible face
1093  bool newvisibleface = false;
1094  visibleFace(I, cMo, newvisibleface);
1095 
1096  // cam.computeFov(I.getWidth(), I.getHeight());
1097  if (useScanLine) {
1100  }
1101 
1102  updateMovingEdge(I);
1103 
1104  initMovingEdge(I, cMo);
1105  // Reinit the moving edge for the lines which need it.
1106  reinitMovingEdge(I, cMo);
1107 
1108  if (computeProjError)
1110 
1111  upScale(lvl);
1112  } catch (const vpException &e) {
1113  if (lvl != 0) {
1114  cMo = cMo_1;
1115  reInitLevel(lvl);
1116  upScale(lvl);
1117  } else {
1118  upScale(lvl);
1119  throw(e);
1120  }
1121  }
1122  }
1123  } while (lvl != 0);
1124 
1126 }
1127 
1134 {
1135  if (!modelInitialised) {
1136  throw vpException(vpException::fatalError, "model not initialized");
1137  }
1138 
1139  bool a = false;
1140 
1141 #ifdef VISP_HAVE_OGRE
1142  if (useOgre) {
1143  if (!faces.isOgreInitialised()) {
1146  faces.initOgre(cam);
1147  // Turn off Ogre config dialog display for the next call to this
1148  // function since settings are saved in the ogre.cfg file and used
1149  // during the next call
1150  ogreShowConfigDialog = false;
1151  }
1152  }
1153 #endif
1154 
1155  if (clippingFlag > 2)
1156  cam.computeFov(I.getWidth(), I.getHeight());
1157 
1158  visibleFace(I, cMo, a);
1159  resetMovingEdge();
1160 
1161  if (useScanLine) {
1162  if (clippingFlag <= 2)
1163  cam.computeFov(I.getWidth(), I.getHeight());
1164 
1167  }
1168 
1169  initPyramid(I, Ipyramid);
1170  unsigned int i = (unsigned int)scales.size();
1171  do {
1172  i--;
1173  if (scales[i]) {
1174  downScale(i);
1175  initMovingEdge(*Ipyramid[i], cMo);
1176  upScale(i);
1177  }
1178  } while (i != 0);
1179 
1181 }
1182 
1191 {
1192  cMo = cdMo;
1193 
1194  init(I);
1195 }
1196 
1210 void vpMbEdgeTracker::loadConfigFile(const std::string &configFile)
1211 {
1212  // Load projection error config
1213  vpMbTracker::loadConfigFile(configFile);
1214 
1215 #ifdef VISP_HAVE_XML2
1216  vpMbtXmlParser xmlp;
1217 
1218  xmlp.setCameraParameters(cam);
1221  xmlp.setMovingEdge(me);
1222 
1223  try {
1224  std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1225  xmlp.parse(configFile);
1226  } catch (...) {
1227  throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
1228  }
1229 
1230  vpCameraParameters camera;
1231  vpMe meParser;
1232  xmlp.getCameraParameters(camera);
1233  xmlp.getMe(meParser);
1234 
1235  setCameraParameters(camera);
1236  setMovingEdge(meParser);
1239 
1240  if (xmlp.hasNearClippingDistance())
1242 
1243  if (xmlp.hasFarClippingDistance())
1245 
1246  if (xmlp.getFovClipping())
1248 
1249  useLodGeneral = xmlp.getLodState();
1252 
1253  applyLodSettingInConfig = false;
1254  if (this->getNbPolygon() > 0) {
1255  applyLodSettingInConfig = true;
1259  }
1260 
1261 #else
1262  vpTRACE("You need the libXML2 to read the config file %s", configFile.c_str());
1263 #endif
1264 }
1265 
1278  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1279  const bool displayFullModel)
1280 {
1281  for (unsigned int i = 0; i < scales.size(); i += 1) {
1282  if (scales[i]) {
1283  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1284  ++it) {
1285  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1286  }
1287 
1288  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1289  it != cylinders[scaleLevel].end(); ++it) {
1290  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1291  }
1292 
1293  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1294  it != circles[scaleLevel].end(); ++it) {
1295  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1296  }
1297 
1298  break; // displaying model on one scale only
1299  }
1300  }
1301 
1302 #ifdef VISP_HAVE_OGRE
1303  if (useOgre)
1304  faces.displayOgre(cMo_);
1305 #endif
1306 }
1307 
1320  const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
1321  const bool displayFullModel)
1322 {
1323  for (unsigned int i = 0; i < scales.size(); i += 1) {
1324  if (scales[i]) {
1325  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1326  ++it) {
1327  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1328  }
1329 
1330  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1331  it != cylinders[scaleLevel].end(); ++it) {
1332  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1333  }
1334 
1335  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1336  it != circles[scaleLevel].end(); ++it) {
1337  (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
1338  }
1339  break; // displaying model on one scale only
1340  }
1341  }
1342 
1343 #ifdef VISP_HAVE_OGRE
1344  if (useOgre)
1345  faces.displayOgre(cMo_);
1346 #endif
1347 }
1349 void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage<unsigned char> &I, const unsigned int lvl)
1350 {
1351  if (lvl == 0) {
1352  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1353  vpMbtDistanceLine *l = *it;
1354  if (l->isVisible() && l->isTracked()) {
1355  l->displayMovingEdges(I);
1356  }
1357  }
1358 
1359  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1360  ++it) {
1361  vpMbtDistanceCylinder *cy = *it;
1362  if (cy->isVisible() && cy->isTracked()) {
1363  cy->displayMovingEdges(I);
1364  }
1365  }
1366 
1367  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1368  vpMbtDistanceCircle *ci = *it;
1369  if (ci->isVisible() && ci->isTracked()) {
1370  ci->displayMovingEdges(I);
1371  }
1372  }
1373  }
1374 }
1375 
1385 {
1386  const bool doNotTrack = false;
1387 
1388  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1389  ++it) {
1390  vpMbtDistanceLine *l = *it;
1391  bool isvisible = false;
1392 
1393  for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1394  ++itindex) {
1395  int index = *itindex;
1396  if (index == -1)
1397  isvisible = true;
1398  else {
1399  if (l->hiddenface->isVisible((unsigned int)index))
1400  isvisible = true;
1401  }
1402  }
1403 
1404  // Si la ligne n'appartient a aucune face elle est tout le temps visible
1405  if (l->Lindex_polygon.empty())
1406  isvisible = true; // Not sure that this can occur
1407 
1408  if (isvisible) {
1409  l->setVisible(true);
1410  l->updateTracked();
1411  if (l->meline.empty() && l->isTracked())
1412  l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1413  } else {
1414  l->setVisible(false);
1415  for (size_t a = 0; a < l->meline.size(); a++) {
1416  if (l->meline[a] != NULL)
1417  delete l->meline[a];
1418  if (a < l->nbFeature.size())
1419  l->nbFeature[a] = 0;
1420  }
1421  l->nbFeatureTotal = 0;
1422  l->meline.clear();
1423  l->nbFeature.clear();
1424  }
1425  }
1426 
1427  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1428  it != cylinders[scaleLevel].end(); ++it) {
1429  vpMbtDistanceCylinder *cy = *it;
1430 
1431  bool isvisible = false;
1432 
1433  int index = cy->index_polygon;
1434  if (index == -1)
1435  isvisible = true;
1436  else {
1437  if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1438  cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1439  isvisible = true;
1440  }
1441  // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1442 
1443  if (isvisible) {
1444  cy->setVisible(true);
1445  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1446  if (cy->isTracked())
1447  cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1448  }
1449  } else {
1450  cy->setVisible(false);
1451  if (cy->meline1 != NULL)
1452  delete cy->meline1;
1453  if (cy->meline2 != NULL)
1454  delete cy->meline2;
1455  cy->meline1 = NULL;
1456  cy->meline2 = NULL;
1457  cy->nbFeature = 0;
1458  cy->nbFeaturel1 = 0;
1459  cy->nbFeaturel2 = 0;
1460  }
1461  }
1462 
1463  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1464  it != circles[scaleLevel].end(); ++it) {
1465  vpMbtDistanceCircle *ci = *it;
1466  bool isvisible = false;
1467 
1468  int index = ci->index_polygon;
1469  if (index == -1)
1470  isvisible = true;
1471  else {
1472  if (ci->hiddenface->isVisible((unsigned int)index))
1473  isvisible = true;
1474  }
1475 
1476  if (isvisible) {
1477  ci->setVisible(true);
1478  if (ci->meEllipse == NULL) {
1479  if (ci->isTracked())
1480  ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1481  }
1482  } else {
1483  ci->setVisible(false);
1484  if (ci->meEllipse != NULL)
1485  delete ci->meEllipse;
1486  ci->meEllipse = NULL;
1487  ci->nbFeature = 0;
1488  }
1489  }
1490 }
1491 
1498 {
1499  const bool doNotTrack = false;
1500 
1501  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1502  ++it) {
1503  vpMbtDistanceLine *l = *it;
1504  if (l->isVisible() && l->isTracked()) {
1505  if (l->meline.empty()) {
1506  l->initMovingEdge(I, cMo, doNotTrack, m_mask);
1507  }
1508  l->trackMovingEdge(I);
1509  }
1510  }
1511 
1512  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1513  it != cylinders[scaleLevel].end(); ++it) {
1514  vpMbtDistanceCylinder *cy = *it;
1515  if (cy->isVisible() && cy->isTracked()) {
1516  if (cy->meline1 == NULL || cy->meline2 == NULL) {
1517  cy->initMovingEdge(I, cMo, doNotTrack, m_mask);
1518  }
1519  cy->trackMovingEdge(I, cMo);
1520  }
1521  }
1522 
1523  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1524  it != circles[scaleLevel].end(); ++it) {
1525  vpMbtDistanceCircle *ci = *it;
1526  if (ci->isVisible() && ci->isTracked()) {
1527  if (ci->meEllipse == NULL) {
1528  ci->initMovingEdge(I, cMo, doNotTrack, m_mask);
1529  }
1530  ci->trackMovingEdge(I, cMo);
1531  }
1532  }
1533 }
1534 
1541 {
1542  vpMbtDistanceLine *l;
1543  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1544  ++it) {
1545  if ((*it)->isTracked()) {
1546  l = *it;
1547  l->updateMovingEdge(I, cMo);
1548  if (l->nbFeatureTotal == 0 && l->isVisible()) {
1549  l->Reinit = true;
1550  }
1551  }
1552  }
1553 
1555  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1556  it != cylinders[scaleLevel].end(); ++it) {
1557  if ((*it)->isTracked()) {
1558  cy = *it;
1559  cy->updateMovingEdge(I, cMo);
1560  if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1561  cy->Reinit = true;
1562  }
1563  }
1564  }
1565 
1566  vpMbtDistanceCircle *ci;
1567  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1568  it != circles[scaleLevel].end(); ++it) {
1569  if ((*it)->isTracked()) {
1570  ci = *it;
1571  ci->updateMovingEdge(I, cMo);
1572  if (ci->nbFeature == 0 && ci->isVisible()) {
1573  ci->Reinit = true;
1574  }
1575  }
1576  }
1577 }
1580 {
1581  unsigned int n = 0;
1582 
1583  vpMbtDistanceLine *l;
1584  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1585  ++it) {
1586  if ((*it)->isTracked()) {
1587  l = *it;
1588  unsigned int indexLine = 0;
1589  double wmean = 0;
1590  for (size_t a = 0; a < l->meline.size(); a++) {
1591  if (l->nbFeature[a] > 0) {
1592  std::list<vpMeSite>::iterator itListLine;
1593  itListLine = l->meline[a]->getMeList().begin();
1594 
1595  for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1596  wmean += m_w_edge[n + indexLine];
1597  vpMeSite p = *itListLine;
1598  if (m_w_edge[n + indexLine] < 0.5) {
1600 
1601  *itListLine = p;
1602  }
1603 
1604  ++itListLine;
1605  indexLine++;
1606  }
1607  }
1608  }
1609  n += l->nbFeatureTotal;
1610 
1611  if (l->nbFeatureTotal != 0)
1612  wmean /= l->nbFeatureTotal;
1613  else
1614  wmean = 1;
1615 
1616  l->setMeanWeight(wmean);
1617 
1618  if (wmean < 0.8)
1619  l->Reinit = true;
1620  }
1621  }
1622 
1623  // Same thing with cylinders as with lines
1625  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1626  it != cylinders[scaleLevel].end(); ++it) {
1627  if ((*it)->isTracked()) {
1628  cy = *it;
1629  double wmean = 0;
1630  std::list<vpMeSite>::iterator itListCyl1;
1631  std::list<vpMeSite>::iterator itListCyl2;
1632 
1633  if (cy->nbFeature > 0) {
1634  itListCyl1 = cy->meline1->getMeList().begin();
1635  itListCyl2 = cy->meline2->getMeList().begin();
1636 
1637  for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1638  wmean += m_w_edge[n + i];
1639  vpMeSite p = *itListCyl1;
1640  if (m_w_edge[n + i] < 0.5) {
1642 
1643  *itListCyl1 = p;
1644  }
1645 
1646  ++itListCyl1;
1647  }
1648  }
1649 
1650  if (cy->nbFeaturel1 != 0)
1651  wmean /= cy->nbFeaturel1;
1652  else
1653  wmean = 1;
1654 
1655  cy->setMeanWeight1(wmean);
1656 
1657  if (wmean < 0.8) {
1658  cy->Reinit = true;
1659  }
1660 
1661  wmean = 0;
1662  for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1663  wmean += m_w_edge[n + i];
1664  vpMeSite p = *itListCyl2;
1665  if (m_w_edge[n + i] < 0.5) {
1667 
1668  *itListCyl2 = p;
1669  }
1670 
1671  ++itListCyl2;
1672  }
1673 
1674  if (cy->nbFeaturel2 != 0)
1675  wmean /= cy->nbFeaturel2;
1676  else
1677  wmean = 1;
1678 
1679  cy->setMeanWeight2(wmean);
1680 
1681  if (wmean < 0.8) {
1682  cy->Reinit = true;
1683  }
1684 
1685  n += cy->nbFeature;
1686  }
1687  }
1688 
1689  // Same thing with circles as with lines
1690  vpMbtDistanceCircle *ci;
1691  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1692  it != circles[scaleLevel].end(); ++it) {
1693  if ((*it)->isTracked()) {
1694  ci = *it;
1695  double wmean = 0;
1696  std::list<vpMeSite>::iterator itListCir;
1697 
1698  if (ci->nbFeature > 0) {
1699  itListCir = ci->meEllipse->getMeList().begin();
1700  }
1701 
1702  wmean = 0;
1703  for (unsigned int i = 0; i < ci->nbFeature; i++) {
1704  wmean += m_w_edge[n + i];
1705  vpMeSite p = *itListCir;
1706  if (m_w_edge[n + i] < 0.5) {
1708 
1709  *itListCir = p;
1710  }
1711 
1712  ++itListCir;
1713  }
1714 
1715  if (ci->nbFeature != 0)
1716  wmean /= ci->nbFeature;
1717  else
1718  wmean = 1;
1719 
1720  ci->setMeanWeight(wmean);
1721 
1722  if (wmean < 0.8) {
1723  ci->Reinit = true;
1724  }
1725 
1726  n += ci->nbFeature;
1727  }
1728  }
1729 }
1730 
1741 {
1742  vpMbtDistanceLine *l;
1743  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1744  ++it) {
1745  if ((*it)->isTracked()) {
1746  l = *it;
1747  if (l->Reinit && l->isVisible())
1748  l->reinitMovingEdge(I, _cMo, m_mask);
1749  }
1750  }
1751 
1753  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1754  it != cylinders[scaleLevel].end(); ++it) {
1755  if ((*it)->isTracked()) {
1756  cy = *it;
1757  if (cy->Reinit && cy->isVisible())
1758  cy->reinitMovingEdge(I, _cMo, m_mask);
1759  }
1760  }
1761 
1762  vpMbtDistanceCircle *ci;
1763  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1764  it != circles[scaleLevel].end(); ++it) {
1765  if ((*it)->isTracked()) {
1766  ci = *it;
1767  if (ci->Reinit && ci->isVisible())
1768  ci->reinitMovingEdge(I, _cMo, m_mask);
1769  }
1770  }
1771 }
1774 {
1775  for (unsigned int i = 0; i < scales.size(); i += 1) {
1776  if (scales[i]) {
1777  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1778  for (size_t a = 0; a < (*it)->meline.size(); a++) {
1779  if ((*it)->meline[a] != NULL) {
1780  delete (*it)->meline[a];
1781  (*it)->meline[a] = NULL;
1782  }
1783  }
1784 
1785  (*it)->meline.clear();
1786  (*it)->nbFeature.clear();
1787  (*it)->nbFeatureTotal = 0;
1788  }
1789 
1790  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1791  ++it) {
1792  if ((*it)->meline1 != NULL) {
1793  delete (*it)->meline1;
1794  (*it)->meline1 = NULL;
1795  }
1796  if ((*it)->meline2 != NULL) {
1797  delete (*it)->meline2;
1798  (*it)->meline2 = NULL;
1799  }
1800 
1801  (*it)->nbFeature = 0;
1802  (*it)->nbFeaturel1 = 0;
1803  (*it)->nbFeaturel2 = 0;
1804  }
1805 
1806  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1807  if ((*it)->meEllipse != NULL) {
1808  delete (*it)->meEllipse;
1809  (*it)->meEllipse = NULL;
1810  }
1811  (*it)->nbFeature = 0;
1812  }
1813  }
1814  }
1815 }
1816 
1829 void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1830 {
1831  {
1832  // suppress line already in the model
1833  bool already_here = false;
1834  vpMbtDistanceLine *l;
1835 
1836  for (unsigned int i = 0; i < scales.size(); i += 1) {
1837  if (scales[i]) {
1838  downScale(i);
1839  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1840  l = *it;
1841  if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
1842  (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
1843  already_here = true;
1844  l->addPolygon(polygon);
1845  l->hiddenface = &faces;
1846  }
1847  }
1848 
1849  if (!already_here) {
1850  l = new vpMbtDistanceLine;
1851 
1853  l->buildFrom(P1, P2);
1854  l->addPolygon(polygon);
1855  l->setMovingEdge(&me);
1856  l->hiddenface = &faces;
1857  l->useScanLine = useScanLine;
1858 
1859  l->setIndex(nline);
1860  l->setName(name);
1861 
1864 
1867 
1870 
1871  nline += 1;
1872  lines[i].push_back(l);
1873  }
1874  upScale(i);
1875  }
1876  }
1877  }
1878 }
1879 
1885 void vpMbEdgeTracker::removeLine(const std::string &name)
1886 {
1887  vpMbtDistanceLine *l;
1888 
1889  for (unsigned int i = 0; i < scales.size(); i++) {
1890  if (scales[i]) {
1891  for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1892  l = *it;
1893  if (name.compare(l->getName()) == 0) {
1894  lines[i].erase(it);
1895  break;
1896  }
1897  }
1898  }
1899  }
1900 }
1901 
1912 void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace,
1913  const std::string &name)
1914 {
1915  {
1916  bool already_here = false;
1917  vpMbtDistanceCircle *ci;
1918 
1919  for (unsigned int i = 0; i < scales.size(); i += 1) {
1920  if (scales[i]) {
1921  downScale(i);
1922  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1923  ci = *it;
1924  if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
1925  (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
1926  already_here =
1927  (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
1928  }
1929  }
1930 
1931  if (!already_here) {
1932  ci = new vpMbtDistanceCircle;
1933 
1934  ci->setCameraParameters(cam);
1935  ci->buildFrom(P1, P2, P3, r);
1936  ci->setMovingEdge(&me);
1937  ci->setIndex(ncircle);
1938  ci->setName(name);
1939  ci->index_polygon = idFace;
1940  ci->hiddenface = &faces;
1941 
1942  // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
1943  // ci->getPolygon().setClipping(clippingFlag);
1944 
1945  // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
1946  // vpPolygon3D::NEAR_CLIPPING)
1947  // ci->getPolygon().setNearClippingDistance(distNearClip);
1948 
1949  // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
1950  // vpPolygon3D::FAR_CLIPPING)
1951  // ci->getPolygon().setFarClippingDistance(distFarClip);
1952 
1953  ncircle += 1;
1954  circles[i].push_back(ci);
1955  }
1956  upScale(i);
1957  }
1958  }
1959  }
1960 }
1961 
1971 void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace,
1972  const std::string &name)
1973 {
1974  {
1975  bool already_here = false;
1977 
1978  for (unsigned int i = 0; i < scales.size(); i += 1) {
1979  if (scales[i]) {
1980  downScale(i);
1981  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1982  ++it) {
1983  cy = *it;
1984  if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
1985  (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
1986  already_here =
1987  (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
1988  }
1989  }
1990 
1991  if (!already_here) {
1992  cy = new vpMbtDistanceCylinder;
1993 
1994  cy->setCameraParameters(cam);
1995  cy->buildFrom(P1, P2, r);
1996  cy->setMovingEdge(&me);
1997  cy->setIndex(ncylinder);
1998  cy->setName(name);
1999  cy->index_polygon = idFace;
2000  cy->hiddenface = &faces;
2001  ncylinder += 1;
2002  cylinders[i].push_back(cy);
2003  }
2004  upScale(i);
2005  }
2006  }
2007  }
2008 }
2009 
2015 void vpMbEdgeTracker::removeCylinder(const std::string &name)
2016 {
2018 
2019  for (unsigned int i = 0; i < scales.size(); i++) {
2020  if (scales[i]) {
2021  for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
2022  cy = *it;
2023  if (name.compare(cy->getName()) == 0) {
2024  cylinders[i].erase(it);
2025  break;
2026  }
2027  }
2028  }
2029  }
2030 }
2031 
2037 void vpMbEdgeTracker::removeCircle(const std::string &name)
2038 {
2039  vpMbtDistanceCircle *ci;
2040 
2041  for (unsigned int i = 0; i < scales.size(); i++) {
2042  if (scales[i]) {
2043  for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2044  ci = *it;
2045  if (name.compare(ci->getName()) == 0) {
2046  circles[i].erase(it);
2047  break;
2048  }
2049  }
2050  }
2051  }
2052 }
2053 
2060 {
2061  unsigned int nbpt = p.getNbPoint();
2062  if (nbpt > 0) {
2063  for (unsigned int i = 0; i < nbpt - 1; i++)
2064  addLine(p.p[i], p.p[i + 1], p.getIndex());
2065  addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
2066  }
2067 }
2068 
2081  bool &newvisibleline)
2082 {
2083  unsigned int n;
2084  bool changed = false;
2085 
2086  if (!useOgre) {
2087  // n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89),
2088  // changed);
2089  n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2090  } else {
2091 #ifdef VISP_HAVE_OGRE
2092  n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2093 #else
2094  n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed);
2095 #endif
2096  }
2097 
2098  if (n > nbvisiblepolygone) {
2099  // cout << "une nouvelle face est visible " << endl;
2100  newvisibleline = true;
2101  } else
2102  newvisibleline = false;
2103 
2104  nbvisiblepolygone = n;
2105 }
2106 
2123 {
2124  unsigned int nbpt = polygon.getNbPoint();
2125  if (nbpt > 0) {
2126  for (unsigned int i = 0; i < nbpt - 1; i++)
2127  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2128  vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
2129  }
2130 }
2147 {
2148  unsigned int nbpt = polygon.getNbPoint();
2149  if (nbpt > 0) {
2150  for (unsigned int i = 0; i < nbpt - 1; i++)
2151  vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2152  }
2153 }
2155 unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
2156  unsigned int &nberrors_circles)
2157 {
2158  unsigned int nbrow = 0;
2159  nberrors_lines = 0;
2160  nberrors_cylinders = 0;
2161  nberrors_circles = 0;
2162 
2163  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2164  ++it) {
2165 
2166  vpMbtDistanceLine *l = *it;
2167 
2168  if (l->isTracked()) {
2170  nbrow += l->nbFeatureTotal;
2171  nberrors_lines += l->nbFeatureTotal;
2172  }
2173  }
2174 
2175  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2176  it != cylinders[scaleLevel].end(); ++it) {
2177  vpMbtDistanceCylinder *cy = *it;
2178 
2179  if (cy->isTracked()) {
2181  nbrow += cy->nbFeature;
2182  nberrors_cylinders += cy->nbFeature;
2183  }
2184  }
2185 
2186  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2187  it != circles[scaleLevel].end(); ++it) {
2188  vpMbtDistanceCircle *ci = *it;
2189 
2190  if (ci->isTracked()) {
2192  nbrow += ci->nbFeature;
2193  nberrors_circles += ci->nbFeature;
2194  }
2195  }
2196 
2197  return nbrow;
2198 }
2199 
2211 void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
2212  const int idFace, const std::string &name)
2213 {
2214  addCircle(p1, p2, p3, radius, (int)idFace, name);
2215 }
2216 
2227 void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
2228  const std::string &name)
2229 {
2230  addCylinder(p1, p2, radius, (int)idFace, name);
2231 }
2232 
2239 {
2240  this->cMo.eye();
2241  vpMbtDistanceLine *l;
2243  vpMbtDistanceCircle *ci;
2244 
2245  for (unsigned int i = 0; i < scales.size(); i += 1) {
2246  if (scales[i]) {
2247  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2248  l = *it;
2249  if (l != NULL)
2250  delete l;
2251  l = NULL;
2252  }
2253 
2254  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2255  ++it) {
2256  cy = *it;
2257  if (cy != NULL)
2258  delete cy;
2259  cy = NULL;
2260  }
2261 
2262  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2263  ci = *it;
2264  if (ci != NULL)
2265  delete ci;
2266  ci = NULL;
2267  }
2268  lines[i].clear();
2269  cylinders[i].clear();
2270  circles[i].clear();
2271  }
2272  }
2273 
2274  faces.reset();
2275 
2276  useScanLine = false;
2277 
2278 #ifdef VISP_HAVE_OGRE
2279  useOgre = false;
2280 #endif
2281 
2282  m_computeInteraction = true;
2283  nline = 0;
2284  ncylinder = 0;
2285  m_lambda = 1.0;
2286  nbvisiblepolygone = 0;
2287  percentageGdPt = 0.4;
2288 
2289  angleAppears = vpMath::rad(89);
2292 
2294 
2295  // reinitialization of the scales.
2296  this->setScales(scales);
2297 }
2298 
2311 void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2312  const vpHomogeneousMatrix &cMo_, const bool verbose,
2313  const vpHomogeneousMatrix &T)
2314 {
2315  this->cMo.eye();
2316  vpMbtDistanceLine *l;
2318  vpMbtDistanceCircle *ci;
2319 
2320  for (unsigned int i = 0; i < scales.size(); i += 1) {
2321  if (scales[i]) {
2322  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2323  l = *it;
2324  if (l != NULL)
2325  delete l;
2326  l = NULL;
2327  }
2328 
2329  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2330  ++it) {
2331  cy = *it;
2332  if (cy != NULL)
2333  delete cy;
2334  cy = NULL;
2335  }
2336 
2337  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2338  ci = *it;
2339  if (ci != NULL)
2340  delete ci;
2341  ci = NULL;
2342  }
2343 
2344  lines[i].clear();
2345  cylinders[i].clear();
2346  circles[i].clear();
2347  }
2348  }
2349 
2350  faces.reset();
2351 
2352  // compute_interaction=1;
2353  nline = 0;
2354  ncylinder = 0;
2355  ncircle = 0;
2356  // lambda = 1;
2357  nbvisiblepolygone = 0;
2358 
2359  loadModel(cad_name, verbose, T);
2360  initFromPose(I, cMo_);
2361 }
2362 
2373 unsigned int vpMbEdgeTracker::getNbPoints(const unsigned int level) const
2374 {
2375  if ((level > scales.size()) || !scales[level]) {
2376  throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2377  level);
2378  }
2379 
2380  unsigned int nbGoodPoints = 0;
2381  vpMbtDistanceLine *l;
2382  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2383  l = *it;
2384  if (l->isVisible() && l->isTracked()) {
2385  for (size_t a = 0; a < l->meline.size(); a++) {
2386  if (l->nbFeature[a] != 0)
2387  for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2388  itme != l->meline[a]->getMeList().end(); ++itme) {
2389  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2390  nbGoodPoints++;
2391  }
2392  }
2393  }
2394  }
2395 
2397  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2398  ++it) {
2399  cy = *it;
2400  if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
2401  for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2402  itme1 != cy->meline1->getMeList().end(); ++itme1) {
2403  if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2404  nbGoodPoints++;
2405  }
2406  for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2407  itme2 != cy->meline2->getMeList().end(); ++itme2) {
2408  if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2409  nbGoodPoints++;
2410  }
2411  }
2412  }
2413 
2414  vpMbtDistanceCircle *ci;
2415  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2416  ci = *it;
2417  if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
2418  for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2419  itme != ci->meEllipse->getMeList().end(); ++itme) {
2420  if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2421  nbGoodPoints++;
2422  }
2423  }
2424  }
2425 
2426  return nbGoodPoints;
2427 }
2428 
2450 void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2451 {
2452  unsigned int nbActivatedLevels = 0;
2453  for (unsigned int i = 0; i < scale.size(); i++) {
2454  if (scale[i]) {
2455  nbActivatedLevels++;
2456  }
2457  }
2458 
2459  if (scale.empty() || (nbActivatedLevels == 0)) {
2460  vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2461  "tracking. Use the global one");
2462  this->scales.resize(0);
2463  this->scales.push_back(true);
2464 
2465  lines.resize(1);
2466  lines[0].clear();
2467 
2468  cylinders.resize(1);
2469  cylinders[0].clear();
2470 
2471  circles.resize(1);
2472  circles[0].clear();
2473  } else {
2474  this->scales = scale;
2475 
2476  lines.resize(scale.size());
2477  cylinders.resize(scale.size());
2478  circles.resize(scale.size());
2479 
2480  for (unsigned int i = 0; i < lines.size(); i++) {
2481  lines[i].clear();
2482  cylinders[i].clear();
2483  circles[i].clear();
2484  }
2485  }
2486 }
2487 
2493 void vpMbEdgeTracker::setFarClippingDistance(const double &dist)
2494 {
2496  std::cerr << "Far clipping value cannot be inferior than near clipping "
2497  "value. Far clipping won't be considered."
2498  << std::endl;
2499  else if (dist < 0)
2500  std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2501  "won't be considered."
2502  << std::endl;
2503  else {
2505  vpMbtDistanceLine *l;
2506 
2507  for (unsigned int i = 0; i < scales.size(); i += 1) {
2508  if (scales[i]) {
2509  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2510  l = *it;
2512  }
2513  }
2514  }
2515  }
2516 }
2517 
2523 void vpMbEdgeTracker::setNearClippingDistance(const double &dist)
2524 {
2526  std::cerr << "Near clipping value cannot be superior than far clipping "
2527  "value. Near clipping won't be considered."
2528  << std::endl;
2529  else if (dist < 0)
2530  std::cerr << "Near clipping value cannot be inferior than 0. Near "
2531  "clipping won't be considered."
2532  << std::endl;
2533  else {
2535  vpMbtDistanceLine *l;
2536 
2537  for (unsigned int i = 0; i < scales.size(); i += 1) {
2538  if (scales[i]) {
2539  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2540  l = *it;
2542  }
2543  }
2544  }
2545  }
2546 }
2547 
2555 void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2556 {
2557  vpMbTracker::setClipping(flags);
2558 
2559  vpMbtDistanceLine *l;
2560 
2561  for (unsigned int i = 0; i < scales.size(); i += 1) {
2562  if (scales[i]) {
2563  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2564  l = *it;
2566  }
2567  }
2568  }
2569 }
2570 
2587  std::vector<const vpImage<unsigned char> *> &_pyramid)
2588 {
2589  _pyramid.resize(scales.size());
2590 
2591  if (scales[0]) {
2592  _pyramid[0] = &_I;
2593  } else {
2594  _pyramid[0] = NULL;
2595  }
2596 
2597  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2598  if (scales[i]) {
2599  unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2600  vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2601 #if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
2602  IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
2603  vpI0->imageData = (char *)(_I.bitmap);
2604  IplImage *vpI =
2605  cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
2606  cvResize(vpI0, vpI, CV_INTER_NN);
2607  vpImageConvert::convert(vpI, *I);
2608  cvReleaseImage(&vpI);
2609  vpI0->imageData = NULL;
2610  cvReleaseImageHeader(&vpI0);
2611 #else
2612  for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2613  for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2614  (*I)[k][l] = _I[ii][jj];
2615  }
2616  }
2617 #endif
2618  _pyramid[i] = I;
2619  } else {
2620  _pyramid[i] = NULL;
2621  }
2622  }
2623 }
2624 
2631 void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2632 {
2633  if (_pyramid.size() > 0) {
2634  _pyramid[0] = NULL;
2635  for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2636  if (_pyramid[i] != NULL) {
2637  delete _pyramid[i];
2638  _pyramid[i] = NULL;
2639  }
2640  }
2641  _pyramid.resize(0);
2642  }
2643 }
2644 
2655 void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
2656 {
2657  if (level > scales.size() || !scales[level]) {
2658  std::ostringstream oss;
2659  oss << level;
2660  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2661  throw vpException(vpException::dimensionError, errorMsg);
2662  }
2663 
2664  linesList = lines[level];
2665 }
2666 
2677 void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, const unsigned int level) const
2678 {
2679  if (level > scales.size() || !scales[level]) {
2680  std::ostringstream oss;
2681  oss << level;
2682  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2683  throw vpException(vpException::dimensionError, errorMsg);
2684  }
2685 
2686  cylindersList = cylinders[level];
2687 }
2688 
2699 void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
2700 {
2701  if (level > scales.size() || !scales[level]) {
2702  std::ostringstream oss;
2703  oss << level;
2704  std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2705  throw vpException(vpException::dimensionError, errorMsg);
2706  }
2707 
2708  circlesList = circles[level];
2709 }
2710 
2717 void vpMbEdgeTracker::downScale(const unsigned int _scale)
2718 {
2719  const double ratio = pow(2., (int)_scale);
2720  scaleLevel = _scale;
2721 
2722  vpMatrix K = cam.get_K();
2723 
2724  K[0][0] /= ratio;
2725  K[1][1] /= ratio;
2726  K[0][2] /= ratio;
2727  K[1][2] /= ratio;
2728 
2730 }
2731 
2738 void vpMbEdgeTracker::upScale(const unsigned int _scale)
2739 {
2740  const double ratio = pow(2., (int)_scale);
2741  scaleLevel = 0;
2742 
2743  vpMatrix K = cam.get_K();
2744 
2745  K[0][0] *= ratio;
2746  K[1][1] *= ratio;
2747  K[0][2] *= ratio;
2748  K[1][2] *= ratio;
2749 
2751 }
2752 
2760 void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2761 {
2762  unsigned int scaleLevel_1 = scaleLevel;
2763  scaleLevel = _lvl;
2764 
2765  vpMbtDistanceLine *l;
2766  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2767  ++it) {
2768  if ((*it)->isTracked()) {
2769  l = *it;
2770  l->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2771  }
2772  }
2773 
2775  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2776  it != cylinders[scaleLevel].end(); ++it) {
2777  if ((*it)->isTracked()) {
2778  cy = *it;
2779  cy->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2780  }
2781  }
2782 
2783  vpMbtDistanceCircle *ci;
2784  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2785  it != circles[scaleLevel].end(); ++it) {
2786  if ((*it)->isTracked()) {
2787  ci = *it;
2788  ci->reinitMovingEdge(*Ipyramid[_lvl], cMo, m_mask);
2789  }
2790  }
2791 
2792  trackMovingEdge(*Ipyramid[_lvl]);
2793  updateMovingEdge(*Ipyramid[_lvl]);
2794  scaleLevel = scaleLevel_1;
2795 }
2796 
2804 void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2805 {
2806  for (unsigned int i = 0; i < scales.size(); i += 1) {
2807  if (scales[i]) {
2808  for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2809  /*(*it)->setTracked(useEdgeTracking);
2810  for(std::list<int>::const_iterator
2811  itpoly=(*it)->Lindex_polygon.begin();
2812  itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2813  if(faces[(*itpoly)]->getName() != name){
2814  (*it)->setTracked(true);
2815  break;
2816  }
2817  }*/
2818 
2819  (*it)->setTracked(name, useEdgeTracking);
2820  }
2821 
2822  for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2823  ++it) {
2824  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2825  (*it)->setTracked(useEdgeTracking);
2826  }
2827  }
2828 
2829  for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2830  if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2831  (*it)->setTracked(useEdgeTracking);
2832  }
2833  }
2834  }
2835  }
2836 }
vpMbHiddenFaces::setOgreShowConfigDialog
void setOgreShowConfigDialog(const bool showConfigDialog)
Definition: vpMbHiddenFaces.h:253
vpTrackingException
Error that can be emited by the vpTracker class and its derivates.
Definition: vpTrackingException.h:68
vpMbEdgeTracker::setClipping
virtual void setClipping(const unsigned int &flags)
Definition: vpMbEdgeTracker.cpp:2554
vpMbtDistanceCylinder::meline1
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Definition: vpMbtDistanceCylinder.h:80
vpMbHiddenFaces::isVisible
bool isVisible(const unsigned int i)
Definition: vpMbHiddenFaces.h:186
vpMbEdgeTracker::m_error_edge
vpColVector m_error_edge
(s - s*)
Definition: vpMbEdgeTracker.h:337
vpMbEdgeTracker::removeCircle
void removeCircle(const std::string &name)
Definition: vpMbEdgeTracker.cpp:2036
vpMbEdgeTracker::upScale
void upScale(const unsigned int _scale)
Definition: vpMbEdgeTracker.cpp:2737
vpMbTracker::loadConfigFile
virtual void loadConfigFile(const std::string &configFile)
Definition: vpMbTracker.cpp:3406
vpMbtDistanceCircle::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCircle.cpp:188
vpMbtDistanceLine::meline
std::vector< vpMbtMeLine * > meline
The moving edge container.
Definition: vpMbtDistanceLine.h:84
vpMbEdgeTracker::initCircle
virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=0, const std::string &name="")
Definition: vpMbEdgeTracker.cpp:2210
vpMbEdgeTracker::m_weightedError_edge
vpColVector m_weightedError_edge
Weighted error.
Definition: vpMbEdgeTracker.h:341
vpMbEdgeTracker::setCameraParameters
virtual void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbEdgeTracker.h:410
vpMbtDistanceLine::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceLine.cpp:452
vpColVector::resize
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:243
vpMbEdgeTracker::addPolygon
void addPolygon(vpMbtPolygon &p)
Definition: vpMbEdgeTracker.cpp:2058
vpMbTracker::setLod
virtual void setLod(const bool useLod, const std::string &name="")
Definition: vpMbTracker.cpp:2361
vpXmlParser::parse
void parse(const std::string &filename)
Definition: vpXmlParser.cpp:420
vpMbEdgeTracker::downScale
void downScale(const unsigned int _scale)
Definition: vpMbEdgeTracker.cpp:2716
vpMbTracker::m_computeInteraction
bool m_computeInteraction
Definition: vpMbTracker.h:190
vpMbEdgeTracker::m_robustCircles
vpRobust m_robustCircles
Definition: vpMbEdgeTracker.h:327
vpMbEdgeTracker::m_factor
vpColVector m_factor
Edge VVS variables.
Definition: vpMbEdgeTracker.h:324
vpMbEdgeTracker::setScales
void setScales(const std::vector< bool > &_scales)
Definition: vpMbEdgeTracker.cpp:2449
vpMbEdgeTracker::init
void init(const vpImage< unsigned char > &I)
Definition: vpMbEdgeTracker.cpp:1132
vpMbtDistanceLine::getName
std::string getName() const
Definition: vpMbtDistanceLine.h:176
vpMbTracker::computeJTR
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
Definition: vpMbTracker.cpp:2475
vpMbEdgeTracker::track
void track(const vpImage< unsigned char > &I)
Definition: vpMbEdgeTracker.cpp:1031
vpMbTracker::covarianceMatrix
vpMatrix covarianceMatrix
Covariance matrix.
Definition: vpMbTracker.h:135
vpMbTracker::distNearClip
double distNearClip
Distance for near clipping.
Definition: vpMbTracker.h:154
vpMath::sqr
static double sqr(double x)
Definition: vpMath.h:107
vpMbtDistanceCylinder::Reinit
bool Reinit
Indicates if the line has to be reinitialized.
Definition: vpMbtDistanceCylinder.h:108
vpMbtDistanceLine::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceLine.h:92
vpMbtDistanceLine::nbFeature
std::vector< unsigned int > nbFeature
The number of moving edges.
Definition: vpMbtDistanceLine.h:97
vpMbtDistanceCylinder
Manage a cylinder used in the model-based tracker.
Definition: vpMbtDistanceCylinder.h:65
vpMbtDistanceLine::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceLine.cpp:420
vpMbEdgeTracker::~vpMbEdgeTracker
virtual ~vpMbEdgeTracker()
Definition: vpMbEdgeTracker.cpp:88
vpMbHiddenFaces::isAppearing
bool isAppearing(const unsigned int i)
Definition: vpMbHiddenFaces.h:168
vpMbTracker::computeVVSCheckLevenbergMarquardt
virtual void computeVVSCheckLevenbergMarquardt(const unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
Definition: vpMbTracker.cpp:2524
vpMbHiddenFaces::setVisibleOgre
unsigned int setVisibleOgre(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
Definition: vpMbHiddenFaces.h:768
vpMbtDistanceCylinder::nbFeature
unsigned int nbFeature
The number of moving edges.
Definition: vpMbtDistanceCylinder.h:102
vpMbtDistanceLine::Lindex_polygon
std::list< int > Lindex_polygon
Index of the faces which contain the line.
Definition: vpMbtDistanceLine.h:105
vpMbHiddenFaces::reset
void reset()
Definition: vpMbHiddenFaces.h:397
vpMbHiddenFaces::displayOgre
void displayOgre(const vpHomogeneousMatrix &cMo)
Definition: vpMbHiddenFaces.h:742
vpMbtDistanceLine::initMovingEdge
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceLine.cpp:306
vpMbtDistanceLine::buildFrom
void buildFrom(vpPoint &_p1, vpPoint &_p2)
Definition: vpMbtDistanceLine.cpp:165
vpRobust::setThreshold
void setThreshold(const double noise_threshold)
Definition: vpRobust.h:114
vpMbEdgeTracker::computeVVSWeights
virtual void computeVVSWeights()
Definition: vpMbEdgeTracker.cpp:842
vpMbTracker::m_projectionErrorDisplayLength
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:220
vpMeSite::M_ESTIMATOR
Definition: vpMeSite.h:79
vpMbtDistanceLine::useScanLine
bool useScanLine
Use scanline rendering.
Definition: vpMbtDistanceLine.h:81
vpMbtDistanceCylinder::radius
double radius
The radius of the cylinder.
Definition: vpMbtDistanceCylinder.h:90
vpMbHiddenFaces::computeClippedPolygons
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
Definition: vpMbHiddenFaces.h:440
vpMbEdgeTracker::visibleFace
void visibleFace(const vpImage< unsigned char > &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
Definition: vpMbEdgeTracker.cpp:2079
vpMbtDistanceLine::nbFeatureTotal
unsigned int nbFeatureTotal
The number of moving edges.
Definition: vpMbtDistanceLine.h:99
vpMbTracker::useOgre
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:160
vpMbTracker::loadModel
virtual void loadModel(const std::string &modelFile, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Definition: vpMbTracker.cpp:1147
vpImageConvert::convert
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition: vpImageConvert.cpp:78
vpMbEdgeTracker::m_errorCircles
vpColVector m_errorCircles
Definition: vpMbEdgeTracker.h:333
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:101
vpMbtDistanceLine::p1
vpPoint * p1
The first extremity.
Definition: vpMbtDistanceLine.h:88
vpMbTracker::cMo
vpHomogeneousMatrix cMo
The current pose.
Definition: vpMbTracker.h:118
vpMbXmlParser::getMinPolygonAreaThreshold
double getMinPolygonAreaThreshold() const
Definition: vpMbXmlParser.h:167
vpMbEdgeTracker::ncircle
unsigned int ncircle
Definition: vpMbEdgeTracker.h:295
vpMatrix::AtA
vpMatrix AtA() const
Definition: vpMatrix.cpp:523
vpMbtDistanceCircle::getName
std::string getName() const
Definition: vpMbtDistanceCircle.h:166
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpMbEdgeTracker::setMovingEdge
void setMovingEdge(const vpMe &me)
Definition: vpMbEdgeTracker.cpp:135
vpMbtDistanceLine::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceLine.cpp:723
vpMbEdgeTracker::m_errorLines
vpColVector m_errorLines
Definition: vpMbEdgeTracker.h:331
vpMbtDistanceLine::getPolygon
vpMbtPolygon & getPolygon()
Definition: vpMbtDistanceLine.h:183
vpMbEdgeTracker::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I)
Definition: vpMbEdgeTracker.cpp:1496
vpMbTracker::GAUSS_NEWTON_OPT
Definition: vpMbTracker.h:112
vpMbEdgeTracker::loadConfigFile
void loadConfigFile(const std::string &configFile)
Definition: vpMbEdgeTracker.cpp:1209
vpMbTracker::m_projectionErrorDisplay
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
Definition: vpMbTracker.h:218
vpMbtDistanceCircle::initMovingEdge
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceCircle.cpp:147
vpMbtDistanceCylinder::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceCylinder.h:98
vpMbtDistanceCylinder::hiddenface
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
Definition: vpMbtDistanceCylinder.h:113
vpMbHiddenFaces::isOgreInitialised
bool isOgreInitialised()
Definition: vpMbHiddenFaces.h:176
vpPolygon3D::setFarClippingDistance
void setFarClippingDistance(const double &dist)
Definition: vpPolygon3D.h:193
vpMbtDistanceLine::isTracked
bool isTracked() const
Definition: vpMbtDistanceLine.h:195
vpRobust::MEstimator
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:175
vpMbXmlParser::getFarClippingDistance
double getFarClippingDistance() const
Definition: vpMbXmlParser.h:139
vpMath::deg
static double deg(double rad)
Definition: vpMath.h:94
vpMbEdgeTracker::initMovingEdge
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
Definition: vpMbEdgeTracker.cpp:1383
vpMbtDistanceCylinder::setMovingEdge
void setMovingEdge(vpMe *Me)
Definition: vpMbtDistanceCylinder.cpp:161
vpMbXmlParser::getCameraParameters
void getCameraParameters(vpCameraParameters &_cam) const
Definition: vpMbXmlParser.h:132
vpMbtDistanceCircle::p3
vpPoint * p3
An other point on the plane containing the circle.
Definition: vpMbtDistanceCircle.h:89
vpMbtDistanceCircle::hiddenface
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
Definition: vpMbtDistanceCircle.h:100
vpMbtDistanceLine::setVisible
void setVisible(bool _isvisible)
Definition: vpMbtDistanceLine.h:251
vpMbEdgeTracker::getLcircle
void getLcircle(std::list< vpMbtDistanceCircle * > &circlesList, const unsigned int level=0) const
Definition: vpMbEdgeTracker.cpp:2698
vpHomogeneousMatrix::eye
void eye()
Definition: vpHomogeneousMatrix.cpp:663
vpException::fatalError
Fatal error.
Definition: vpException.h:95
vpMbtDistanceCylinder::error
vpColVector error
The error vector.
Definition: vpMbtDistanceCylinder.h:100
vpMbEdgeTracker::computeVVSFirstPhase
void computeVVSFirstPhase(const vpImage< unsigned char > &I, const unsigned int iter, double &count, const unsigned int lvl=0)
Definition: vpMbEdgeTracker.cpp:292
vpMbTracker::computeCovariance
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:133
vpMbXmlParser::hasFarClippingDistance
bool hasFarClippingDistance() const
Definition: vpMbXmlParser.h:181
vpMbEdgeTracker::setNearClippingDistance
virtual void setNearClippingDistance(const double &dist)
Definition: vpMbEdgeTracker.cpp:2522
vpException::ioError
I/O error.
Definition: vpException.h:90
vpMbTracker::modelInitialised
bool modelInitialised
Definition: vpMbTracker.h:128
vpMbTracker::faces
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:148
vpMbEdgeTracker::computeVVSFirstPhaseFactor
void computeVVSFirstPhaseFactor(const vpImage< unsigned char > &I, const unsigned int lvl=0)
Definition: vpMbEdgeTracker.cpp:558
vpMbtDistanceLine::Reinit
bool Reinit
Indicates if the line has to be reinitialized.
Definition: vpMbtDistanceLine.h:101
vpMbEdgeTracker::scaleLevel
unsigned int scaleLevel
Definition: vpMbEdgeTracker.h:318
vpMbtDistanceCylinder::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceCylinder.cpp:488
vpMbEdgeTracker::m_robustLines
vpRobust m_robustLines
Definition: vpMbEdgeTracker.h:325
vpMbEdgeTracker::initMbtTracking
unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles)
Definition: vpMbEdgeTracker.cpp:2154
vpMbtDistanceCylinder::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:681
vpMbtDistanceCylinder::meline2
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
Definition: vpMbtDistanceCylinder.h:82
vpMbEdgeTracker::m_wCircles
vpColVector m_wCircles
Definition: vpMbEdgeTracker.h:330
vpMbtDistanceCylinder::p2
vpPoint * p2
The second extremity on the axe.
Definition: vpMbtDistanceCylinder.h:95
vpPolygon3D::FOV_CLIPPING
Definition: vpPolygon3D.h:69
vpMbtDistanceCircle
Manage a circle used in the model-based tracker.
Definition: vpMbtDistanceCircle.h:60
vpMbEdgeTracker::reInitModel
virtual void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_, const bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
Definition: vpMbEdgeTracker.cpp:2310
vpMbtDistanceCircle::isTracked
bool isTracked() const
Definition: vpMbtDistanceCircle.h:185
vpMbXmlParser::setAngleAppear
void setAngleAppear(const double &aappear)
Definition: vpMbXmlParser.h:200
vpMbtDistanceCylinder::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCylinder.cpp:649
vpMbXmlParser::getFovClipping
bool getFovClipping() const
Definition: vpMbXmlParser.h:146
vpMbtDistanceCylinder::buildFrom
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
Definition: vpMbtDistanceCylinder.cpp:118
vpMbtDistanceCircle::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceCircle.cpp:247
vpMeSite::NO_SUPPRESSION
Definition: vpMeSite.h:76
vpMbtDistanceCylinder::isVisible
bool isVisible() const
Definition: vpMbtDistanceCylinder.h:208
vpMbtDistanceCircle::buildFrom
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
Definition: vpMbtDistanceCircle.cpp:102
vpMbtPolygon::getName
std::string getName() const
Definition: vpMbtPolygon.h:107
vpMbEdgeTracker::reInitLevel
void reInitLevel(const unsigned int _lvl)
Definition: vpMbEdgeTracker.cpp:2759
vpMbtDistanceCylinder::getName
std::string getName() const
Definition: vpMbtDistanceCylinder.h:189
vpTrackingException::notEnoughPointError
Definition: vpTrackingException.h:79
vpMbEdgeTracker::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
Definition: vpMbEdgeTracker.cpp:1739
vpCameraParameters::get_K
vpMatrix get_K() const
Definition: vpCameraParameters.cpp:492
vpMbEdgeTracker::removeCylinder
void removeCylinder(const std::string &name)
Definition: vpMbEdgeTracker.cpp:2014
vpMath::sign
static int() sign(double x)
Definition: vpMath.h:261
vpMbEdgeTracker::scales
std::vector< bool > scales
Vector of scale level to use for the multi-scale tracking.
Definition: vpMbEdgeTracker.h:309
vpMbtDistanceLine::error
vpColVector error
The error vector.
Definition: vpMbtDistanceLine.h:94
vpMbEdgeTracker::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I)
Definition: vpMbEdgeTracker.cpp:1539
vpPolygon3D::p
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:80
vpMbtDistanceCircle::nbFeature
unsigned int nbFeature
The number of moving edges.
Definition: vpMbtDistanceCircle.h:96
vpMbEdgeTracker::m_w_edge
vpColVector m_w_edge
Robust weights.
Definition: vpMbEdgeTracker.h:339
vpException::dimensionError
Bad dimension.
Definition: vpException.h:94
vpMe
Definition: vpMe.h:59
vpMbEdgeTracker::removeLine
void removeLine(const std::string &name)
Definition: vpMbEdgeTracker.cpp:1884
vpMbtDistanceCircle::setIndex
void setIndex(const unsigned int i)
Definition: vpMbtDistanceCircle.h:215
vpMbtDistanceLine::setIndex
void setIndex(const unsigned int i)
Definition: vpMbtDistanceLine.h:218
vpMbEdgeTracker::m_robustCylinders
vpRobust m_robustCylinders
Definition: vpMbEdgeTracker.h:326
vpMbTracker::cam
vpCameraParameters cam
The camera parameters.
Definition: vpMbTracker.h:116
vpMbXmlParser::getAngleAppear
double getAngleAppear() const
Definition: vpMbXmlParser.h:123
vpMbTracker::m_lambda
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:192
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:329
vpMbEdgeTracker::resetMovingEdge
void resetMovingEdge()
Definition: vpMbEdgeTracker.cpp:1772
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:71
vpMbtDistanceCircle::p1
vpPoint * p1
The center of the circle.
Definition: vpMbtDistanceCircle.h:85
vpMbTracker::useScanLine
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:163
vpMbEdgeTracker::me
vpMe me
The moving edges parameters.
Definition: vpMbEdgeTracker.h:277
vpMbEdgeTracker::initCylinder
virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace=0, const std::string &name="")
Definition: vpMbEdgeTracker.cpp:2226
vpMbEdgeTracker::cleanPyramid
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
Definition: vpMbEdgeTracker.cpp:2630
vpAROgre::setWindowName
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:268
vpMbtDistanceCircle::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCircle.cpp:212
vpMbTracker::initFromPose
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
Definition: vpMbTracker.cpp:841
vpMbTracker::projectionError
double projectionError
Definition: vpMbTracker.h:141
vpTrackingException::fatalError
Definition: vpTrackingException.h:81
vpImage::bitmap
Type * bitmap
points toward the bitmap
Definition: vpImage.h:132
vpMbEdgeTracker::computeVVS
void computeVVS(const vpImage< unsigned char > &_I, const unsigned int lvl)
Definition: vpMbEdgeTracker.cpp:169
vpMbEdgeTracker::setFarClippingDistance
virtual void setFarClippingDistance(const double &dist)
Definition: vpMbEdgeTracker.cpp:2492
vpMbtDistanceCylinder::nbFeaturel1
unsigned int nbFeaturel1
The number of moving edges on line 1.
Definition: vpMbtDistanceCylinder.h:104
vpMbXmlParser::setCameraParameters
void setCameraParameters(const vpCameraParameters &_cam)
Definition: vpMbXmlParser.h:209
vpMbXmlParser::getLodState
bool getLodState() const
Definition: vpMbXmlParser.h:153
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:103
vpMbtXmlParser::setMovingEdge
void setMovingEdge(const vpMe &_ecm)
Definition: vpMbtXmlParser.h:105
vpMbTracker::setMinPolygonAreaThresh
virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="")
Definition: vpMbTracker.cpp:2396
vpMbEdgeTracker::percentageGdPt
double percentageGdPt
Definition: vpMbEdgeTracker.h:306
vpMbtPolygon
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:65
vpMbTracker::setMinLineLengthThresh
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="")
Definition: vpMbTracker.cpp:2379
vpMbEdgeTracker::getLline
void getLline(std::list< vpMbtDistanceLine * > &linesList, const unsigned int level=0) const
Definition: vpMbEdgeTracker.cpp:2654
vpMbtDistanceCylinder::updateMovingEdge
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:353
vpColVector::insert
void insert(unsigned int i, const vpColVector &v)
Definition: vpColVector.cpp:1275
vpMbTracker::m_initialMu
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:198
vpMbtPolygon::getIndex
int getIndex() const
Definition: vpMbtPolygon.h:100
vpMbEdgeTracker::nbvisiblepolygone
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
Definition: vpMbEdgeTracker.h:302
vpMbtDistanceCircle::setMovingEdge
void setMovingEdge(vpMe *Me)
Definition: vpMbtDistanceCircle.cpp:128
vpMbtDistanceCircle::meEllipse
vpMbtMeEllipse * meEllipse
The moving edge containers.
Definition: vpMbtDistanceCircle.h:76
vpMbTracker::distFarClip
double distFarClip
Distance for near clipping.
Definition: vpMbTracker.h:156
vpMbtDistanceCylinder::isTracked
bool isTracked() const
Definition: vpMbtDistanceCylinder.h:201
vpMbTracker::m_optimizationMethod
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:145
vpMbtDistanceLine::setCameraParameters
void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbtDistanceLine.h:211
vpMbEdgeTracker::resetTracker
void resetTracker()
Definition: vpMbEdgeTracker.cpp:2237
vpMbEdgeTracker::m_wCylinders
vpColVector m_wCylinders
Definition: vpMbEdgeTracker.h:329
vpMbtDistanceCircle::radius
double radius
The radius of the circle.
Definition: vpMbtDistanceCircle.h:82
vpMbTracker::displayFeatures
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:143
vpMbtDistanceCylinder::p1
vpPoint * p1
The first extremity on the axe.
Definition: vpMbtDistanceCylinder.h:93
vpMbHiddenFaces::setBackgroundSizeOgre
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
Definition: vpMbHiddenFaces.h:210
vpMbTracker::getNbPolygon
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:367
vpMbEdgeTracker::addCylinder
void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace=-1, const std::string &name="")
Definition: vpMbEdgeTracker.cpp:1970
vpMbtDistanceCylinder::setMeanWeight1
void setMeanWeight1(const double wmean)
Definition: vpMbtDistanceCylinder.h:238
vpMbTracker::oJo
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:120
vpMbEdgeTracker::initFaceFromCorners
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
Definition: vpMbEdgeTracker.cpp:2121
vpMbTracker::applyLodSettingInConfig
bool applyLodSettingInConfig
Definition: vpMbTracker.h:180
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:177
vpMbXmlParser::hasNearClippingDistance
bool hasNearClippingDistance() const
Definition: vpMbXmlParser.h:188
vpMbEdgeTracker::nbFeaturesForProjErrorComputation
unsigned int nbFeaturesForProjErrorComputation
Number of features used in the computation of the projection error.
Definition: vpMbEdgeTracker.h:321
vpMbtDistanceCylinder::setVisible
void setVisible(bool _isvisible)
Definition: vpMbtDistanceCylinder.h:269
vpMbEdgeTracker::computeProjectionError
void computeProjectionError(const vpImage< unsigned char > &_I)
Definition: vpMbEdgeTracker.cpp:867
vpMbtDistanceCircle::index_polygon
int index_polygon
Index of the faces which contain the line.
Definition: vpMbtDistanceCircle.h:102
vpMbEdgeTracker::initFaceFromLines
virtual void initFaceFromLines(vpMbtPolygon &polygon)
Definition: vpMbEdgeTracker.cpp:2145
vpMeSite
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:70
vpMbtDistanceLine::p2
vpPoint * p2
The second extremity.
Definition: vpMbtDistanceLine.h:90
vpMbEdgeTracker::updateMovingEdgeWeights
void updateMovingEdgeWeights()
Definition: vpMbEdgeTracker.cpp:1578
vpMbEdgeTracker::cylinders
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
Definition: vpMbEdgeTracker.h:287
vpMbtDistanceCircle::isVisible
bool isVisible() const
Definition: vpMbtDistanceCircle.h:192
vpMbtDistanceCylinder::index_polygon
int index_polygon
Index of the face which contains the cylinder.
Definition: vpMbtDistanceCylinder.h:115
vpMbtDistanceLine::updateTracked
void updateTracked()
Definition: vpMbtDistanceLine.cpp:258
vpMbEdgeTracker::computeVVSInit
virtual void computeVVSInit()
Definition: vpMbEdgeTracker.cpp:727
vpMbEdgeTracker::ncylinder
unsigned int ncylinder
Definition: vpMbEdgeTracker.h:299
vpMbEdgeTracker::addCircle
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace=-1, const std::string &name="")
Definition: vpMbEdgeTracker.cpp:1911
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:113
vpMbTracker::minLineLengthThresholdGeneral
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:182
vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation
void computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
Definition: vpMbEdgeTracker.cpp:658
vpMbtDistanceCircle::L
vpMatrix L
The interaction matrix.
Definition: vpMbtDistanceCircle.h:92
vpMbEdgeTracker::displayFeaturesOnImage
void displayFeaturesOnImage(const vpImage< unsigned char > &I, const unsigned int lvl)
Definition: vpMbEdgeTracker.cpp:1348
vpMath::minimum
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:144
vpMbTracker::angleAppears
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:150
vpMbTracker::minPolygonAreaThresholdGeneral
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:184
vpMbEdgeTracker::initPyramid
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
Definition: vpMbEdgeTracker.cpp:2585
vpMbEdgeTracker::Ipyramid
std::vector< const vpImage< unsigned char > * > Ipyramid
Definition: vpMbEdgeTracker.h:313
vpMbTracker::computeVVSPoseEstimation
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, const unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
Definition: vpMbTracker.cpp:2546
vpMbtXmlParser
Parse an Xml file to extract configuration parameters of a mbtConfig object.
Definition: vpMbtXmlParser.h:67
vpMbtDistanceCylinder::setName
void setName(const std::string &cyl_name)
Definition: vpMbtDistanceCylinder.h:254
vpMbTracker::m_SobelY
vpMatrix m_SobelY
Sobel kernel in Y.
Definition: vpMbTracker.h:216
vpMbTracker::m_SobelX
vpMatrix m_SobelX
Sobel kernel in X.
Definition: vpMbTracker.h:214
vpExponentialMap::direct
static vpHomogeneousMatrix direct(const vpColVector &v)
Definition: vpExponentialMap.cpp:58
vpVelocityTwistMatrix::buildFrom
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition: vpVelocityTwistMatrix.cpp:364
vpPolygon3D::getNbPoint
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:131
vpMbtDistanceLine
Manage the line of a polygon used in the model-based tracker.
Definition: vpMbtDistanceLine.h:65
vpERROR_TRACE
#define vpERROR_TRACE
Definition: vpDebug.h:392
vpRobust::TUKEY
Definition: vpRobust.h:62
vpMbTracker::m_mask
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:226
vpMbHiddenFaces::initOgre
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
Definition: vpMbHiddenFaces.h:710
vpMbXmlParser::getAngleDisappear
double getAngleDisappear() const
Definition: vpMbXmlParser.h:130
vpMbtDistanceCircle::displayMovingEdges
void displayMovingEdges(const vpImage< unsigned char > &I)
Definition: vpMbtDistanceCircle.cpp:338
vpMbtDistanceLine::initInteractionMatrixError
void initInteractionMatrixError()
Definition: vpMbtDistanceLine.cpp:734
vpRobust::resize
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:127
vpMbTracker::isoJoIdentity
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:122
vpMbEdgeTracker::testTracking
void testTracking()
Definition: vpMbEdgeTracker.cpp:944
vpMbTracker::m_projectionErrorDisplayThickness
unsigned int m_projectionErrorDisplayThickness
Thickness of the arrows used to show the gradient and model orientation.
Definition: vpMbTracker.h:222
vpMbEdgeTracker::m_errorCylinders
vpColVector m_errorCylinders
Definition: vpMbEdgeTracker.h:332
vpMbtDistanceCircle::setName
void setName(const std::string &circle_name)
Definition: vpMbtDistanceCircle.h:231
vpMbTracker::setClipping
virtual void setClipping(const unsigned int &flags)
Definition: vpMbTracker.cpp:2437
vpMbTracker::m_maxIter
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:194
vpMbTracker::ogreShowConfigDialog
bool ogreShowConfigDialog
Definition: vpMbTracker.h:161
vpMbtDistanceCylinder::setCameraParameters
void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbtDistanceCylinder.h:217
vpMbEdgeTracker::nline
unsigned int nline
Definition: vpMbEdgeTracker.h:291
vpMbEdgeTracker::m_L_edge
vpMatrix m_L_edge
Interaction matrix.
Definition: vpMbEdgeTracker.h:335
vpMbtDistanceCylinder::setMeanWeight2
void setMeanWeight2(const double wmean)
Definition: vpMbtDistanceCylinder.h:245
vpMbtDistanceCylinder::nbFeaturel2
unsigned int nbFeaturel2
The number of moving edges on line 2.
Definition: vpMbtDistanceCylinder.h:106
vpMbEdgeTracker::getLcylinder
void getLcylinder(std::list< vpMbtDistanceCylinder * > &cylindersList, const unsigned int level=0) const
Definition: vpMbEdgeTracker.cpp:2676
vpMbTracker::clippingFlag
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:158
vpMbtDistanceLine::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceLine.cpp:754
vpMbEdgeTracker::setUseEdgeTracking
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
Definition: vpMbEdgeTracker.cpp:2803
vpMbtDistanceLine::hiddenface
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
Definition: vpMbtDistanceLine.h:103
vpMbEdgeTracker::circles
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
Definition: vpMbEdgeTracker.h:284
vpMeSite::setState
void setState(const vpMeSiteState &flag)
Definition: vpMeSite.h:173
vpPolygon3D::setClipping
void setClipping(const unsigned int &flags)
Definition: vpPolygon3D.h:186
vpMeSite::getState
vpMeSiteState getState() const
Definition: vpMeSite.h:187
vpMbEdgeTracker::vpMbEdgeTracker
vpMbEdgeTracker()
Definition: vpMbEdgeTracker.cpp:68
vpMbHiddenFaces::getOgreContext
vpAROgre * getOgreContext()
Definition: vpMbHiddenFaces.h:154
vpMbEdgeTracker::addLine
void addLine(vpPoint &p1, vpPoint &p2, int polygon=-1, std::string name="")
Definition: vpMbEdgeTracker.cpp:1828
vpMbTracker::setNearClippingDistance
virtual void setNearClippingDistance(const double &dist)
Definition: vpMbTracker.cpp:2410
vpMbTracker::setFarClippingDistance
virtual void setFarClippingDistance(const double &dist)
Definition: vpMbTracker.cpp:2331
vpMbTracker::computeCovarianceMatrixVVS
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
Definition: vpMbTracker.cpp:2444
vpMatrix::pseudoInverse
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1931
vpHomogeneousMatrix::inverse
vpHomogeneousMatrix inverse() const
Definition: vpHomogeneousMatrix.cpp:642
vpImage< unsigned char >
vpMbtDistanceLine::setName
void setName(const std::string &line_name)
Definition: vpMbtDistanceLine.h:234
vpMbXmlParser::getNearClippingDistance
double getNearClippingDistance() const
Definition: vpMbXmlParser.h:174
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpArray2D::resize
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:170
vpMbtDistanceCircle::computeInteractionMatrixError
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCircle.cpp:364
vpMbtDistanceLine::addPolygon
void addPolygon(const int &index)
Definition: vpMbtDistanceLine.cpp:215
vpMbtDistanceCircle::initInteractionMatrixError
void initInteractionMatrixError()
Definition: vpMbtDistanceCircle.cpp:350
vpColor
Class to define colors available for display functionnalities.
Definition: vpColor.h:119
vpMbtDistanceLine::isVisible
bool isVisible() const
Definition: vpMbtDistanceLine.h:202
vpMbtDistanceLine::closeToImageBorder
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
Definition: vpMbtDistanceLine.cpp:829
vpMbHiddenFaces::setVisible
unsigned int setVisible(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
Definition: vpMbHiddenFaces.h:659
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpMbtDistanceCylinder::trackMovingEdge
void trackMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
Definition: vpMbtDistanceCylinder.cpp:322
vpMbEdgeTracker::display
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, const unsigned int thickness=1, const bool displayFullModel=false)
Definition: vpMbEdgeTracker.cpp:1276
vpMbtDistanceCylinder::setIndex
void setIndex(const unsigned int i)
Definition: vpMbtDistanceCylinder.h:231
vpPolygon3D::setNearClippingDistance
void setNearClippingDistance(const double &dist)
Definition: vpPolygon3D.h:206
vpMbtDistanceCircle::error
vpColVector error
The error vector.
Definition: vpMbtDistanceCircle.h:94
vpMath::maximum
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:136
vpMbHiddenFaces::computeScanLineRender
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
Definition: vpMbHiddenFaces.h:463
vpMbtDistanceLine::setMovingEdge
void setMovingEdge(vpMe *Me)
Definition: vpMbtDistanceLine.cpp:281
vpMbtXmlParser::getMe
void getMe(vpMe &_ecm) const
Definition: vpMbtXmlParser.h:95
vpMbEdgeTracker::getNbPoints
virtual unsigned int getNbPoints(const unsigned int level=0) const
Definition: vpMbEdgeTracker.cpp:2372
vpMbtDistanceCircle::setVisible
void setVisible(bool _isvisible)
Definition: vpMbtDistanceCircle.h:246
vpTRACE
#define vpTRACE
Definition: vpDebug.h:415
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpCameraParameters::computeFov
void computeFov(const unsigned int &w, const unsigned int &h)
Definition: vpCameraParameters.cpp:432
vpMbtDistanceCircle::setMeanWeight
void setMeanWeight(const double _wmean)
Definition: vpMbtDistanceCircle.h:222
vpPolygon3D::NO_CLIPPING
Definition: vpPolygon3D.h:62
vpMbtDistanceCylinder::initInteractionMatrixError
void initInteractionMatrixError()
Definition: vpMbtDistanceCylinder.cpp:662
vpMbtDistanceCylinder::initMovingEdge
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const bool doNotTrack, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceCylinder.cpp:183
vpMbtDistanceCircle::Reinit
bool Reinit
Indicates if the circle has to be reinitialized.
Definition: vpMbtDistanceCircle.h:98
vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu
virtual void computeVVSInteractionMatrixAndResidu()
Definition: vpMbEdgeTracker.cpp:770
vpMbtDistanceLine::reinitMovingEdge
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
Definition: vpMbtDistanceLine.cpp:584
vpMbtDistanceCircle::setCameraParameters
void setCameraParameters(const vpCameraParameters &camera)
Definition: vpMbtDistanceCircle.h:201
vpCameraParameters::initFromCalibrationMatrix
void initFromCalibrationMatrix(const vpMatrix &_K)
Definition: vpCameraParameters.cpp:298
vpMbTracker::angleDisappears
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:152
vpMbTracker::computeProjError
bool computeProjError
Definition: vpMbTracker.h:138
vpPolygon3D::NEAR_CLIPPING
Definition: vpPolygon3D.h:63
vpMbEdgeTracker::lines
std::vector< std::list< vpMbtDistanceLine * > > lines
Definition: vpMbEdgeTracker.h:281
vpMbEdgeTracker::setPose
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
Definition: vpMbEdgeTracker.cpp:1189
vpMbXmlParser::getMinLineLengthThreshold
double getMinLineLengthThreshold() const
Definition: vpMbXmlParser.h:160
vpMbTracker::useLodGeneral
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:177
vpMbEdgeTracker::m_wLines
vpColVector m_wLines
Definition: vpMbEdgeTracker.h:328
vpMbTracker::samePoint
bool samePoint(const vpPoint &P1, const vpPoint &P2) const
Definition: vpMbTracker.cpp:2764
vpMatrix::eye
void eye()
Definition: vpMatrix.cpp:359
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:238
vpMbtDistanceLine::setMeanWeight
void setMeanWeight(const double w_mean)
Definition: vpMbtDistanceLine.h:225
vpMbtDistanceCircle::p2
vpPoint * p2
A point on the plane containing the circle.
Definition: vpMbtDistanceCircle.h:87
vpPolygon3D::FAR_CLIPPING
Definition: vpPolygon3D.h:64
vpMbXmlParser::setAngleDisappear
void setAngleDisappear(const double &adisappear)
Definition: vpMbXmlParser.h:207
vpArray2D::getRows
unsigned int getRows() const
Definition: vpArray2D.h:155