Visual Servoing Platform version 3.5.0
vpMbEdgeKltTracker.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 * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
33 *
34 * Authors:
35 * Romain Tallonneau
36 * Aurelien Yol
37 *
38 *****************************************************************************/
39
40//#define VP_DEBUG_MODE 1 // Activate debug level 1
41
42#include <visp3/core/vpDebug.h>
43#include <visp3/core/vpTrackingException.h>
44#include <visp3/core/vpVelocityTwistMatrix.h>
45#include <visp3/mbt/vpMbEdgeKltTracker.h>
46#include <visp3/mbt/vpMbtXmlGenericParser.h>
47
48#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
49
51 : m_thresholdKLT(2.), m_thresholdMBT(2.), m_maxIterKlt(30), m_w_mbt(), m_w_klt(), m_error_hybrid(), m_w_hybrid()
52{
53 computeCovariance = false;
54
55#ifdef VISP_HAVE_OGRE
56 faces.getOgreContext()->setWindowName("MBT Hybrid");
57#endif
58
59 m_lambda = 0.8;
60 m_maxIter = 200;
61}
62
68
76{
78
80
82
83 unsigned int i = (unsigned int)scales.size();
84 do {
85 i--;
86 if (scales[i]) {
87 downScale(i);
89 upScale(i);
90 }
91 } while (i != 0);
92
94}
95
106{
108
110
111 if (useScanLine) {
115 }
116
118
119 unsigned int i = (unsigned int)scales.size();
120 do {
121 i--;
122 if (scales[i]) {
123 downScale(i);
125 upScale(i);
126 }
127 } while (i != 0);
128
130}
131
142{
145
147
148 if (useScanLine) {
152 }
153
155
156 unsigned int i = (unsigned int)scales.size();
157 do {
158 i--;
159 if (scales[i]) {
160 downScale(i);
162 upScale(i);
163 }
164 } while (i != 0);
165
167}
168
174{
177}
178
179unsigned int vpMbEdgeKltTracker::initMbtTracking(unsigned int lvl)
180{
181 if (lvl >= scales.size() || !scales[lvl]) {
182 throw vpException(vpException::dimensionError, "lvl not used.");
183 }
184
185 unsigned int nbrow = 0;
186 for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
187 vpMbtDistanceLine *l = *it;
188
189 if (l->isTracked()) {
191 nbrow += l->nbFeatureTotal;
192 }
193 }
194
195 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
196 ++it) {
197 vpMbtDistanceCylinder *cy = *it;
198
199 if (cy->isTracked()) {
201 nbrow += cy->nbFeature;
202 }
203 }
204
205 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
206 vpMbtDistanceCircle *ci = *it;
207
208 if (ci->isTracked()) {
210 nbrow += ci->nbFeature;
211 }
212 }
213
214 return nbrow;
215}
216
277void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
278{
279 // Load projection error config
280 vpMbTracker::loadConfigFile(configFile, verbose);
281
283 xmlp.setVerbose(verbose);
287
288 xmlp.setEdgeMe(me);
289
290 xmlp.setKltMaxFeatures(10000);
291 xmlp.setKltWindowSize(5);
292 xmlp.setKltQuality(0.01);
293 xmlp.setKltMinDistance(5);
294 xmlp.setKltHarrisParam(0.01);
295 xmlp.setKltBlockSize(3);
296 xmlp.setKltPyramidLevels(3);
298
299 try {
300 if (verbose) {
301 std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
302 }
303 xmlp.parse(configFile.c_str());
304 } catch (...) {
305 vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
306 throw vpException(vpException::ioError, "problem to parse configuration file.");
307 }
308
309 vpCameraParameters camera;
310 xmlp.getCameraParameters(camera);
311 setCameraParameters(camera);
312
315
316 if (xmlp.hasNearClippingDistance())
318
319 if (xmlp.hasFarClippingDistance())
321
322 if (xmlp.getFovClipping()) {
324 }
325
326 useLodGeneral = xmlp.getLodState();
329
331 if (this->getNbPolygon() > 0) {
336 }
337
338 vpMe meParser;
339 xmlp.getEdgeMe(meParser);
341
350
351 // if(useScanLine)
352 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
353}
354
359 unsigned int lvl)
360{
361 postTrackingMbt(w_mbt, lvl);
362
363 if (displayFeatures) {
364 if (lvl == 0) {
365 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
366 vpMbtDistanceLine *l = *it;
367 if (l->isVisible() && l->isTracked()) {
368 l->displayMovingEdges(I);
369 }
370 }
371
372 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
373 ++it) {
374 vpMbtDistanceCylinder *cy = *it;
375 // A cylinder is always visible: #FIXME AY: Still valid?
376 if (cy->isTracked())
377 cy->displayMovingEdges(I);
378 }
379
380 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
381 vpMbtDistanceCircle *ci = *it;
382 if (ci->isVisible() && ci->isTracked()) {
383 ci->displayMovingEdges(I);
384 }
385 }
386 }
387 }
388
389 bool reInit = vpMbKltTracker::postTracking(I, w_klt);
390
391 if (useScanLine) {
395 }
396
398
401
404
405 if (reInit)
406 return true;
407
408 return false;
409}
410
415 unsigned int lvl)
416{
417 postTrackingMbt(w_mbt, lvl);
418
419 if (displayFeatures) {
420 if (lvl == 0) {
421 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
422 vpMbtDistanceLine *l = *it;
423 if (l->isVisible() && l->isTracked()) {
424 l->displayMovingEdges(I_color);
425 }
426 }
427
428 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
429 ++it) {
430 vpMbtDistanceCylinder *cy = *it;
431 // A cylinder is always visible: #FIXME AY: Still valid?
432 if (cy->isTracked())
434 }
435
436 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
437 vpMbtDistanceCircle *ci = *it;
438 if (ci->isVisible() && ci->isTracked()) {
440 }
441 }
442 }
443 }
444
445 bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
446
447 if (useScanLine) {
451 }
452
454
457
460
461 if (reInit)
462 return true;
463
464 return false;
465}
466
478{
479 if (lvl >= scales.size() || !scales[lvl]) {
480 throw vpException(vpException::dimensionError, "_lvl not used.");
481 }
482
483 unsigned int n = 0;
485 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
486 if ((*it)->isTracked()) {
487 l = *it;
488 unsigned int indexLine = 0;
489 double wmean = 0;
490
491 for (size_t a = 0; a < l->meline.size(); a++) {
492 std::list<vpMeSite>::iterator itListLine;
493 if (l->nbFeature[a] > 0)
494 itListLine = l->meline[a]->getMeList().begin();
495
496 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
497 wmean += w[n + indexLine];
498 vpMeSite p = *itListLine;
499 if (w[n + indexLine] < 0.5) {
501 *itListLine = p;
502 }
503
504 ++itListLine;
505 indexLine++;
506 }
507 }
508
509 n += l->nbFeatureTotal;
510
511 if (l->nbFeatureTotal != 0)
512 wmean /= l->nbFeatureTotal;
513 else
514 wmean = 1;
515
516 l->setMeanWeight(wmean);
517
518 if (wmean < 0.8)
519 l->Reinit = true;
520 }
521 }
522
523 // Same thing with cylinders as with lines
525 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
526 ++it) {
527 if ((*it)->isTracked()) {
528 cy = *it;
529 double wmean = 0;
530 std::list<vpMeSite>::iterator itListCyl1;
531 std::list<vpMeSite>::iterator itListCyl2;
532 if (cy->nbFeature > 0) {
533 itListCyl1 = cy->meline1->getMeList().begin();
534 itListCyl2 = cy->meline2->getMeList().begin();
535 }
536
537 wmean = 0;
538 for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
539 wmean += w[n + i];
540 vpMeSite p = *itListCyl1;
541 if (w[n + i] < 0.5) {
543
544 *itListCyl1 = p;
545 }
546
547 ++itListCyl1;
548 }
549
550 if (cy->nbFeaturel1 != 0)
551 wmean /= cy->nbFeaturel1;
552 else
553 wmean = 1;
554
555 cy->setMeanWeight1(wmean);
556
557 if (wmean < 0.8) {
558 cy->Reinit = true;
559 }
560
561 wmean = 0;
562 for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
563 wmean += w[n + i];
564 vpMeSite p = *itListCyl2;
565 if (w[n + i] < 0.5) {
567
568 *itListCyl2 = p;
569 }
570
571 ++itListCyl2;
572 }
573
574 if (cy->nbFeaturel2 != 0)
575 wmean /= cy->nbFeaturel2;
576 else
577 wmean = 1;
578
579 cy->setMeanWeight2(wmean);
580
581 if (wmean < 0.8) {
582 cy->Reinit = true;
583 }
584
585 n += cy->nbFeature;
586 }
587 }
588
589 // Same thing with circles as with lines
591 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
592 if ((*it)->isTracked()) {
593 ci = *it;
594 double wmean = 0;
595 std::list<vpMeSite>::iterator itListCir;
596
597 if (ci->nbFeature > 0) {
598 itListCir = ci->meEllipse->getMeList().begin();
599 }
600
601 wmean = 0;
602 for (unsigned int i = 0; i < ci->nbFeature; i++) {
603 wmean += w[n + i];
604 vpMeSite p = *itListCir;
605 if (w[n + i] < 0.5) {
607
608 *itListCir = p;
609 }
610
611 ++itListCir;
612 }
613
614 if (ci->nbFeature != 0)
615 wmean /= ci->nbFeature;
616 else
617 wmean = 1;
618
619 ci->setMeanWeight(wmean);
620
621 if (wmean < 0.8) {
622 ci->Reinit = true;
623 }
624
625 n += ci->nbFeature;
626 }
627 }
628}
629
638void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
639 unsigned int lvl)
640{
641 vpColVector factor;
642 nbrow = trackFirstLoop(I, factor, lvl);
643
644 if (nbrow < 4 && nbInfos < 4) {
645 throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
646 } else if (nbrow < 4)
647 nbrow = 0;
648
649 unsigned int totalNbRows = nbrow + 2 * nbInfos;
650 double residu = 0;
651 double residu_1 = -1;
652 unsigned int iter = 0;
653
654 vpMatrix L(totalNbRows, 6);
655 vpMatrix L_mbt, L_klt; // interaction matrix
656 vpColVector weighted_error(totalNbRows);
657 vpColVector R_mbt, R_klt; // residu
658 vpMatrix L_true;
659 vpMatrix LVJ_true;
660
661 if (nbrow != 0) {
662 L_mbt.resize(nbrow, 6, false, false);
663 R_mbt.resize(nbrow, false);
664 }
665
666 if (nbInfos != 0) {
667 L_klt.resize(2 * nbInfos, 6, false, false);
668 R_klt.resize(2 * nbInfos, false);
669 }
670
671 vpColVector v; // "speed" for VVS
672 vpRobust robust_mbt, robust_klt;
673 vpHomography H;
674
675 vpMatrix LTL;
676 vpColVector LTR;
677
678 double factorMBT; // = 1.0;
679 double factorKLT; // = 1.0;
680
681 // More efficient weight repartition for hybrid tracker should come soon...
682 // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
683 // factorKLT = 1.0 - factorMBT;
684 factorMBT = 0.35;
685 factorKLT = 0.65;
686
687 if (nbrow < 4)
688 factorKLT = 1.;
689 if (nbInfos < 4)
690 factorMBT = 1.;
691
692 double residuMBT = 0;
693 double residuKLT = 0;
694
695 vpHomogeneousMatrix cMoPrev;
696 vpHomogeneousMatrix ctTc0_Prev;
697 vpColVector m_error_prev;
698 vpColVector m_w_prev;
699
700 // Init size
701 m_error_hybrid.resize(totalNbRows, false);
702 m_w_hybrid.resize(totalNbRows, false);
703
704 if (nbrow != 0) {
705 m_w_mbt.resize(nbrow, false);
706 m_w_mbt = 1; // needed in vpRobust::psiTukey()
707 }
708
709 if (nbInfos != 0) {
710 m_w_klt.resize(2 * nbInfos, false);
711 m_w_klt = 1; // needed in vpRobust::psiTukey()
712 }
713
714 double mu = m_initialMu;
715
716 while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
717 if (nbrow >= 4)
718 trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
719
720 if (nbInfos >= 4) {
721 unsigned int shift = 0;
722
723 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
724 it != vpMbKltTracker::kltPolygons.end(); ++it) {
725 vpMbtDistanceKltPoints *kltpoly = *it;
726 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
727 vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
728 vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
729 kltpoly->computeHomography(ctTc0, H);
730 kltpoly->computeInteractionMatrixAndResidu(subR, subL);
731 shift += 2 * kltpoly->getCurrentNumberPoints();
732 }
733 }
734
735 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
736 ++it) {
737 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
738
739 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
740 vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
741 vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
742 try {
743 kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
744 } catch (...) {
745 throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
746 }
747
748 shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
749 }
750 }
751 }
752
753 /* residuals */
754 if (nbrow > 3) {
755 m_error_hybrid.insert(0, R_mbt);
756 }
757
758 if (nbInfos > 3) {
759 m_error_hybrid.insert(nbrow, R_klt);
760 }
761
762 unsigned int cpt = 0;
763 while (cpt < (nbrow + 2 * nbInfos)) {
764 if (cpt < (unsigned)nbrow) {
765 m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
766 } else {
767 m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
768 }
769 cpt++;
770 }
771
772 bool reStartFromLastIncrement = false;
773 computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
774 &m_w_prev);
775 if (reStartFromLastIncrement) {
776 ctTc0 = ctTc0_Prev;
777 }
778
779 if (!reStartFromLastIncrement) {
780 /* robust */
781 if (nbrow > 3) {
782 residuMBT = 0;
783 for (unsigned int i = 0; i < R_mbt.getRows(); i++)
784 residuMBT += fabs(R_mbt[i]);
785 residuMBT /= R_mbt.getRows();
786
788 robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
789
790 L.insert(L_mbt, 0, 0);
791 }
792
793 if (nbInfos > 3) {
794 residuKLT = 0;
795 for (unsigned int i = 0; i < R_klt.getRows(); i++)
796 residuKLT += fabs(R_klt[i]);
797 residuKLT /= R_klt.getRows();
798
800 robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
801
802 L.insert(L_klt, nbrow, 0);
803 }
804
805 cpt = 0;
806 while (cpt < (nbrow + 2 * nbInfos)) {
807 if (cpt < (unsigned)nbrow) {
808 m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
809 } else {
810 m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
811 }
812 cpt++;
813 }
814
815 if (computeCovariance) {
816 L_true = L;
817 if (!isoJoIdentity) {
819 cVo.buildFrom(m_cMo);
820 LVJ_true = (L * cVo * oJo);
821 }
822 }
823
824 residu_1 = residu;
825 residu = 0;
826 double num = 0;
827 double den = 0;
828
829 for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
830 num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
831 den += m_w_hybrid[i];
832
833 weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
835 for (unsigned int j = 0; j < 6; j += 1) {
836 L[i][j] *= m_w_hybrid[i];
837 }
838 }
839 }
840
841 residu = sqrt(num / den);
842
843 computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
844 &m_w_hybrid, &m_w_prev);
845
846 cMoPrev = m_cMo;
847 ctTc0_Prev = ctTc0;
849 m_cMo = ctTc0 * c0Mo;
850 }
851
852 iter++;
853 }
854
856}
857
859{
860 throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
861}
862
864{
865 throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
866 "computeVVSInteractionMatrixAndR"
867 "esidu() should not be called!");
868}
869
878{
879 try {
881 } catch (...) {
882 }
883
884 if (m_nbInfos >= 4) {
885 unsigned int old_maxIter = m_maxIter;
888 m_maxIter = old_maxIter;
889 } else {
890 m_nbInfos = 0;
891 // std::cout << "[Warning] Unable to init with KLT" << std::endl;
892 }
893
895
896 unsigned int nbrow = 0;
897 computeVVS(I, m_nbInfos, nbrow);
898
899 if (postTracking(I, m_w_mbt, m_w_klt)) {
901
902 // AY : Removed as edge tracked, if necessary, is reinitialized in
903 // postTracking()
904
905 // initPyramid(I, Ipyramid);
906
907 // unsigned int i = (unsigned int)scales.size();
908 // do {
909 // i--;
910 // if(scales[i]){
911 // downScale(i);
912 // initMovingEdge(*Ipyramid[i], cMo);
913 // upScale(i);
914 // }
915 // } while(i != 0);
916
917 // cleanPyramid(Ipyramid);
918 }
919
920 if (displayFeatures) {
922 }
923}
924
933{
935 try {
937 } catch (...) {
938 }
939
940 if (m_nbInfos >= 4) {
941 unsigned int old_maxIter = m_maxIter;
944 m_maxIter = old_maxIter;
945 } else {
946 m_nbInfos = 0;
947 // std::cout << "[Warning] Unable to init with KLT" << std::endl;
948 }
949
951
952 unsigned int nbrow = 0;
953 computeVVS(m_I, m_nbInfos, nbrow);
954
955 if (postTracking(I_color, m_w_mbt, m_w_klt)) {
957
958 // AY : Removed as edge tracked, if necessary, is reinitialized in
959 // postTracking()
960
961 // initPyramid(I, Ipyramid);
962
963 // unsigned int i = (unsigned int)scales.size();
964 // do {
965 // i--;
966 // if(scales[i]){
967 // downScale(i);
968 // initMovingEdge(*Ipyramid[i], cMo);
969 // upScale(i);
970 // }
971 // } while(i != 0);
972
973 // cleanPyramid(Ipyramid);
974 }
975
976 if (displayFeatures) {
978 }
979}
980
982 unsigned int lvl)
983{
987
988 if (lvl >= scales.size() || !scales[lvl]) {
989 throw vpException(vpException::dimensionError, "_lvl not used.");
990 }
991
992 unsigned int nbrow = initMbtTracking(lvl);
993
994 if (nbrow == 0) {
995 // throw vpTrackingException(vpTrackingException::notEnoughPointError,
996 // "Error: not enough features in the interaction matrix...");
997 return nbrow;
998 }
999
1000 factor.resize(nbrow, false);
1001 factor = 1;
1002
1003 unsigned int n = 0;
1004 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1005 if ((*it)->isTracked()) {
1006 l = *it;
1008
1009 double fac = 1;
1010 for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1011 ++itindex) {
1012 int index = *itindex;
1013 if (l->hiddenface->isAppearing((unsigned int)index)) {
1014 fac = 0.2;
1015 break;
1016 }
1017 if (l->closeToImageBorder(I, 10)) {
1018 fac = 0.1;
1019 break;
1020 }
1021 }
1022
1023 unsigned int indexFeature = 0;
1024 for (size_t a = 0; a < l->meline.size(); a++) {
1025 std::list<vpMeSite>::const_iterator itListLine;
1026 if (l->meline[a] != NULL) {
1027 itListLine = l->meline[a]->getMeList().begin();
1028
1029 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1030 factor[n + i] = fac;
1031 vpMeSite site = *itListLine;
1032 if (site.getState() != vpMeSite::NO_SUPPRESSION)
1033 factor[n + i] = 0.2;
1034 ++itListLine;
1035 indexFeature++;
1036 }
1037 n += l->nbFeature[a];
1038 }
1039 }
1040 }
1041 }
1042
1043 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1044 ++it) {
1045 if ((*it)->isTracked()) {
1046 cy = *it;
1048 double fac = 1.0;
1049
1050 std::list<vpMeSite>::const_iterator itCyl1;
1051 std::list<vpMeSite>::const_iterator itCyl2;
1052 if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1053 itCyl1 = cy->meline1->getMeList().begin();
1054 itCyl2 = cy->meline2->getMeList().begin();
1055 }
1056
1057 for (unsigned int i = 0; i < cy->nbFeature; i++) {
1058 factor[n + i] = fac;
1059 vpMeSite site;
1060 if (i < cy->nbFeaturel1) {
1061 site = *itCyl1;
1062 ++itCyl1;
1063 } else {
1064 site = *itCyl2;
1065 ++itCyl2;
1066 }
1067 if (site.getState() != vpMeSite::NO_SUPPRESSION)
1068 factor[n + i] = 0.2;
1069 }
1070
1071 n += cy->nbFeature;
1072 }
1073 }
1074
1075 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1076 if ((*it)->isTracked()) {
1077 ci = *it;
1079 double fac = 1.0;
1080
1081 std::list<vpMeSite>::const_iterator itCir;
1082 if (ci->meEllipse != NULL) {
1083 itCir = ci->meEllipse->getMeList().begin();
1084 }
1085
1086 for (unsigned int i = 0; i < ci->nbFeature; i++) {
1087 factor[n + i] = fac;
1088 vpMeSite site = *itCir;
1089 if (site.getState() != vpMeSite::NO_SUPPRESSION)
1090 factor[n + i] = 0.2;
1091 ++itCir;
1092 }
1093
1094 n += ci->nbFeature;
1095 }
1096 }
1097
1098 return nbrow;
1099}
1100
1102 const vpHomogeneousMatrix &cMo, unsigned int lvl)
1103{
1107
1108 unsigned int n = 0;
1109 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1110 if ((*it)->isTracked()) {
1111 l = *it;
1113 for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1114 for (unsigned int j = 0; j < 6; j++) {
1115 L[n + i][j] = l->L[i][j];
1116 error[n + i] = l->error[i];
1117 }
1118 }
1119 n += l->nbFeatureTotal;
1120 }
1121 }
1122
1123 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1124 ++it) {
1125 if ((*it)->isTracked()) {
1126 cy = *it;
1128 for (unsigned int i = 0; i < cy->nbFeature; i++) {
1129 for (unsigned int j = 0; j < 6; j++) {
1130 L[n + i][j] = cy->L[i][j];
1131 error[n + i] = cy->error[i];
1132 }
1133 }
1134 n += cy->nbFeature;
1135 }
1136 }
1137 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1138 if ((*it)->isTracked()) {
1139 ci = *it;
1141 for (unsigned int i = 0; i < ci->nbFeature; i++) {
1142 for (unsigned int j = 0; j < 6; j++) {
1143 L[n + i][j] = ci->L[i][j];
1144 error[n + i] = ci->error[i];
1145 }
1146 }
1147
1148 n += ci->nbFeature;
1149 }
1150 }
1151}
1152
1159{
1160 m_cam = cam;
1161
1164}
1165
1173{
1176}
1184{
1187}
1188
1199void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1200 int idFace, const std::string &name)
1201{
1202 vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1203}
1204
1215void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1216 const std::string &name)
1217{
1218 vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1219 vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1220}
1221
1234 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1235 bool displayFullModel)
1236{
1237 std::vector<std::vector<double> > models = vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1238
1239 for (size_t i = 0; i < models.size(); i++) {
1240 if (vpMath::equal(models[i][0], 0)) {
1241 vpImagePoint ip1(models[i][1], models[i][2]);
1242 vpImagePoint ip2(models[i][3], models[i][4]);
1243 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1244 } else if (vpMath::equal(models[i][0], 1)) {
1245 vpImagePoint center(models[i][1], models[i][2]);
1246 double n20 = models[i][3];
1247 double n11 = models[i][4];
1248 double n02 = models[i][5];
1249 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1250 }
1251 }
1252
1253 if (displayFeatures) {
1254 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1258
1260 double id = m_featuresToBeDisplayedKlt[i][5];
1261 std::stringstream ss;
1262 ss << id;
1263 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1264 }
1265 }
1266 }
1267
1268#ifdef VISP_HAVE_OGRE
1269 if (useOgre)
1270 faces.displayOgre(cMo);
1271#endif
1272}
1273
1286 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1287 bool displayFullModel)
1288{
1289 std::vector<std::vector<double> > models = 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 if (displayFeatures) {
1306 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1310
1312 double id = m_featuresToBeDisplayedKlt[i][5];
1313 std::stringstream ss;
1314 ss << id;
1315 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1316 }
1317 }
1318 }
1319
1320#ifdef VISP_HAVE_OGRE
1321 if (useOgre)
1322 faces.displayOgre(cMo);
1323#endif
1324}
1325
1326std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1327 const vpHomogeneousMatrix &cMo,
1328 const vpCameraParameters &cam,
1329 bool displayFullModel)
1330{
1331 std::vector<std::vector<double> > models;
1332
1333 for (unsigned int i = 0; i < scales.size(); i += 1) {
1334 if (scales[i]) {
1335 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1336 ++it) {
1337 std::vector<std::vector<double> > currentModel =
1338 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1339 models.insert(models.end(), currentModel.begin(), currentModel.end());
1340 }
1341
1342 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1343 it != cylinders[scaleLevel].end(); ++it) {
1344 std::vector<std::vector<double> > currentModel =
1345 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1346 models.insert(models.end(), currentModel.begin(), currentModel.end());
1347 }
1348
1349 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1350 it != circles[scaleLevel].end(); ++it) {
1351 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1352 models.push_back(paramsCircle);
1353 }
1354
1355 break; // displaying model on one scale only
1356 }
1357 }
1358
1359#ifdef VISP_HAVE_OGRE
1360 if (useOgre)
1361 faces.displayOgre(cMo);
1362#endif
1363
1364 return models;
1365}
1366
1379void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1380 const vpHomogeneousMatrix &cMo, bool verbose,
1381 const vpHomogeneousMatrix &T)
1382{
1383 // Reinit klt
1384 #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1385 if (cur != NULL) {
1386 cvReleaseImage(&cur);
1387 cur = NULL;
1388 }
1389 #endif
1390
1391 // delete the Klt Polygon features
1392 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1393 vpMbtDistanceKltPoints *kltpoly = *it;
1394 if (kltpoly != NULL) {
1395 delete kltpoly;
1396 }
1397 kltpoly = NULL;
1398 }
1399 kltPolygons.clear();
1400
1401 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1402 ++it) {
1403 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1404 if (kltPolyCylinder != NULL) {
1405 delete kltPolyCylinder;
1406 }
1407 kltPolyCylinder = NULL;
1408 }
1409 kltCylinders.clear();
1410
1411 // delete the structures used to display circles
1413 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1414 ci = *it;
1415 if (ci != NULL) {
1416 delete ci;
1417 }
1418 ci = NULL;
1419 }
1420
1421 circles_disp.clear();
1422
1423 firstInitialisation = true;
1424
1425 // Reinit edge
1428
1429 for (unsigned int i = 0; i < scales.size(); i += 1) {
1430 if (scales[i]) {
1431 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1432 l = *it;
1433 if (l != NULL)
1434 delete l;
1435 l = NULL;
1436 }
1437
1438 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1439 ++it) {
1440 cy = *it;
1441 if (cy != NULL)
1442 delete cy;
1443 cy = NULL;
1444 }
1445
1446 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1447 ci = *it;
1448 if (ci != NULL)
1449 delete ci;
1450 ci = NULL;
1451 }
1452
1453 lines[i].clear();
1454 cylinders[i].clear();
1455 circles[i].clear();
1456 }
1457 }
1458
1459 // compute_interaction=1;
1460 nline = 0;
1461 ncylinder = 0;
1462 ncircle = 0;
1463 // lambda = 1;
1465
1466 // Reinit common parts
1467 faces.reset();
1468
1469 loadModel(cad_name, verbose, T);
1470
1471 m_cMo = cMo;
1472 init(I);
1473}
1474
1475#elif !defined(VISP_BUILD_SHARED_LIBS)
1476// Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1477// no symbols
1478void dummy_vpMbEdgeKltTracker(){};
1479#endif // VISP_HAVE_OPENCV
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.
void computeFov(const unsigned int &w, const unsigned int &h)
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 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)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
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
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:175
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
unsigned int getHeight() const
Definition: vpImage.h:188
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:110
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 double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
virtual void setClipping(const unsigned int &flags)
virtual void setFarClippingDistance(const double &dist)
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void initCylinder(const vpPoint &, const vpPoint &, double r, int idFace, const std::string &name="")
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void computeVVSInit()
vpColVector m_w_mbt
Robust weights for Edge.
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w_mbt, vpColVector &w_klt, unsigned int lvl=0)
unsigned int trackFirstLoop(const vpImage< unsigned char > &I, vpColVector &factor, unsigned int lvl=0)
void postTrackingMbt(vpColVector &w, unsigned int level=0)
void trackSecondLoop(const vpImage< unsigned char > &I, vpMatrix &L, vpColVector &_error, const vpHomogeneousMatrix &cMo, unsigned int lvl=0)
unsigned int initMbtTracking(unsigned int level=0)
virtual void computeVVSInteractionMatrixAndResidu()
virtual void init(const vpImage< unsigned char > &I)
void reInitModel(const vpImage< unsigned char > &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
vpColVector m_error_hybrid
(s - s*)
virtual void setNearClippingDistance(const double &dist)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double r, int idFace=0, const std::string &name="")
vpColVector m_w_klt
Robust weights for KLT.
virtual void track(const vpImage< unsigned char > &I)
double m_thresholdMBT
The threshold used in the robust estimation of MBT.
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
unsigned int m_maxIterKlt
The maximum iteration of the virtual visual servoing stage.
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpColVector m_w_hybrid
Robust weights.
void upScale(const unsigned int _scale)
virtual void setCameraParameters(const vpCameraParameters &cam)
virtual void initFaceFromLines(vpMbtPolygon &polygon)
std::vector< std::list< vpMbtDistanceLine * > > lines
vpMe me
The moving edges parameters.
void computeProjectionError(const vpImage< unsigned char > &_I)
unsigned int ncylinder
void downScale(const unsigned int _scale)
void cleanPyramid(std::vector< const vpImage< unsigned char > * > &_pyramid)
std::vector< std::list< vpMbtDistanceCylinder * > > cylinders
Vector of the tracked cylinders.
void initPyramid(const vpImage< unsigned char > &_I, std::vector< const vpImage< unsigned char > * > &_pyramid)
unsigned int nbvisiblepolygone
Number of polygon (face) currently visible.
std::vector< std::list< vpMbtDistanceCircle * > > circles
Vector of the tracked circles.
void initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
unsigned int scaleLevel
void trackMovingEdge(const vpImage< unsigned char > &I)
std::vector< const vpImage< unsigned char > * > Ipyramid
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 updateMovingEdge(const vpImage< unsigned char > &I)
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo)
void setMovingEdge(const vpMe &me)
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
unsigned int nline
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
bool isAppearing(unsigned int i)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpAROgre * getOgreContext()
void displayOgre(const vpHomogeneousMatrix &cMo)
vpMbScanLine & getMbScanLineRenderer()
std::list< vpMbtDistanceKltCylinder * > kltCylinders
vpHomogeneousMatrix c0Mo
Initial pose.
vpHomogeneousMatrix ctTc0
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
cv::Mat cur
Temporary OpenCV image for fast conversion.
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
vpKltOpencv tracker
Points tracker.
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
void preTracking(const vpImage< unsigned char > &I)
unsigned int m_nbInfos
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
virtual void reinit(const vpImage< unsigned char > &I)
unsigned int maskBorder
Erosion of the mask.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
void setCameraParameters(const vpCameraParameters &cam)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void init(const vpImage< unsigned char > &I)
double m_lambda
Gain of the virtual visual servoing stage.
Definition: vpMbTracker.h:187
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
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
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 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="")
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
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
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="")
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
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
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
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
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.
vpColVector error
The error vector.
unsigned int nbFeature
The number of moving edges.
vpMatrix L
The interaction matrix.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
void setMeanWeight(double _wmean)
void displayMovingEdges(const vpImage< unsigned char > &I)
bool Reinit
Indicates if the circle has to be reinitialized.
vpMbtMeEllipse * meEllipse
The moving edge containers.
Manage a cylinder used in the model-based tracker.
void setMeanWeight1(double wmean)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage< unsigned char > &I)
vpMbtMeLine * meline2
The moving edge containers (second line of the cylinder)
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.
void setMeanWeight2(double wmean)
unsigned int nbFeaturel1
The number of moving edges on line 1.
vpColVector error
The error vector.
void displayMovingEdges(const vpImage< unsigned char > &I)
unsigned int nbFeature
The number of moving edges.
vpMbtMeLine * meline1
The moving edge containers (first line of the cylinder)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
unsigned int getCurrentNumberPoints() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
unsigned int getCurrentNumberPoints() const
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
Manage the line of a polygon used in the model-based tracker.
std::vector< unsigned int > nbFeature
The number of moving edges.
void displayMovingEdges(const vpImage< unsigned char > &I)
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool isVisible() const
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
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)
bool isTracked() const
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setMeanWeight(double w_mean)
Implementation of a polygon of the model used by the model-based tracker.
Definition: vpMbtPolygon.h:67
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
Parse an Xml file to extract configuration parameters of a mbtConfig object.
unsigned int getKltMaxFeatures() const
void setKltMinDistance(const double &mD)
unsigned int getKltBlockSize() const
void getCameraParameters(vpCameraParameters &cam) const
void setEdgeMe(const vpMe &ecm)
void setKltMaskBorder(const unsigned int &mb)
void getEdgeMe(vpMe &ecm) const
double getLodMinLineLengthThreshold() const
unsigned int getKltMaskBorder() const
void setAngleDisappear(const double &adisappear)
void setKltPyramidLevels(const unsigned int &pL)
void setKltWindowSize(const unsigned int &w)
void setKltMaxFeatures(const unsigned int &mF)
void setAngleAppear(const double &aappear)
void setKltBlockSize(const unsigned int &bs)
void setKltHarrisParam(const double &hp)
void parse(const std::string &filename)
void setKltQuality(const double &q)
unsigned int getKltPyramidLevels() const
unsigned int getKltWindowSize() const
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
@ 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
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ 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
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition: vpSubMatrix.h:63
Error that can be emited by the vpTracker class and its derivates.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:393