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