Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbEdgeTracker.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
41#include <visp3/core/vpDebug.h>
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpExponentialMap.h>
44#include <visp3/core/vpMath.h>
45#include <visp3/core/vpMatrixException.h>
46#include <visp3/core/vpPixelMeterConversion.h>
47#include <visp3/core/vpPolygon3D.h>
48#include <visp3/core/vpTrackingException.h>
49#include <visp3/core/vpVelocityTwistMatrix.h>
50#include <visp3/mbt/vpMbEdgeTracker.h>
51#include <visp3/mbt/vpMbtDistanceLine.h>
52#include <visp3/mbt/vpMbtXmlGenericParser.h>
53#include <visp3/vision/vpPose.h>
54
55#include <float.h>
56#include <limits>
57#include <map>
58#include <sstream>
59#include <string>
60
65 : me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0), nbvisiblepolygone(0),
66 percentageGdPt(0.4), scales(1), Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0), m_factor(),
67 m_robustLines(), m_robustCylinders(), m_robustCircles(), m_wLines(), m_wCylinders(), m_wCircles(), m_errorLines(),
68 m_errorCylinders(), m_errorCircles(), m_L_edge(), m_error_edge(), m_w_edge(), m_weightedError_edge(),
69 m_robust_edge(), m_featuresToBeDisplayedEdge()
70{
71 scales[0] = true;
72
73#ifdef VISP_HAVE_OGRE
74 faces.getOgreContext()->setWindowName("MBT Edge");
75#endif
76}
77
82{
86
87 for (unsigned int i = 0; i < scales.size(); i += 1) {
88 if (scales[i]) {
89 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
90 l = *it;
91 if (l != NULL) {
92 delete l;
93 }
94 l = NULL;
95 }
96
97 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
98 ++it) {
99 cy = *it;
100 if (cy != NULL) {
101 delete cy;
102 }
103 cy = NULL;
104 }
105
106 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
107 ci = *it;
108 if (ci != NULL) {
109 delete ci;
110 }
111 ci = NULL;
112 }
113
114 lines[i].clear();
115 cylinders[i].clear();
116 circles[i].clear();
117 }
118 }
119
121}
122
129{
130 this->me = p_me;
131
132 for (unsigned int i = 0; i < scales.size(); i += 1) {
133 if (scales[i]) {
134 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
135 vpMbtDistanceLine *l = *it;
136 l->setMovingEdge(&(this->me));
137 }
138
139 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
140 ++it) {
141 vpMbtDistanceCylinder *cy = *it;
142 cy->setMovingEdge(&(this->me));
143 }
144
145 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
146 vpMbtDistanceCircle *ci = *it;
147 ci->setMovingEdge(&(this->me));
148 }
149 }
150 }
151}
152
163{
164 double residu_1 = 1e3;
165 double r = 1e3 - 1;
166
167 unsigned int iter = 0;
168
170 unsigned int nbrow = m_error_edge.getRows();
171
172 bool reloop = true;
173
174 bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
175 if (isoJoIdentity)
176 oJo.eye();
177
178 /*** First phase ***/
179
180 while (reloop == true && iter < 10) {
181 double count = 0;
182
183 computeVVSFirstPhase(_I, iter, count, lvl);
184
185 count = count / (double)nbrow;
186 if (count >= 0.85) {
187 reloop = false;
188 }
189
190 computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity);
191
192 iter++;
193 }
194
195 // std::cout << "\t First minimization in " << iter << " iteration give as
196 // initial cMo: \n" << cMo << std::endl;
197
198 /*** Second phase ***/
199 vpHomogeneousMatrix cMoPrev;
200 vpColVector W_true(nbrow);
201 vpMatrix L_true;
202 vpMatrix LVJ_true;
203
204 double mu = m_initialMu;
205 vpColVector m_error_prev;
206 vpColVector m_w_prev;
207
208 // To avoid to create these matrices each iteration
209 vpMatrix LTL;
210 vpColVector LTR;
211 vpColVector v;
212
213 iter = 0;
214 m_w_edge = 1;
215
216 // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
217 while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
219
220 bool reStartFromLastIncrement = false;
221 computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
222 &m_w_edge, &m_w_prev);
223
224 if (!reStartFromLastIncrement) {
226
227 L_true = m_L_edge;
229
230 if (computeCovariance) {
231 L_true = m_L_edge;
232 if (!isoJoIdentity) {
233 cVo.buildFrom(m_cMo);
234 LVJ_true = (m_L_edge * cVo * oJo);
235 }
236 }
237
238 double wi = 0.0, eri = 0.0;
239 double num = 0.0, den = 0.0;
240 if ((iter == 0) || m_computeInteraction) {
241 for (unsigned int i = 0; i < nbrow; i++) {
242 wi = m_w_edge[i] * m_factor[i];
243 W_true[i] = wi;
244 eri = m_error_edge[i];
245 num += wi * vpMath::sqr(eri);
246 den += wi;
247
248 m_weightedError_edge[i] = wi * eri;
249
250 for (unsigned int j = 0; j < 6; j++) {
251 m_L_edge[i][j] = wi * m_L_edge[i][j];
252 }
253 }
254 } else {
255 for (unsigned int i = 0; i < nbrow; i++) {
256 wi = m_w_edge[i] * m_factor[i];
257 W_true[i] = wi;
258 eri = m_error_edge[i];
259 num += wi * vpMath::sqr(eri);
260 den += wi;
261
262 m_weightedError_edge[i] = wi * eri;
263 }
264 }
265
266 residu_1 = r;
267 r = sqrt(num / den); // Le critere d'arret prend en compte le poids
268
269 computeVVSPoseEstimation(isoJoIdentity, iter, m_L_edge, LTL, m_weightedError_edge, m_error_edge, m_error_prev,
270 LTR, mu, v, &m_w_edge, &m_w_prev);
271
272 cMoPrev = m_cMo;
274
275 } // endif(!restartFromLast)
276
277 iter++;
278 }
279
280 computeCovarianceMatrixVVS(isoJoIdentity, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);
281
283}
284
285void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_I, unsigned int iter, double &count,
286 unsigned int lvl)
287{
291
292 double limite = 3; // Une limite de 3 pixels
293 limite = limite / m_cam.get_px(); // Transformation limite pixel en limite metre.
294
295 unsigned int n = 0;
296
297 // Parametre pour la premiere phase d'asservissement
298 double e_prev = 0, e_cur, e_next;
299
300 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
301 if ((*it)->isTracked()) {
302 l = *it;
304
305 double fac = 1;
306 if (iter == 0) {
307 for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
308 ++itindex) {
309 int index = *itindex;
310 if (l->hiddenface->isAppearing((unsigned int)index)) {
311 fac = 0.2;
312 break;
313 }
314 if (l->closeToImageBorder(_I, 10)) {
315 fac = 0.1;
316 break;
317 }
318 }
319 }
320
321 std::list<vpMeSite>::const_iterator itListLine;
322
323 unsigned int indexFeature = 0;
324
325 for (size_t a = 0; a < l->meline.size(); a++) {
326 if (iter == 0 && l->meline[a] != NULL)
327 itListLine = l->meline[a]->getMeList().begin();
328
329 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
330 for (unsigned int j = 0; j < 6; j++) {
331 m_L_edge[n + i][j] = l->L[indexFeature][j]; // On remplit la matrice d'interaction globale
332 }
333 m_error_edge[n + i] = l->error[indexFeature]; // On remplit la matrice d'erreur
334
335 if (m_error_edge[n + i] <= limite)
336 count = count + 1.0; // Si erreur proche de 0 on incremente cur
337
338 m_w_edge[n + i] = 0;
339
340 if (iter == 0) {
341 m_factor[n + i] = fac;
342 vpMeSite site = *itListLine;
344 m_factor[n + i] = 0.2;
345 ++itListLine;
346 }
347
348 // If pour la premiere extremite des moving edges
349 if (indexFeature == 0) {
350 e_cur = l->error[0];
351 if (l->nbFeature[a] > 1) {
352 e_next = l->error[1];
353 if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
354 m_w_edge[n + i] = 1 /*0.5*/;
355 }
356 e_prev = e_cur;
357 } else
358 m_w_edge[n + i] = 1;
359 }
360
361 // If pour la derniere extremite des moving edges
362 else if (indexFeature == l->nbFeatureTotal - 1) {
363 e_cur = l->error[indexFeature];
364 if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
365 m_w_edge[n + i] += 1 /*0.5*/;
366 }
367 }
368
369 else {
370 e_cur = l->error[indexFeature];
371 e_next = l->error[indexFeature + 1];
372 if (fabs(e_cur - e_prev) < limite) {
373 m_w_edge[n + i] += 0.5;
374 }
375 if (fabs(e_cur - e_next) < limite) {
376 m_w_edge[n + i] += 0.5;
377 }
378 e_prev = e_cur;
379 }
380 indexFeature++;
381 }
382 n += l->nbFeature[a];
383 }
384 }
385 }
386
387 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
388 ++it) {
389 if ((*it)->isTracked()) {
390 cy = *it;
392 double fac = 1.0;
393
394 std::list<vpMeSite>::const_iterator itCyl1;
395 std::list<vpMeSite>::const_iterator itCyl2;
396 if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)) {
397 itCyl1 = cy->meline1->getMeList().begin();
398 itCyl2 = cy->meline2->getMeList().begin();
399 }
400
401 for (unsigned int i = 0; i < cy->nbFeature; i++) {
402 for (unsigned int j = 0; j < 6; j++) {
403 m_L_edge[n + i][j] = cy->L[i][j]; // On remplit la matrice d'interaction globale
404 }
405 m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur
406
407 if (m_error_edge[n + i] <= limite)
408 count = count + 1.0; // Si erreur proche de 0 on incremente cur
409
410 m_w_edge[n + i] = 0;
411
412 if (iter == 0) {
413 m_factor[n + i] = fac;
414 vpMeSite site;
415 if (i < cy->nbFeaturel1) {
416 site = *itCyl1;
417 ++itCyl1;
418 } else {
419 site = *itCyl2;
420 ++itCyl2;
421 }
423 m_factor[n + i] = 0.2;
424 }
425
426 // If pour la premiere extremite des moving edges
427 if (i == 0) {
428 e_cur = cy->error[0];
429 if (cy->nbFeature > 1) {
430 e_next = cy->error[1];
431 if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
432 m_w_edge[n + i] = 1 /*0.5*/;
433 }
434 e_prev = e_cur;
435 } else
436 m_w_edge[n + i] = 1;
437 }
438 if (i == cy->nbFeaturel1) {
439 e_cur = cy->error[i];
440 if (cy->nbFeaturel2 > 1) {
441 e_next = cy->error[i + 1];
442 if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
443 m_w_edge[n + i] = 1 /*0.5*/;
444 }
445 e_prev = e_cur;
446 } else
447 m_w_edge[n + i] = 1;
448 }
449
450 // If pour la derniere extremite des moving edges
451 else if (i == cy->nbFeaturel1 - 1) {
452 e_cur = cy->error[i];
453 if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
454 m_w_edge[n + i] += 1 /*0.5*/;
455 }
456 }
457 // If pour la derniere extremite des moving edges
458 else if (i == cy->nbFeature - 1) {
459 e_cur = cy->error[i];
460 if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
461 m_w_edge[n + i] += 1 /*0.5*/;
462 }
463 }
464
465 else {
466 e_cur = cy->error[i];
467 e_next = cy->error[i + 1];
468 if (fabs(e_cur - e_prev) < limite) {
469 m_w_edge[n + i] += 0.5;
470 }
471 if (fabs(e_cur - e_next) < limite) {
472 m_w_edge[n + i] += 0.5;
473 }
474 e_prev = e_cur;
475 }
476 }
477
478 n += cy->nbFeature;
479 }
480 }
481
482 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
483 if ((*it)->isTracked()) {
484 ci = *it;
486 double fac = 1.0;
487
488 std::list<vpMeSite>::const_iterator itCir;
489 if (iter == 0 && (ci->meEllipse != NULL)) {
490 itCir = ci->meEllipse->getMeList().begin();
491 }
492
493 for (unsigned int i = 0; i < ci->nbFeature; i++) {
494 for (unsigned int j = 0; j < 6; j++) {
495 m_L_edge[n + i][j] = ci->L[i][j]; // On remplit la matrice d'interaction globale
496 }
497 m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur
498
499 if (m_error_edge[n + i] <= limite)
500 count = count + 1.0; // Si erreur proche de 0 on incremente cur
501
502 m_w_edge[n + i] = 0;
503
504 if (iter == 0) {
505 m_factor[n + i] = fac;
506 vpMeSite site = *itCir;
508 m_factor[n + i] = 0.2;
509 ++itCir;
510 }
511
512 // If pour la premiere extremite des moving edges
513 if (i == 0) {
514 e_cur = ci->error[0];
515 if (ci->nbFeature > 1) {
516 e_next = ci->error[1];
517 if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
518 m_w_edge[n + i] = 1 /*0.5*/;
519 }
520 e_prev = e_cur;
521 } else
522 m_w_edge[n + i] = 1;
523 }
524
525 // If pour la derniere extremite des moving edges
526 else if (i == ci->nbFeature - 1) {
527 e_cur = ci->error[i];
528 if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
529 m_w_edge[n + i] += 1 /*0.5*/;
530 }
531 }
532
533 else {
534 e_cur = ci->error[i];
535 e_next = ci->error[i + 1];
536 if (fabs(e_cur - e_prev) < limite) {
537 m_w_edge[n + i] += 0.5;
538 }
539 if (fabs(e_cur - e_next) < limite) {
540 m_w_edge[n + i] += 0.5;
541 }
542 e_prev = e_cur;
543 }
544 }
545
546 n += ci->nbFeature;
547 }
548 }
549}
550
552{
556
557 unsigned int n = 0;
558 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
559 if ((*it)->isTracked()) {
560 l = *it;
562
563 double fac = 1;
564 for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
565 ++itindex) {
566 int index = *itindex;
567 if (l->hiddenface->isAppearing((unsigned int)index)) {
568 fac = 0.2;
569 break;
570 }
571 if (l->closeToImageBorder(I, 10)) {
572 fac = 0.1;
573 break;
574 }
575 }
576
577 for (size_t a = 0; a < l->meline.size(); a++) {
578 std::list<vpMeSite>::const_iterator itListLine;
579 if (l->meline[a] != NULL) {
580 itListLine = l->meline[a]->getMeList().begin();
581
582 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
583 m_factor[n + i] = fac;
584 vpMeSite site = *itListLine;
586 m_factor[n + i] = 0.2;
587 ++itListLine;
588 }
589 n += l->nbFeature[a];
590 }
591 }
592 }
593 }
594
595 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
596 ++it) {
597 if ((*it)->isTracked()) {
598 cy = *it;
600
601 std::list<vpMeSite>::const_iterator itCyl1;
602 std::list<vpMeSite>::const_iterator itCyl2;
603 if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
604 itCyl1 = cy->meline1->getMeList().begin();
605 itCyl2 = cy->meline2->getMeList().begin();
606
607 double fac = 1.0;
608 for (unsigned int i = 0; i < cy->nbFeature; i++) {
609 m_factor[n + i] = fac;
610 vpMeSite site;
611 if (i < cy->nbFeaturel1) {
612 site = *itCyl1;
613 ++itCyl1;
614 } else {
615 site = *itCyl2;
616 ++itCyl2;
617 }
619 m_factor[n + i] = 0.2;
620 }
621 n += cy->nbFeature;
622 }
623 }
624 }
625
626 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
627 if ((*it)->isTracked()) {
628 ci = *it;
630
631 std::list<vpMeSite>::const_iterator itCir;
632 if (ci->meEllipse != NULL) {
633 itCir = ci->meEllipse->getMeList().begin();
634 double fac = 1.0;
635
636 for (unsigned int i = 0; i < ci->nbFeature; i++) {
637 m_factor[n + i] = fac;
638 vpMeSite site = *itCir;
640 m_factor[n + i] = 0.2;
641 ++itCir;
642 }
643 n += ci->nbFeature;
644 }
645 }
646 }
647}
648
649void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
650{
651 unsigned int nerror = m_weightedError_edge.getRows();
652
653 double wi, eri;
654 if ((iter == 0) || m_computeInteraction) {
655 for (unsigned int i = 0; i < nerror; i++) {
656 wi = m_w_edge[i] * m_factor[i];
657 eri = m_error_edge[i];
658
659 m_weightedError_edge[i] = wi * eri;
660
661 for (unsigned int j = 0; j < 6; j++) {
662 m_L_edge[i][j] = wi * m_L_edge[i][j];
663 }
664 }
665 } else {
666 for (unsigned int i = 0; i < nerror; i++) {
667 wi = m_w_edge[i] * m_factor[i];
668 eri = m_error_edge[i];
669
670 m_weightedError_edge[i] = wi * eri;
671 }
672 }
673
675
676 // If all the 6 dof should be estimated, we check if the interaction matrix
677 // is full rank. If not we remove automatically the dof that cannot be
678 // estimated This is particularly useful when considering circles (rank 5) and
679 // cylinders (rank 4)
680 if (isoJoIdentity) {
681 cVo.buildFrom(m_cMo);
682
683 vpMatrix K; // kernel
684 unsigned int rank = (m_L_edge * cVo).kernel(K);
685 if (rank == 0) {
686 throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
687 }
688 if (rank != 6) {
689 vpMatrix I; // Identity
690 I.eye(6);
691 oJo = I - K.AtA();
692
693 isoJoIdentity = false;
694 }
695 }
696
697 vpColVector v;
698 vpMatrix LTL;
699 vpColVector LTR;
700
701 if (isoJoIdentity) {
702 LTL = m_L_edge.AtA();
704 v = -0.7 * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
705 } else {
706 cVo.buildFrom(m_cMo);
707 vpMatrix LVJ = (m_L_edge * cVo * oJo);
708 vpMatrix LVJTLVJ = (LVJ).AtA();
709 vpColVector LVJTR;
710 computeJTR(LVJ, m_weightedError_edge, LVJTR);
711 v = -0.7 * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
712 v = cVo * v;
713 }
714
716}
717
719{
720 // Nombre de moving edges
721 unsigned int nbrow = 0;
722 unsigned int nberrors_lines = 0;
723 unsigned int nberrors_cylinders = 0;
724 unsigned int nberrors_circles = 0;
725
726 nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
727
728 if (nbrow == 0) {
730 "No data found to compute the interaction matrix...");
731 }
732
733 m_L_edge.resize(nbrow, 6, false, false);
734 m_error_edge.resize(nbrow, false);
735
736 m_weightedError_edge.resize(nbrow, false);
737 m_w_edge.resize(nbrow, false);
738 m_w_edge = 1;
739 m_factor.resize(nbrow, false);
740 m_factor = 1;
741
745
746 m_wLines.resize(nberrors_lines, false);
747 m_wLines = 1;
748 m_wCylinders.resize(nberrors_cylinders, false);
749 m_wCylinders = 1;
750 m_wCircles.resize(nberrors_circles, false);
751 m_wCircles = 1;
752
753 m_errorLines.resize(nberrors_lines, false);
754 m_errorCylinders.resize(nberrors_cylinders, false);
755 m_errorCircles.resize(nberrors_circles, false);
756}
757
759{
760 throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
761 "computeVVSInteractionMatrixAndR"
762 "esidu() should not be called!");
763}
764
766{
770
771 unsigned int n = 0;
772 unsigned int nlines = 0;
773 unsigned int ncylinders = 0;
774 unsigned int ncircles = 0;
775
776 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
777 ++it) {
778 if ((*it)->isTracked()) {
779 l = *it;
781 for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
782 for (unsigned int j = 0; j < 6; j++) {
783 m_L_edge[n + i][j] = l->L[i][j];
784 m_error_edge[n + i] = l->error[i];
785 m_errorLines[nlines + i] = m_error_edge[n + i];
786 }
787 }
788 n += l->nbFeatureTotal;
789 nlines += l->nbFeatureTotal;
790 }
791 }
792
793 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
794 it != cylinders[scaleLevel].end(); ++it) {
795 if ((*it)->isTracked()) {
796 cy = *it;
798 for (unsigned int i = 0; i < cy->nbFeature; i++) {
799 for (unsigned int j = 0; j < 6; j++) {
800 m_L_edge[n + i][j] = cy->L[i][j];
801 m_error_edge[n + i] = cy->error[i];
802 m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
803 }
804 }
805
806 n += cy->nbFeature;
807 ncylinders += cy->nbFeature;
808 }
809 }
810
811 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
812 it != circles[scaleLevel].end(); ++it) {
813 if ((*it)->isTracked()) {
814 ci = *it;
816 for (unsigned int i = 0; i < ci->nbFeature; i++) {
817 for (unsigned int j = 0; j < 6; j++) {
818 m_L_edge[n + i][j] = ci->L[i][j];
819 m_error_edge[n + i] = ci->error[i];
820 m_errorCircles[ncircles + i] = m_error_edge[n + i];
821 }
822 }
823
824 n += ci->nbFeature;
825 ncircles += ci->nbFeature;
826 }
827 }
828}
829
831{
832 unsigned int nberrors_lines = m_errorLines.getRows(), nberrors_cylinders = m_errorCylinders.getRows(),
833 nberrors_circles = m_errorCircles.getRows();
834
835 if (nberrors_lines > 0)
837 if (nberrors_cylinders > 0)
839 if (nberrors_circles > 0)
841
845}
846
856{
857 projectionError = 0.0;
858 unsigned int nbFeatures = 0;
859 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
860 ++it) {
861 vpMbtDistanceLine *l = *it;
862 if (l->isVisible() && l->isTracked()) {
863 for (size_t a = 0; a < l->meline.size(); a++) {
864 if (l->meline[a] != NULL) {
865 double lineNormGradient;
866 unsigned int lineNbFeatures;
867 l->meline[a]->computeProjectionError(_I, lineNormGradient, lineNbFeatures, m_SobelX, m_SobelY,
870 projectionError += lineNormGradient;
871 nbFeatures += lineNbFeatures;
872 }
873 }
874 }
875 }
876
877 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
878 it != cylinders[scaleLevel].end(); ++it) {
879 vpMbtDistanceCylinder *cy = *it;
880 if (cy->isVisible() && cy->isTracked()) {
881 if (cy->meline1 != NULL) {
882 double cylinderNormGradient = 0;
883 unsigned int cylinderNbFeatures = 0;
884 cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
887 projectionError += cylinderNormGradient;
888 nbFeatures += cylinderNbFeatures;
889 }
890
891 if (cy->meline2 != NULL) {
892 double cylinderNormGradient = 0;
893 unsigned int cylinderNbFeatures = 0;
894 cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures, m_SobelX, m_SobelY,
897 projectionError += cylinderNormGradient;
898 nbFeatures += cylinderNbFeatures;
899 }
900 }
901 }
902
903 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
904 it != circles[scaleLevel].end(); ++it) {
905 vpMbtDistanceCircle *c = *it;
906 if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
907 double circleNormGradient = 0;
908 unsigned int circleNbFeatures = 0;
909 c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures, m_SobelX, m_SobelY,
912 projectionError += circleNormGradient;
913 nbFeatures += circleNbFeatures;
914 }
915 }
916
917 if (nbFeatures > 0) {
918 projectionError = vpMath::deg(projectionError / (double)nbFeatures);
919 } else {
920 projectionError = 90.0;
921 }
922
924 // std::cout << "Norm Gradient = " << errorGradient << std::endl;
925}
926
933{
934 int nbExpectedPoint = 0;
935 int nbGoodPoint = 0;
936 int nbBadPoint = 0;
937
938 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
939 ++it) {
940 vpMbtDistanceLine *l = *it;
941 if (l->isVisible() && l->isTracked()) {
942 for (size_t a = 0; a < l->meline.size(); a++) {
943 if (l->meline[a] != NULL) {
944 nbExpectedPoint += (int)l->meline[a]->expecteddensity;
945 for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
946 itme != l->meline[a]->getMeList().end(); ++itme) {
947 vpMeSite pix = *itme;
949 nbGoodPoint++;
950 else
951 nbBadPoint++;
952 }
953 }
954 }
955 }
956 }
957
958 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
959 it != cylinders[scaleLevel].end(); ++it) {
960 vpMbtDistanceCylinder *cy = *it;
961 if ((cy->meline1 != NULL && cy->meline2 != NULL) && cy->isVisible() && cy->isTracked()) {
962 nbExpectedPoint += (int)cy->meline1->expecteddensity;
963 for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
964 itme1 != cy->meline1->getMeList().end(); ++itme1) {
965 vpMeSite pix = *itme1;
967 nbGoodPoint++;
968 else
969 nbBadPoint++;
970 }
971 nbExpectedPoint += (int)cy->meline2->expecteddensity;
972 for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
973 itme2 != cy->meline2->getMeList().end(); ++itme2) {
974 vpMeSite pix = *itme2;
976 nbGoodPoint++;
977 else
978 nbBadPoint++;
979 }
980 }
981 }
982
983 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
984 it != circles[scaleLevel].end(); ++it) {
985 vpMbtDistanceCircle *ci = *it;
986 if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
987 nbExpectedPoint += ci->meEllipse->getExpectedDensity();
988 for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
989 itme != ci->meEllipse->getMeList().end(); ++itme) {
990 vpMeSite pix = *itme;
992 nbGoodPoint++;
993 else
994 nbBadPoint++;
995 }
996 }
997 }
998
999 // Compare the number of good points with the min between the number of
1000 // expected points and number of points that are tracked
1001 int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
1002 // int nb_min = (std::min)(val1, val2);
1003 if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
1004 std::ostringstream oss;
1005 oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
1006 << ". Try to reduce the threshold=" << percentageGdPt
1007 << " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
1009 }
1010}
1011
1020{
1022
1023 unsigned int lvl = (unsigned int)scales.size();
1024 do {
1025 lvl--;
1026
1027 projectionError = 90.0;
1028
1029 if (scales[lvl]) {
1030 vpHomogeneousMatrix cMo_1 = m_cMo;
1031 try {
1032 downScale(lvl);
1033
1034 try {
1036 } catch (...) {
1037 vpTRACE("Error in moving edge tracking");
1038 throw;
1039 }
1040
1041 // initialize the vector that contains the error and the matrix that
1042 // contains the interaction matrix AY: Useless as it is done in
1043 // coputeVVS()
1044 /*
1045 for(std::list<vpMbtDistanceLine*>::const_iterator
1046 it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
1047 (l->isVisible()){ l->initInteractionMatrixError();
1048 }
1049 }
1050
1051 for(std::list<vpMbtDistanceCylinder*>::const_iterator
1052 it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
1053 if(cy->isVisible()) {
1054 cy->initInteractionMatrixError();
1055 }
1056 }
1057
1058 for(std::list<vpMbtDistanceCircle*>::const_iterator
1059 it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
1060 (ci->isVisible()){ ci->initInteractionMatrixError();
1061 }
1062 }
1063 */
1064
1065 try {
1066 computeVVS(*Ipyramid[lvl], lvl);
1067 } catch (...) {
1068 covarianceMatrix = -1;
1069 throw; // throw the original exception
1070 }
1071
1072 testTracking();
1073
1074 if (displayFeatures) {
1076 }
1077
1078 // Looking for new visible face
1079 bool newvisibleface = false;
1080 visibleFace(I, m_cMo, newvisibleface);
1081
1082 // cam.computeFov(I.getWidth(), I.getHeight());
1083 if (useScanLine) {
1086 }
1087
1089
1091 // Reinit the moving edge for the lines which need it.
1093
1094 if (computeProjError)
1096
1097 upScale(lvl);
1098 } catch (const vpException &e) {
1099 if (lvl != 0) {
1100 m_cMo = cMo_1;
1101 reInitLevel(lvl);
1102 upScale(lvl);
1103 } else {
1104 upScale(lvl);
1105 throw(e);
1106 }
1107 }
1108 }
1109 } while (lvl != 0);
1110
1112}
1113
1119
1126{
1127 if (!modelInitialised) {
1128 throw vpException(vpException::fatalError, "model not initialized");
1129 }
1130
1131 bool a = false;
1132
1133#ifdef VISP_HAVE_OGRE
1134 if (useOgre) {
1135 if (!faces.isOgreInitialised()) {
1139 // Turn off Ogre config dialog display for the next call to this
1140 // function since settings are saved in the ogre.cfg file and used
1141 // during the next call
1142 ogreShowConfigDialog = false;
1143 }
1144 }
1145#endif
1146
1147 if (clippingFlag > 2)
1149
1150 visibleFace(I, m_cMo, a);
1152
1153 if (useScanLine) {
1154 if (clippingFlag <= 2)
1156
1159 }
1160
1162 unsigned int i = (unsigned int)scales.size();
1163 do {
1164 i--;
1165 if (scales[i]) {
1166 downScale(i);
1168 upScale(i);
1169 }
1170 } while (i != 0);
1171
1173}
1174
1183{
1184 m_cMo = cdMo;
1185
1186 init(I);
1187}
1188
1197{
1198 m_cMo = cdMo;
1199
1200 vpImageConvert::convert(I_color, m_I);
1201 init(m_I);
1202}
1203
1215void vpMbEdgeTracker::loadConfigFile(const std::string &configFile, bool verbose)
1216{
1217 // Load projection error config
1218 vpMbTracker::loadConfigFile(configFile, verbose);
1219
1221 xmlp.setVerbose(verbose);
1225 xmlp.setEdgeMe(me);
1226
1227 try {
1228 if (verbose) {
1229 std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
1230 }
1231 xmlp.parse(configFile);
1232 } catch (...) {
1233 throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile.c_str());
1234 }
1235
1236 vpCameraParameters camera;
1237 vpMe meParser;
1238 xmlp.getCameraParameters(camera);
1239 xmlp.getEdgeMe(meParser);
1240
1241 setCameraParameters(camera);
1242 setMovingEdge(meParser);
1245
1246 if (xmlp.hasNearClippingDistance())
1248
1249 if (xmlp.hasFarClippingDistance())
1251
1252 if (xmlp.getFovClipping())
1254
1255 useLodGeneral = xmlp.getLodState();
1258
1260 if (this->getNbPolygon() > 0) {
1265 }
1266}
1267
1280 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1281 bool displayFullModel)
1282{
1283 // Display first the Moving-Edges
1284 if (displayFeatures) {
1286 }
1287
1288 std::vector<std::vector<double> > models =
1289 vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1290
1291 for (size_t i = 0; i < models.size(); i++) {
1292 if (vpMath::equal(models[i][0], 0)) {
1293 vpImagePoint ip1(models[i][1], models[i][2]);
1294 vpImagePoint ip2(models[i][3], models[i][4]);
1295 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1296 } else if (vpMath::equal(models[i][0], 1)) {
1297 vpImagePoint center(models[i][1], models[i][2]);
1298 double n20 = models[i][3];
1299 double n11 = models[i][4];
1300 double n02 = models[i][5];
1301 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1302 }
1303 }
1304
1305#ifdef VISP_HAVE_OGRE
1306 if (useOgre)
1307 faces.displayOgre(cMo);
1308#endif
1309}
1310
1323 const vpColor &col, unsigned int thickness, bool displayFullModel)
1324{
1325 // Display first the Moving-Edges
1326 if (displayFeatures) {
1328 }
1329
1330 std::vector<std::vector<double> > models =
1331 vpMbEdgeTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1332
1333 for (size_t i = 0; i < models.size(); i++) {
1334 if (vpMath::equal(models[i][0], 0)) {
1335 vpImagePoint ip1(models[i][1], models[i][2]);
1336 vpImagePoint ip2(models[i][3], models[i][4]);
1337 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1338 } else if (vpMath::equal(models[i][0], 1)) {
1339 vpImagePoint center(models[i][1], models[i][2]);
1340 double n20 = models[i][3];
1341 double n11 = models[i][4];
1342 double n02 = models[i][5];
1343 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1344 }
1345 }
1346
1347#ifdef VISP_HAVE_OGRE
1348 if (useOgre)
1349 faces.displayOgre(cMo);
1350#endif
1351}
1352
1353std::vector<std::vector<double> > vpMbEdgeTracker::getFeaturesForDisplayEdge()
1354{
1355 std::vector<std::vector<double> > features;
1356
1357 const unsigned int lvl = 0;
1358 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1359 vpMbtDistanceLine *l = *it;
1360 if (l->isVisible() && l->isTracked()) {
1361 std::vector<std::vector<double> > currentFeatures = l->getFeaturesForDisplay();
1362 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1363 }
1364 }
1365
1366 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1367 ++it) {
1368 vpMbtDistanceCylinder *cy = *it;
1369 if (cy->isVisible() && cy->isTracked()) {
1370 std::vector<std::vector<double> > currentFeatures = cy->getFeaturesForDisplay();
1371 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1372 }
1373 }
1374
1375 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1376 vpMbtDistanceCircle *ci = *it;
1377 if (ci->isVisible() && ci->isTracked()) {
1378 std::vector<std::vector<double> > currentFeatures = ci->getFeaturesForDisplay();
1379 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1380 }
1381 }
1382
1383 return features;
1384}
1385
1401std::vector<std::vector<double> > vpMbEdgeTracker::getModelForDisplay(unsigned int width, unsigned int height,
1402 const vpHomogeneousMatrix &cMo,
1403 const vpCameraParameters &cam,
1404 bool displayFullModel)
1405{
1406 std::vector<std::vector<double> > models;
1407
1408 for (unsigned int i = 0; i < scales.size(); i += 1) {
1409 if (scales[i]) {
1410 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1411 ++it) {
1412 std::vector<std::vector<double> > currentModel =
1413 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1414 models.insert(models.end(), currentModel.begin(), currentModel.end());
1415 }
1416
1417 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1418 it != cylinders[scaleLevel].end(); ++it) {
1419 std::vector<std::vector<double> > currentModel =
1420 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1421 models.insert(models.end(), currentModel.begin(), currentModel.end());
1422 }
1423
1424 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1425 it != circles[scaleLevel].end(); ++it) {
1426 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1427 if (!paramsCircle.empty()) {
1428 models.push_back(paramsCircle);
1429 }
1430 }
1431 break; // displaying model on one scale only
1432 }
1433 }
1434
1435 return models;
1436}
1437
1439{
1440 for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1443 int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1444
1445 switch (state) {
1448 break;
1449
1450 case vpMeSite::CONTRAST:
1452 break;
1453
1456 break;
1457
1460 break;
1461
1462 case vpMeSite::TOO_NEAR:
1464 break;
1465
1466 default:
1468 }
1469 }
1470 }
1471}
1472
1474{
1475 for (size_t i = 0; i < m_featuresToBeDisplayedEdge.size(); i++) {
1478 int state = static_cast<int>(m_featuresToBeDisplayedEdge[i][3]);
1479
1480 switch (state) {
1483 break;
1484
1485 case vpMeSite::CONTRAST:
1487 break;
1488
1491 break;
1492
1495 break;
1496
1497 case vpMeSite::TOO_NEAR:
1499 break;
1500
1501 default:
1503 }
1504 }
1505 }
1506}
1507
1517{
1518 const bool doNotTrack = false;
1519
1520 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1521 ++it) {
1522 vpMbtDistanceLine *l = *it;
1523 bool isvisible = false;
1524
1525 for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1526 ++itindex) {
1527 int index = *itindex;
1528 if (index == -1)
1529 isvisible = true;
1530 else {
1531 if (l->hiddenface->isVisible((unsigned int)index))
1532 isvisible = true;
1533 }
1534 }
1535
1536 // Si la ligne n'appartient a aucune face elle est tout le temps visible
1537 if (l->Lindex_polygon.empty())
1538 isvisible = true; // Not sure that this can occur
1539
1540 if (isvisible) {
1541 l->setVisible(true);
1542 l->updateTracked();
1543 if (l->meline.empty() && l->isTracked())
1544 l->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1545 } else {
1546 l->setVisible(false);
1547 for (size_t a = 0; a < l->meline.size(); a++) {
1548 if (l->meline[a] != NULL)
1549 delete l->meline[a];
1550 if (a < l->nbFeature.size())
1551 l->nbFeature[a] = 0;
1552 }
1553 l->nbFeatureTotal = 0;
1554 l->meline.clear();
1555 l->nbFeature.clear();
1556 }
1557 }
1558
1559 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1560 it != cylinders[scaleLevel].end(); ++it) {
1561 vpMbtDistanceCylinder *cy = *it;
1562
1563 bool isvisible = false;
1564
1565 int index = cy->index_polygon;
1566 if (index == -1)
1567 isvisible = true;
1568 else {
1569 if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
1570 cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
1571 isvisible = true;
1572 }
1573 // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
1574
1575 if (isvisible) {
1576 cy->setVisible(true);
1577 if (cy->meline1 == NULL || cy->meline2 == NULL) {
1578 if (cy->isTracked())
1579 cy->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1580 }
1581 } else {
1582 cy->setVisible(false);
1583 if (cy->meline1 != NULL)
1584 delete cy->meline1;
1585 if (cy->meline2 != NULL)
1586 delete cy->meline2;
1587 cy->meline1 = NULL;
1588 cy->meline2 = NULL;
1589 cy->nbFeature = 0;
1590 cy->nbFeaturel1 = 0;
1591 cy->nbFeaturel2 = 0;
1592 }
1593 }
1594
1595 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1596 it != circles[scaleLevel].end(); ++it) {
1597 vpMbtDistanceCircle *ci = *it;
1598 bool isvisible = false;
1599
1600 int index = ci->index_polygon;
1601 if (index == -1)
1602 isvisible = true;
1603 else {
1604 if (ci->hiddenface->isVisible((unsigned int)index))
1605 isvisible = true;
1606 }
1607
1608 if (isvisible) {
1609 ci->setVisible(true);
1610 if (ci->meEllipse == NULL) {
1611 if (ci->isTracked())
1612 ci->initMovingEdge(I, _cMo, doNotTrack, m_mask);
1613 }
1614 } else {
1615 ci->setVisible(false);
1616 if (ci->meEllipse != NULL)
1617 delete ci->meEllipse;
1618 ci->meEllipse = NULL;
1619 ci->nbFeature = 0;
1620 }
1621 }
1622}
1623
1630{
1631 const bool doNotTrack = false;
1632
1633 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1634 ++it) {
1635 vpMbtDistanceLine *l = *it;
1636 if (l->isVisible() && l->isTracked()) {
1637 if (l->meline.empty()) {
1638 l->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1639 }
1640 l->trackMovingEdge(I);
1641 }
1642 }
1643
1644 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1645 it != cylinders[scaleLevel].end(); ++it) {
1646 vpMbtDistanceCylinder *cy = *it;
1647 if (cy->isVisible() && cy->isTracked()) {
1648 if (cy->meline1 == NULL || cy->meline2 == NULL) {
1649 cy->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1650 }
1651 cy->trackMovingEdge(I, m_cMo);
1652 }
1653 }
1654
1655 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1656 it != circles[scaleLevel].end(); ++it) {
1657 vpMbtDistanceCircle *ci = *it;
1658 if (ci->isVisible() && ci->isTracked()) {
1659 if (ci->meEllipse == NULL) {
1660 ci->initMovingEdge(I, m_cMo, doNotTrack, m_mask);
1661 }
1662 ci->trackMovingEdge(I, m_cMo);
1663 }
1664 }
1665}
1666
1673{
1675 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1676 ++it) {
1677 if ((*it)->isTracked()) {
1678 l = *it;
1679 l->updateMovingEdge(I, m_cMo);
1680 if (l->nbFeatureTotal == 0 && l->isVisible()) {
1681 l->Reinit = true;
1682 }
1683 }
1684 }
1685
1687 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1688 it != cylinders[scaleLevel].end(); ++it) {
1689 if ((*it)->isTracked()) {
1690 cy = *it;
1691 cy->updateMovingEdge(I, m_cMo);
1692 if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
1693 cy->Reinit = true;
1694 }
1695 }
1696 }
1697
1699 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1700 it != circles[scaleLevel].end(); ++it) {
1701 if ((*it)->isTracked()) {
1702 ci = *it;
1703 ci->updateMovingEdge(I, m_cMo);
1704 if (ci->nbFeature == 0 && ci->isVisible()) {
1705 ci->Reinit = true;
1706 }
1707 }
1708 }
1709}
1710
1712{
1713 unsigned int n = 0;
1714
1716 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1717 ++it) {
1718 if ((*it)->isTracked()) {
1719 l = *it;
1720 unsigned int indexLine = 0;
1721 double wmean = 0;
1722 for (size_t a = 0; a < l->meline.size(); a++) {
1723 if (l->nbFeature[a] > 0) {
1724 std::list<vpMeSite>::iterator itListLine;
1725 itListLine = l->meline[a]->getMeList().begin();
1726
1727 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1728 wmean += m_w_edge[n + indexLine];
1729 vpMeSite p = *itListLine;
1730 if (m_w_edge[n + indexLine] < 0.5) {
1732
1733 *itListLine = p;
1734 }
1735
1736 ++itListLine;
1737 indexLine++;
1738 }
1739 }
1740 }
1741 n += l->nbFeatureTotal;
1742
1743 if (l->nbFeatureTotal != 0)
1744 wmean /= l->nbFeatureTotal;
1745 else
1746 wmean = 1;
1747
1748 l->setMeanWeight(wmean);
1749
1750 if (wmean < 0.8)
1751 l->Reinit = true;
1752 }
1753 }
1754
1755 // Same thing with cylinders as with lines
1757 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1758 it != cylinders[scaleLevel].end(); ++it) {
1759 if ((*it)->isTracked()) {
1760 cy = *it;
1761 double wmean = 0;
1762 std::list<vpMeSite>::iterator itListCyl1;
1763 std::list<vpMeSite>::iterator itListCyl2;
1764
1765 if (cy->nbFeature > 0) {
1766 itListCyl1 = cy->meline1->getMeList().begin();
1767 itListCyl2 = cy->meline2->getMeList().begin();
1768
1769 for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
1770 wmean += m_w_edge[n + i];
1771 vpMeSite p = *itListCyl1;
1772 if (m_w_edge[n + i] < 0.5) {
1774
1775 *itListCyl1 = p;
1776 }
1777
1778 ++itListCyl1;
1779 }
1780 }
1781
1782 if (cy->nbFeaturel1 != 0)
1783 wmean /= cy->nbFeaturel1;
1784 else
1785 wmean = 1;
1786
1787 cy->setMeanWeight1(wmean);
1788
1789 if (wmean < 0.8) {
1790 cy->Reinit = true;
1791 }
1792
1793 wmean = 0;
1794 for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
1795 wmean += m_w_edge[n + i];
1796 vpMeSite p = *itListCyl2;
1797 if (m_w_edge[n + i] < 0.5) {
1799
1800 *itListCyl2 = p;
1801 }
1802
1803 ++itListCyl2;
1804 }
1805
1806 if (cy->nbFeaturel2 != 0)
1807 wmean /= cy->nbFeaturel2;
1808 else
1809 wmean = 1;
1810
1811 cy->setMeanWeight2(wmean);
1812
1813 if (wmean < 0.8) {
1814 cy->Reinit = true;
1815 }
1816
1817 n += cy->nbFeature;
1818 }
1819 }
1820
1821 // Same thing with circles as with lines
1823 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1824 it != circles[scaleLevel].end(); ++it) {
1825 if ((*it)->isTracked()) {
1826 ci = *it;
1827 double wmean = 0;
1828 std::list<vpMeSite>::iterator itListCir;
1829
1830 if (ci->nbFeature > 0) {
1831 itListCir = ci->meEllipse->getMeList().begin();
1832 }
1833
1834 wmean = 0;
1835 for (unsigned int i = 0; i < ci->nbFeature; i++) {
1836 wmean += m_w_edge[n + i];
1837 vpMeSite p = *itListCir;
1838 if (m_w_edge[n + i] < 0.5) {
1840
1841 *itListCir = p;
1842 }
1843
1844 ++itListCir;
1845 }
1846
1847 if (ci->nbFeature != 0)
1848 wmean /= ci->nbFeature;
1849 else
1850 wmean = 1;
1851
1852 ci->setMeanWeight(wmean);
1853
1854 if (wmean < 0.8) {
1855 ci->Reinit = true;
1856 }
1857
1858 n += ci->nbFeature;
1859 }
1860 }
1861}
1862
1873{
1875 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1876 ++it) {
1877 if ((*it)->isTracked()) {
1878 l = *it;
1879 if (l->Reinit && l->isVisible())
1880 l->reinitMovingEdge(I, _cMo, m_mask);
1881 }
1882 }
1883
1885 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1886 it != cylinders[scaleLevel].end(); ++it) {
1887 if ((*it)->isTracked()) {
1888 cy = *it;
1889 if (cy->Reinit && cy->isVisible())
1890 cy->reinitMovingEdge(I, _cMo, m_mask);
1891 }
1892 }
1893
1895 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1896 it != circles[scaleLevel].end(); ++it) {
1897 if ((*it)->isTracked()) {
1898 ci = *it;
1899 if (ci->Reinit && ci->isVisible())
1900 ci->reinitMovingEdge(I, _cMo, m_mask);
1901 }
1902 }
1903}
1904
1906{
1907 // Clear ME to be displayed
1909
1910 for (unsigned int i = 0; i < scales.size(); i += 1) {
1911 if (scales[i]) {
1912 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1913 for (size_t a = 0; a < (*it)->meline.size(); a++) {
1914 if ((*it)->meline[a] != NULL) {
1915 delete (*it)->meline[a];
1916 (*it)->meline[a] = NULL;
1917 }
1918 }
1919
1920 (*it)->meline.clear();
1921 (*it)->nbFeature.clear();
1922 (*it)->nbFeatureTotal = 0;
1923 }
1924
1925 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1926 ++it) {
1927 if ((*it)->meline1 != NULL) {
1928 delete (*it)->meline1;
1929 (*it)->meline1 = NULL;
1930 }
1931 if ((*it)->meline2 != NULL) {
1932 delete (*it)->meline2;
1933 (*it)->meline2 = NULL;
1934 }
1935
1936 (*it)->nbFeature = 0;
1937 (*it)->nbFeaturel1 = 0;
1938 (*it)->nbFeaturel2 = 0;
1939 }
1940
1941 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1942 if ((*it)->meEllipse != NULL) {
1943 delete (*it)->meEllipse;
1944 (*it)->meEllipse = NULL;
1945 }
1946 (*it)->nbFeature = 0;
1947 }
1948 }
1949 }
1950}
1951
1964void vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name)
1965{
1966 {
1967 // suppress line already in the model
1968 bool already_here = false;
1970
1971 for (unsigned int i = 0; i < scales.size(); i += 1) {
1972 if (scales[i]) {
1973 downScale(i);
1974 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1975 l = *it;
1976 if ((samePoint(*(l->p1), P1) && samePoint(*(l->p2), P2)) ||
1977 (samePoint(*(l->p1), P2) && samePoint(*(l->p2), P1))) {
1978 already_here = true;
1979 l->addPolygon(polygon);
1980 l->hiddenface = &faces;
1981 }
1982 }
1983
1984 if (!already_here) {
1985 l = new vpMbtDistanceLine;
1986
1988 l->buildFrom(P1, P2, m_rand);
1989 l->addPolygon(polygon);
1990 l->setMovingEdge(&me);
1991 l->hiddenface = &faces;
1993
1994 l->setIndex(nline);
1995 l->setName(name);
1996
1999
2002
2005
2006 nline += 1;
2007 lines[i].push_back(l);
2008 }
2009 upScale(i);
2010 }
2011 }
2012 }
2013}
2014
2020void vpMbEdgeTracker::removeLine(const std::string &name)
2021{
2023
2024 for (unsigned int i = 0; i < scales.size(); i++) {
2025 if (scales[i]) {
2026 for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2027 l = *it;
2028 if (name.compare(l->getName()) == 0) {
2029 lines[i].erase(it);
2030 break;
2031 }
2032 }
2033 }
2034 }
2035}
2036
2047void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, int idFace,
2048 const std::string &name)
2049{
2050 {
2051 bool already_here = false;
2053
2054 for (unsigned int i = 0; i < scales.size(); i += 1) {
2055 if (scales[i]) {
2056 downScale(i);
2057 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2058 ci = *it;
2059 if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
2060 (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
2061 already_here =
2062 (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
2063 }
2064 }
2065
2066 if (!already_here) {
2067 ci = new vpMbtDistanceCircle;
2068
2070 ci->buildFrom(P1, P2, P3, r);
2071 ci->setMovingEdge(&me);
2072 ci->setIndex(ncircle);
2073 ci->setName(name);
2074 ci->index_polygon = idFace;
2075 ci->hiddenface = &faces;
2076
2077 // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
2078 // ci->getPolygon().setClipping(clippingFlag);
2079
2080 // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
2081 // vpPolygon3D::NEAR_CLIPPING)
2082 // ci->getPolygon().setNearClippingDistance(distNearClip);
2083
2084 // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
2085 // vpPolygon3D::FAR_CLIPPING)
2086 // ci->getPolygon().setFarClippingDistance(distFarClip);
2087
2088 ncircle += 1;
2089 circles[i].push_back(ci);
2090 }
2091 upScale(i);
2092 }
2093 }
2094 }
2095}
2096
2106void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, double r, int idFace, const std::string &name)
2107{
2108 {
2109 bool already_here = false;
2111
2112 for (unsigned int i = 0; i < scales.size(); i += 1) {
2113 if (scales[i]) {
2114 downScale(i);
2115 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2116 ++it) {
2117 cy = *it;
2118 if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
2119 (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
2120 already_here =
2121 (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
2122 }
2123 }
2124
2125 if (!already_here) {
2126 cy = new vpMbtDistanceCylinder;
2127
2129 cy->buildFrom(P1, P2, r);
2130 cy->setMovingEdge(&me);
2131 cy->setIndex(ncylinder);
2132 cy->setName(name);
2133 cy->index_polygon = idFace;
2134 cy->hiddenface = &faces;
2135 ncylinder += 1;
2136 cylinders[i].push_back(cy);
2137 }
2138 upScale(i);
2139 }
2140 }
2141 }
2142}
2143
2149void vpMbEdgeTracker::removeCylinder(const std::string &name)
2150{
2152
2153 for (unsigned int i = 0; i < scales.size(); i++) {
2154 if (scales[i]) {
2155 for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
2156 cy = *it;
2157 if (name.compare(cy->getName()) == 0) {
2158 cylinders[i].erase(it);
2159 break;
2160 }
2161 }
2162 }
2163 }
2164}
2165
2171void vpMbEdgeTracker::removeCircle(const std::string &name)
2172{
2174
2175 for (unsigned int i = 0; i < scales.size(); i++) {
2176 if (scales[i]) {
2177 for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2178 ci = *it;
2179 if (name.compare(ci->getName()) == 0) {
2180 circles[i].erase(it);
2181 break;
2182 }
2183 }
2184 }
2185 }
2186}
2187
2194{
2195 unsigned int nbpt = p.getNbPoint();
2196 if (nbpt > 0) {
2197 for (unsigned int i = 0; i < nbpt - 1; i++)
2198 addLine(p.p[i], p.p[i + 1], p.getIndex());
2199 addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
2200 }
2201}
2202
2214void vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, bool &newvisibleline)
2215{
2216 unsigned int n;
2217 bool changed = false;
2218
2219 if (!useOgre) {
2220 // n = faces.setVisible(_I.getWidth(), I.getHeight(), m_cam, cMo, vpMath::rad(89), vpMath::rad(89),
2221 // changed);
2222 n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2223 } else {
2224#ifdef VISP_HAVE_OGRE
2226#else
2227 n = faces.setVisible(I.getWidth(), I.getHeight(), m_cam, cMo, angleAppears, angleDisappears, changed);
2228#endif
2229 }
2230
2231 if (n > nbvisiblepolygone) {
2232 // cout << "une nouvelle face est visible " << endl;
2233 newvisibleline = true;
2234 } else
2235 newvisibleline = false;
2236
2238}
2239
2256{
2257 unsigned int nbpt = polygon.getNbPoint();
2258 if (nbpt > 0) {
2259 for (unsigned int i = 0; i < nbpt - 1; i++)
2260 vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2261 vpMbEdgeTracker::addLine(polygon.p[nbpt - 1], polygon.p[0], polygon.getIndex(), polygon.getName());
2262 }
2263}
2280{
2281 unsigned int nbpt = polygon.getNbPoint();
2282 if (nbpt > 0) {
2283 for (unsigned int i = 0; i < nbpt - 1; i++)
2284 vpMbEdgeTracker::addLine(polygon.p[i], polygon.p[i + 1], polygon.getIndex(), polygon.getName());
2285 }
2286}
2287
2288unsigned int vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
2289 unsigned int &nberrors_circles)
2290{
2291 unsigned int nbrow = 0;
2292 nberrors_lines = 0;
2293 nberrors_cylinders = 0;
2294 nberrors_circles = 0;
2295
2296 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2297 ++it) {
2298
2299 vpMbtDistanceLine *l = *it;
2300
2301 if (l->isTracked()) {
2303 nbrow += l->nbFeatureTotal;
2304 nberrors_lines += l->nbFeatureTotal;
2305 }
2306 }
2307
2308 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2309 it != cylinders[scaleLevel].end(); ++it) {
2310 vpMbtDistanceCylinder *cy = *it;
2311
2312 if (cy->isTracked()) {
2314 nbrow += cy->nbFeature;
2315 nberrors_cylinders += cy->nbFeature;
2316 }
2317 }
2318
2319 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2320 it != circles[scaleLevel].end(); ++it) {
2321 vpMbtDistanceCircle *ci = *it;
2322
2323 if (ci->isTracked()) {
2325 nbrow += ci->nbFeature;
2326 nberrors_circles += ci->nbFeature;
2327 }
2328 }
2329
2330 return nbrow;
2331}
2332
2344void vpMbEdgeTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
2345 const std::string &name)
2346{
2347 addCircle(p1, p2, p3, radius, (int)idFace, name);
2348}
2349
2360void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
2361 const std::string &name)
2362{
2363 addCylinder(p1, p2, radius, (int)idFace, name);
2364}
2365
2372{
2373 m_cMo.eye();
2377
2378 for (unsigned int i = 0; i < scales.size(); i += 1) {
2379 if (scales[i]) {
2380 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2381 l = *it;
2382 if (l != NULL)
2383 delete l;
2384 l = NULL;
2385 }
2386
2387 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2388 ++it) {
2389 cy = *it;
2390 if (cy != NULL)
2391 delete cy;
2392 cy = NULL;
2393 }
2394
2395 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2396 ci = *it;
2397 if (ci != NULL)
2398 delete ci;
2399 ci = NULL;
2400 }
2401 lines[i].clear();
2402 cylinders[i].clear();
2403 circles[i].clear();
2404 }
2405 }
2406
2407 faces.reset();
2408
2409 useScanLine = false;
2410
2411#ifdef VISP_HAVE_OGRE
2412 useOgre = false;
2413#endif
2414
2415 m_computeInteraction = true;
2416 nline = 0;
2417 ncylinder = 0;
2418 m_lambda = 1.0;
2420 percentageGdPt = 0.4;
2421
2425
2427
2428 // reinitialization of the scales.
2429 this->setScales(scales);
2430}
2431
2444void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
2445 const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
2446{
2447 m_cMo.eye();
2451
2452 for (unsigned int i = 0; i < scales.size(); i += 1) {
2453 if (scales[i]) {
2454 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2455 l = *it;
2456 if (l != NULL)
2457 delete l;
2458 l = NULL;
2459 }
2460
2461 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2462 ++it) {
2463 cy = *it;
2464 if (cy != NULL)
2465 delete cy;
2466 cy = NULL;
2467 }
2468
2469 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2470 ci = *it;
2471 if (ci != NULL)
2472 delete ci;
2473 ci = NULL;
2474 }
2475
2476 lines[i].clear();
2477 cylinders[i].clear();
2478 circles[i].clear();
2479 }
2480 }
2481
2482 faces.reset();
2483
2484 // compute_interaction=1;
2485 nline = 0;
2486 ncylinder = 0;
2487 ncircle = 0;
2488 // lambda = 1;
2490
2491 loadModel(cad_name, verbose, T);
2492 initFromPose(I, cMo);
2493}
2494
2505unsigned int vpMbEdgeTracker::getNbPoints(unsigned int level) const
2506{
2507 if ((level > scales.size()) || !scales[level]) {
2508 throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
2509 level);
2510 }
2511
2512 unsigned int nbGoodPoints = 0;
2514 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
2515 l = *it;
2516 if (l->isVisible() && l->isTracked()) {
2517 for (size_t a = 0; a < l->meline.size(); a++) {
2518 if (l->nbFeature[a] != 0)
2519 for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
2520 itme != l->meline[a]->getMeList().end(); ++itme) {
2521 if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2522 nbGoodPoints++;
2523 }
2524 }
2525 }
2526 }
2527
2529 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
2530 ++it) {
2531 cy = *it;
2532 if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
2533 for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
2534 itme1 != cy->meline1->getMeList().end(); ++itme1) {
2535 if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
2536 nbGoodPoints++;
2537 }
2538 for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
2539 itme2 != cy->meline2->getMeList().end(); ++itme2) {
2540 if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
2541 nbGoodPoints++;
2542 }
2543 }
2544 }
2545
2547 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
2548 ci = *it;
2549 if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
2550 for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
2551 itme != ci->meEllipse->getMeList().end(); ++itme) {
2552 if (itme->getState() == vpMeSite::NO_SUPPRESSION)
2553 nbGoodPoints++;
2554 }
2555 }
2556 }
2557
2558 return nbGoodPoints;
2559}
2560
2582void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
2583{
2584 unsigned int nbActivatedLevels = 0;
2585 for (unsigned int i = 0; i < scale.size(); i++) {
2586 if (scale[i]) {
2587 nbActivatedLevels++;
2588 }
2589 }
2590
2591 if (scale.empty() || (nbActivatedLevels == 0)) {
2592 vpERROR_TRACE(" !! WARNING : must use at least one level for the "
2593 "tracking. Use the global one");
2594 this->scales.clear();
2595 this->scales.push_back(true);
2596
2597 lines.resize(1);
2598 lines[0].clear();
2599
2600 cylinders.resize(1);
2601 cylinders[0].clear();
2602
2603 circles.resize(1);
2604 circles[0].clear();
2605 } else {
2606 this->scales = scale;
2607
2608 lines.resize(scale.size());
2609 cylinders.resize(scale.size());
2610 circles.resize(scale.size());
2611
2612 for (unsigned int i = 0; i < lines.size(); i++) {
2613 lines[i].clear();
2614 cylinders[i].clear();
2615 circles[i].clear();
2616 }
2617 }
2618}
2619
2626{
2628 std::cerr << "Far clipping value cannot be inferior than near clipping "
2629 "value. Far clipping won't be considered."
2630 << std::endl;
2631 else if (dist < 0)
2632 std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
2633 "won't be considered."
2634 << std::endl;
2635 else {
2638
2639 for (unsigned int i = 0; i < scales.size(); i += 1) {
2640 if (scales[i]) {
2641 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2642 l = *it;
2644 }
2645 }
2646 }
2647 }
2648}
2649
2656{
2658 std::cerr << "Near clipping value cannot be superior than far clipping "
2659 "value. Near clipping won't be considered."
2660 << std::endl;
2661 else if (dist < 0)
2662 std::cerr << "Near clipping value cannot be inferior than 0. Near "
2663 "clipping won't be considered."
2664 << std::endl;
2665 else {
2668
2669 for (unsigned int i = 0; i < scales.size(); i += 1) {
2670 if (scales[i]) {
2671 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2672 l = *it;
2674 }
2675 }
2676 }
2677 }
2678}
2679
2687void vpMbEdgeTracker::setClipping(const unsigned int &flags)
2688{
2690
2692
2693 for (unsigned int i = 0; i < scales.size(); i += 1) {
2694 if (scales[i]) {
2695 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2696 l = *it;
2698 }
2699 }
2700 }
2701}
2702
2719 std::vector<const vpImage<unsigned char> *> &_pyramid)
2720{
2721 _pyramid.resize(scales.size());
2722
2723 if (scales[0]) {
2724 _pyramid[0] = &_I;
2725 } else {
2726 _pyramid[0] = NULL;
2727 }
2728
2729 for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2730 if (scales[i]) {
2731 unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
2732 vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
2733 for (unsigned int k = 0, ii = 0; k < I->getHeight(); k += 1, ii += cScale) {
2734 for (unsigned int l = 0, jj = 0; l < I->getWidth(); l += 1, jj += cScale) {
2735 (*I)[k][l] = _I[ii][jj];
2736 }
2737 }
2738 _pyramid[i] = I;
2739 } else {
2740 _pyramid[i] = NULL;
2741 }
2742 }
2743}
2744
2751void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
2752{
2753 if (_pyramid.size() > 0) {
2754 _pyramid[0] = NULL;
2755 for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
2756 if (_pyramid[i] != NULL) {
2757 delete _pyramid[i];
2758 _pyramid[i] = NULL;
2759 }
2760 }
2761 _pyramid.resize(0);
2762 }
2763}
2764
2775void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, unsigned int level) const
2776{
2777 if (level > scales.size() || !scales[level]) {
2778 std::ostringstream oss;
2779 oss << level;
2780 std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2782 }
2783
2784 linesList = lines[level];
2785}
2786
2797void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, unsigned int level) const
2798{
2799 if (level > scales.size() || !scales[level]) {
2800 std::ostringstream oss;
2801 oss << level;
2802 std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2804 }
2805
2806 cylindersList = cylinders[level];
2807}
2808
2819void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, unsigned int level) const
2820{
2821 if (level > scales.size() || !scales[level]) {
2822 std::ostringstream oss;
2823 oss << level;
2824 std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
2826 }
2827
2828 circlesList = circles[level];
2829}
2830
2837void vpMbEdgeTracker::downScale(const unsigned int _scale)
2838{
2839 const double ratio = pow(2., (int)_scale);
2840 scaleLevel = _scale;
2841
2842 vpMatrix K = m_cam.get_K();
2843
2844 K[0][0] /= ratio;
2845 K[1][1] /= ratio;
2846 K[0][2] /= ratio;
2847 K[1][2] /= ratio;
2848
2850}
2851
2858void vpMbEdgeTracker::upScale(const unsigned int _scale)
2859{
2860 const double ratio = pow(2., (int)_scale);
2861 scaleLevel = 0;
2862
2863 vpMatrix K = m_cam.get_K();
2864
2865 K[0][0] *= ratio;
2866 K[1][1] *= ratio;
2867 K[0][2] *= ratio;
2868 K[1][2] *= ratio;
2869
2871}
2872
2880void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
2881{
2882 unsigned int scaleLevel_1 = scaleLevel;
2883 scaleLevel = _lvl;
2884
2886 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
2887 ++it) {
2888 if ((*it)->isTracked()) {
2889 l = *it;
2891 }
2892 }
2893
2895 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
2896 it != cylinders[scaleLevel].end(); ++it) {
2897 if ((*it)->isTracked()) {
2898 cy = *it;
2899 cy->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2900 }
2901 }
2902
2904 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
2905 it != circles[scaleLevel].end(); ++it) {
2906 if ((*it)->isTracked()) {
2907 ci = *it;
2908 ci->reinitMovingEdge(*Ipyramid[_lvl], m_cMo, m_mask);
2909 }
2910 }
2911
2912 trackMovingEdge(*Ipyramid[_lvl]);
2913 updateMovingEdge(*Ipyramid[_lvl]);
2914 scaleLevel = scaleLevel_1;
2915}
2916
2924void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
2925{
2926 for (unsigned int i = 0; i < scales.size(); i += 1) {
2927 if (scales[i]) {
2928 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
2929 /*(*it)->setTracked(useEdgeTracking);
2930 for(std::list<int>::const_iterator
2931 itpoly=(*it)->Lindex_polygon.begin();
2932 itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
2933 if(faces[(*itpoly)]->getName() != name){
2934 (*it)->setTracked(true);
2935 break;
2936 }
2937 }*/
2938
2939 (*it)->setTracked(name, useEdgeTracking);
2940 }
2941
2942 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
2943 ++it) {
2944 if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2945 (*it)->setTracked(useEdgeTracking);
2946 }
2947 }
2948
2949 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
2950 if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
2951 (*it)->setTracked(useEdgeTracking);
2952 }
2953 }
2954 }
2955 }
2956}
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
void initFromCalibrationMatrix(const vpMatrix &_K)
Implementation of column vector and the associated operations.
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
static const vpColor red
Definition vpColor.h:211
static const vpColor cyan
Definition vpColor.h:220
static const vpColor blue
Definition vpColor.h:217
static const vpColor purple
Definition vpColor.h:222
static const vpColor yellow
Definition vpColor.h:219
static const vpColor green
Definition vpColor.h:214
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 emitted by ViSP classes.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
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 of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static double sqr(double x)
Definition vpMath.h:124
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static Type minimum(const Type &a, const Type &b)
Definition vpMath.h:180
static int sign(double x)
Definition vpMath.h:342
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void eye()
Definition vpMatrix.cpp:446
vpMatrix AtA() const
Definition vpMatrix.cpp:625
vpMatrix pseudoInverse(double svThreshold=1e-6) const
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)
virtual void setClipping(const unsigned int &flags)
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)
void computeVVSFirstPhasePoseEstimation(unsigned int iter, bool &isoJoIdentity)
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.
bool modelInitialised
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
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)
bool m_projectionErrorDisplay
Display gradient and model orientation for projection error computation.
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
unsigned int m_projectionErrorDisplayLength
Length of the arrows used to show the gradient and model orientation.
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.
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
bool m_computeInteraction
vpMatrix oJo
The Degrees of Freedom to estimate.
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
vpUniRand m_rand
Random number generator used in vpMbtDistanceLine::buildFrom()
vpMatrix covarianceMatrix
Covariance matrix.
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
bool computeProjError
vpHomogeneousMatrix m_cMo
The current pose.
vpMatrix m_SobelX
Sobel kernel in X.
vpCameraParameters m_cam
The camera parameters.
double projectionError
bool useOgre
Use Ogre3d for visibility tests.
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
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.
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
bool displayFeatures
If true, the features are displayed.
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
double distFarClip
Distance for near clipping.
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
bool useScanLine
Use Scanline for visibility tests.
void computeJTR(const vpMatrix &J, const vpColVector &R, vpColVector &JTR) const
vpMatrix m_SobelY
Sobel kernel in Y.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
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.
double distNearClip
Distance for near clipping.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
bool ogreShowConfigDialog
unsigned int clippingFlag
Flags specifying which clipping to used.
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 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 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.
std::string getName() const
int getIndex() const
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:65
@ TOO_NEAR
Point removed because too near image borders.
Definition vpMeSite.h:90
@ THRESHOLD
Point removed due to a threshold problem.
Definition vpMeSite.h:88
@ CONTRAST
Point removed due to a contrast problem.
Definition vpMeSite.h:84
@ M_ESTIMATOR
Point removed during virtual visual-servoing because considered as an outlier.
Definition vpMeSite.h:89
@ NO_SUPPRESSION
Point used by the tracker.
Definition vpMeSite.h:83
vpMeSiteState getState() const
Definition vpMeSite.h:261
void setState(const vpMeSiteState &flag)
Definition vpMeSite.h:247
Definition vpMe.h:122
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setFarClippingDistance(const double &dist)
unsigned int getNbPoint() const
void setNearClippingDistance(const double &dist)
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
void setClipping(const unsigned int &flags)
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
Error that can be emitted by the vpTracker class and its derivatives.
@ notEnoughPointError
Not enough point to track.
@ fatalError
Tracker fatal error.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition vpDebug.h:411
#define vpERROR_TRACE
Definition vpDebug.h:388