Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbEdgeKltTracker.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 * Hybrid tracker based on edges (vpMbt) and points of interests (KLT)
33 *
34*****************************************************************************/
35
36//#define VP_DEBUG_MODE 1 // Activate debug level 1
37
38#include <visp3/core/vpDebug.h>
39#include <visp3/core/vpTrackingException.h>
40#include <visp3/core/vpVelocityTwistMatrix.h>
41#include <visp3/mbt/vpMbEdgeKltTracker.h>
42#include <visp3/mbt/vpMbtXmlGenericParser.h>
43
44#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
45
47 : m_thresholdKLT(2.), m_thresholdMBT(2.), m_maxIterKlt(30), m_w_mbt(), m_w_klt(), m_error_hybrid(), m_w_hybrid()
48{
49 computeCovariance = false;
50
51#ifdef VISP_HAVE_OGRE
52 faces.getOgreContext()->setWindowName("MBT Hybrid");
53#endif
54
55 m_lambda = 0.8;
56 m_maxIter = 200;
57}
58
64
72{
74
76
78
79 unsigned int i = (unsigned int)scales.size();
80 do {
81 i--;
82 if (scales[i]) {
83 downScale(i);
85 upScale(i);
86 }
87 } while (i != 0);
88
90}
91
102{
104
106
107 if (useScanLine) {
111 }
112
114
115 unsigned int i = (unsigned int)scales.size();
116 do {
117 i--;
118 if (scales[i]) {
119 downScale(i);
121 upScale(i);
122 }
123 } while (i != 0);
124
126}
127
138{
141
143
144 if (useScanLine) {
148 }
149
151
152 unsigned int i = (unsigned int)scales.size();
153 do {
154 i--;
155 if (scales[i]) {
156 downScale(i);
158 upScale(i);
159 }
160 } while (i != 0);
161
163}
164
174
175unsigned int vpMbEdgeKltTracker::initMbtTracking(unsigned int lvl)
176{
177 if (lvl >= scales.size() || !scales[lvl]) {
178 throw vpException(vpException::dimensionError, "lvl not used.");
179 }
180
181 unsigned int nbrow = 0;
182 for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
183 vpMbtDistanceLine *l = *it;
184
185 if (l->isTracked()) {
187 nbrow += l->nbFeatureTotal;
188 }
189 }
190
191 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
192 ++it) {
193 vpMbtDistanceCylinder *cy = *it;
194
195 if (cy->isTracked()) {
197 nbrow += cy->nbFeature;
198 }
199 }
200
201 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
202 vpMbtDistanceCircle *ci = *it;
203
204 if (ci->isTracked()) {
206 nbrow += ci->nbFeature;
207 }
208 }
209
210 return nbrow;
211}
212
274void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
275{
276 // Load projection error config
277 vpMbTracker::loadConfigFile(configFile, verbose);
278
280 xmlp.setVerbose(verbose);
284
285 xmlp.setEdgeMe(me);
286
287 xmlp.setKltMaxFeatures(10000);
288 xmlp.setKltWindowSize(5);
289 xmlp.setKltQuality(0.01);
290 xmlp.setKltMinDistance(5);
291 xmlp.setKltHarrisParam(0.01);
292 xmlp.setKltBlockSize(3);
293 xmlp.setKltPyramidLevels(3);
295
296 try {
297 if (verbose) {
298 std::cout << " *********** Parsing XML for Mb Edge KLT Tracker ************ " << std::endl;
299 }
300 xmlp.parse(configFile.c_str());
301 }
302 catch (...) {
303 vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
304 throw vpException(vpException::ioError, "problem to parse configuration file.");
305 }
306
307 vpCameraParameters camera;
308 xmlp.getCameraParameters(camera);
309 setCameraParameters(camera);
310
313
314 if (xmlp.hasNearClippingDistance())
316
317 if (xmlp.hasFarClippingDistance())
319
320 if (xmlp.getFovClipping()) {
322 }
323
324 useLodGeneral = xmlp.getLodState();
327
329 if (this->getNbPolygon() > 0) {
334 }
335
336 vpMe meParser;
337 xmlp.getEdgeMe(meParser);
339
348
349 // if(useScanLine)
350 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
351}
352
357 unsigned int lvl)
358{
359 postTrackingMbt(w_mbt, lvl);
360
361 if (displayFeatures) {
362 if (lvl == 0) {
363 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
364 vpMbtDistanceLine *l = *it;
365 if (l->isVisible() && l->isTracked()) {
366 l->displayMovingEdges(I);
367 }
368 }
369
370 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
371 ++it) {
372 vpMbtDistanceCylinder *cy = *it;
373 // A cylinder is always visible: #FIXME AY: Still valid?
374 if (cy->isTracked())
375 cy->displayMovingEdges(I);
376 }
377
378 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
379 vpMbtDistanceCircle *ci = *it;
380 if (ci->isVisible() && ci->isTracked()) {
381 ci->displayMovingEdges(I);
382 }
383 }
384 }
385 }
386
387 bool reInit = vpMbKltTracker::postTracking(I, w_klt);
388
389 if (useScanLine) {
393 }
394
396
399
402
403 if (reInit)
404 return true;
405
406 return false;
407}
408
413 unsigned int lvl)
414{
415 postTrackingMbt(w_mbt, lvl);
416
417 if (displayFeatures) {
418 if (lvl == 0) {
419 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
420 vpMbtDistanceLine *l = *it;
421 if (l->isVisible() && l->isTracked()) {
422 l->displayMovingEdges(I_color);
423 }
424 }
425
426 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
427 ++it) {
428 vpMbtDistanceCylinder *cy = *it;
429 // A cylinder is always visible: #FIXME AY: Still valid?
430 if (cy->isTracked())
432 }
433
434 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
435 vpMbtDistanceCircle *ci = *it;
436 if (ci->isVisible() && ci->isTracked()) {
438 }
439 }
440 }
441 }
442
443 bool reInit = vpMbKltTracker::postTracking(m_I, w_klt);
444
445 if (useScanLine) {
449 }
450
452
455
458
459 if (reInit)
460 return true;
461
462 return false;
463}
464
476{
477 if (lvl >= scales.size() || !scales[lvl]) {
478 throw vpException(vpException::dimensionError, "_lvl not used.");
479 }
480
481 unsigned int n = 0;
483 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
484 if ((*it)->isTracked()) {
485 l = *it;
486 unsigned int indexLine = 0;
487 double wmean = 0;
488
489 for (size_t a = 0; a < l->meline.size(); a++) {
490 std::list<vpMeSite>::iterator itListLine;
491 if (l->nbFeature[a] > 0)
492 itListLine = l->meline[a]->getMeList().begin();
493
494 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
495 wmean += w[n + indexLine];
496 vpMeSite p = *itListLine;
497 if (w[n + indexLine] < 0.5) {
499 *itListLine = p;
500 }
501
502 ++itListLine;
503 indexLine++;
504 }
505 }
506
507 n += l->nbFeatureTotal;
508
509 if (l->nbFeatureTotal != 0)
510 wmean /= l->nbFeatureTotal;
511 else
512 wmean = 1;
513
514 l->setMeanWeight(wmean);
515
516 if (wmean < 0.8)
517 l->Reinit = true;
518 }
519 }
520
521 // Same thing with cylinders as with lines
523 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
524 ++it) {
525 if ((*it)->isTracked()) {
526 cy = *it;
527 double wmean = 0;
528 std::list<vpMeSite>::iterator itListCyl1;
529 std::list<vpMeSite>::iterator itListCyl2;
530 if (cy->nbFeature > 0) {
531 itListCyl1 = cy->meline1->getMeList().begin();
532 itListCyl2 = cy->meline2->getMeList().begin();
533 }
534
535 wmean = 0;
536 for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
537 wmean += w[n + i];
538 vpMeSite p = *itListCyl1;
539 if (w[n + i] < 0.5) {
541
542 *itListCyl1 = p;
543 }
544
545 ++itListCyl1;
546 }
547
548 if (cy->nbFeaturel1 != 0)
549 wmean /= cy->nbFeaturel1;
550 else
551 wmean = 1;
552
553 cy->setMeanWeight1(wmean);
554
555 if (wmean < 0.8) {
556 cy->Reinit = true;
557 }
558
559 wmean = 0;
560 for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
561 wmean += w[n + i];
562 vpMeSite p = *itListCyl2;
563 if (w[n + i] < 0.5) {
565
566 *itListCyl2 = p;
567 }
568
569 ++itListCyl2;
570 }
571
572 if (cy->nbFeaturel2 != 0)
573 wmean /= cy->nbFeaturel2;
574 else
575 wmean = 1;
576
577 cy->setMeanWeight2(wmean);
578
579 if (wmean < 0.8) {
580 cy->Reinit = true;
581 }
582
583 n += cy->nbFeature;
584 }
585 }
586
587 // Same thing with circles as with lines
589 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
590 if ((*it)->isTracked()) {
591 ci = *it;
592 double wmean = 0;
593 std::list<vpMeSite>::iterator itListCir;
594
595 if (ci->nbFeature > 0) {
596 itListCir = ci->meEllipse->getMeList().begin();
597 }
598
599 wmean = 0;
600 for (unsigned int i = 0; i < ci->nbFeature; i++) {
601 wmean += w[n + i];
602 vpMeSite p = *itListCir;
603 if (w[n + i] < 0.5) {
605
606 *itListCir = p;
607 }
608
609 ++itListCir;
610 }
611
612 if (ci->nbFeature != 0)
613 wmean /= ci->nbFeature;
614 else
615 wmean = 1;
616
617 ci->setMeanWeight(wmean);
618
619 if (wmean < 0.8) {
620 ci->Reinit = true;
621 }
622
623 n += ci->nbFeature;
624 }
625 }
626}
627
638void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
639 unsigned int lvl, double *edge_residual, double *klt_residual)
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 }
647 else if (nbrow < 4)
648 nbrow = 0;
649
650 unsigned int totalNbRows = nbrow + 2 * nbInfos;
651 double residu = 0;
652 double residu_1 = -1;
653 unsigned int iter = 0;
654
655 vpMatrix L(totalNbRows, 6);
656 vpMatrix L_mbt, L_klt; // interaction matrix
657 vpColVector weighted_error(totalNbRows);
658 vpColVector R_mbt, R_klt; // residu
659 vpMatrix L_true;
660 vpMatrix LVJ_true;
661
662 if (nbrow != 0) {
663 L_mbt.resize(nbrow, 6, false, false);
664 R_mbt.resize(nbrow, false);
665 }
666
667 if (nbInfos != 0) {
668 L_klt.resize(2 * nbInfos, 6, false, false);
669 R_klt.resize(2 * nbInfos, false);
670 }
671
672 vpColVector v; // "speed" for VVS
673 vpRobust robust_mbt, robust_klt;
674 vpHomography H;
675
676 vpMatrix LTL;
677 vpColVector LTR;
678
679 double factorMBT; // = 1.0;
680 double factorKLT; // = 1.0;
681
682 // More efficient weight repartition for hybrid tracker should come soon...
683 // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
684 // factorKLT = 1.0 - factorMBT;
685 factorMBT = 0.35;
686 factorKLT = 0.65;
687
688 if (nbrow < 4)
689 factorKLT = 1.;
690 if (nbInfos < 4)
691 factorMBT = 1.;
692
693 if (edge_residual != NULL)
694 *edge_residual = 0;
695 if (klt_residual != NULL)
696 *klt_residual = 0;
697
698 vpHomogeneousMatrix cMoPrev;
699 vpHomogeneousMatrix ctTc0_Prev;
700 vpColVector m_error_prev;
701 vpColVector m_w_prev;
702
703 bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
704 if (isoJoIdentity)
705 oJo.eye();
706
707 // Init size
708 m_error_hybrid.resize(totalNbRows, false);
709 m_w_hybrid.resize(totalNbRows, false);
710
711 if (nbrow != 0) {
712 m_w_mbt.resize(nbrow, false);
713 m_w_mbt = 1; // needed in vpRobust::psiTukey()
714 }
715
716 if (nbInfos != 0) {
717 m_w_klt.resize(2 * nbInfos, false);
718 m_w_klt = 1; // needed in vpRobust::psiTukey()
719 }
720
721 double mu = m_initialMu;
722
723 while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
724 if (nbrow >= 4)
725 trackSecondLoop(I, L_mbt, R_mbt, m_cMo, lvl);
726
727 if (nbInfos >= 4) {
728 unsigned int shift = 0;
729
730 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = vpMbKltTracker::kltPolygons.begin();
731 it != vpMbKltTracker::kltPolygons.end(); ++it) {
732 vpMbtDistanceKltPoints *kltpoly = *it;
733 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()) {
734 vpSubColVector subR(R_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
735 vpSubMatrix subL(L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
736 kltpoly->computeHomography(ctTc0, H);
737 kltpoly->computeInteractionMatrixAndResidu(subR, subL);
738 shift += 2 * kltpoly->getCurrentNumberPoints();
739 }
740 }
741
742 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
743 ++it) {
744 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
745
746 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
747 vpSubColVector subR(R_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
748 vpSubMatrix subL(L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
749 try {
750 kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
751 }
752 catch (...) {
753 throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
754 }
755
756 shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
757 }
758 }
759 }
760
761 /* residuals */
762 if (nbrow > 3) {
763 m_error_hybrid.insert(0, R_mbt);
764 }
765
766 if (nbInfos > 3) {
767 m_error_hybrid.insert(nbrow, R_klt);
768 }
769
770 unsigned int cpt = 0;
771 while (cpt < (nbrow + 2 * nbInfos)) {
772 if (cpt < (unsigned)nbrow) {
773 m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
774 }
775 else {
776 m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
777 }
778 cpt++;
779 }
780
781 bool reStartFromLastIncrement = false;
782 computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
783 &m_w_prev);
784 if (reStartFromLastIncrement) {
785 ctTc0 = ctTc0_Prev;
786 }
787
788 if (!reStartFromLastIncrement) {
789 /* robust */
790 if (nbrow > 3) {
791 if (edge_residual != NULL) {
792 *edge_residual = 0;
793 for (unsigned int i = 0; i < R_mbt.getRows(); i++)
794 *edge_residual += fabs(R_mbt[i]);
795 *edge_residual /= R_mbt.getRows();
796 }
797
799 robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, m_w_mbt);
800
801 L.insert(L_mbt, 0, 0);
802 }
803
804 if (nbInfos > 3) {
805 if (klt_residual != NULL) {
806 *klt_residual = 0;
807 for (unsigned int i = 0; i < R_klt.getRows(); i++)
808 *klt_residual += fabs(R_klt[i]);
809 *klt_residual /= R_klt.getRows();
810 }
811
813 robust_klt.MEstimator(vpRobust::TUKEY, R_klt, m_w_klt);
814
815 L.insert(L_klt, nbrow, 0);
816 }
817
818 cpt = 0;
819 while (cpt < (nbrow + 2 * nbInfos)) {
820 if (cpt < (unsigned)nbrow) {
821 m_w_hybrid[cpt] = ((m_w_mbt[cpt] * factor[cpt]) * factorMBT);
822 }
823 else {
824 m_w_hybrid[cpt] = (m_w_klt[cpt - nbrow] * factorKLT);
825 }
826 cpt++;
827 }
828
829 if (computeCovariance) {
830 L_true = L;
831 if (!isoJoIdentity) {
833 cVo.buildFrom(m_cMo);
834 LVJ_true = (L * cVo * oJo);
835 }
836 }
837
838 residu_1 = residu;
839 residu = 0;
840 double num = 0;
841 double den = 0;
842
843 for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
844 num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
845 den += m_w_hybrid[i];
846
847 weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
849 for (unsigned int j = 0; j < 6; j += 1) {
850 L[i][j] *= m_w_hybrid[i];
851 }
852 }
853 }
854
855 residu = sqrt(num / den);
856
857 computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
858 &m_w_hybrid, &m_w_prev);
859
860 cMoPrev = m_cMo;
861 ctTc0_Prev = ctTc0;
863 m_cMo = ctTc0 * c0Mo;
864 }
865
866 iter++;
867 }
868
869 computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybrid, cMoPrev, L_true, LVJ_true, m_error_hybrid);
870}
871
873{
874 throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::computeVVSInit() should not be called!");
875}
876
878{
879 throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
880 "computeVVSInteractionMatrixAndR"
881 "esidu() should not be called!");
882}
883
892{
893 try {
895 }
896 catch (...) {
897 }
898
899 if (m_nbInfos >= 4) {
900 unsigned int old_maxIter = m_maxIter;
903 m_maxIter = old_maxIter;
904 }
905 else {
906 m_nbInfos = 0;
907 // std::cout << "[Warning] Unable to init with KLT" << std::endl;
908 }
909
911
912 unsigned int nbrow = 0;
913 computeVVS(I, m_nbInfos, nbrow);
914
915 if (postTracking(I, m_w_mbt, m_w_klt)) {
917
918 // AY : Removed as edge tracked, if necessary, is reinitialized in
919 // postTracking()
920
921 // initPyramid(I, Ipyramid);
922
923 // unsigned int i = (unsigned int)scales.size();
924 // do {
925 // i--;
926 // if(scales[i]){
927 // downScale(i);
928 // initMovingEdge(*Ipyramid[i], cMo);
929 // upScale(i);
930 // }
931 // } while(i != 0);
932
933 // cleanPyramid(Ipyramid);
934 }
935
936 if (displayFeatures) {
938 }
939}
940
949{
951 try {
953 }
954 catch (...) {
955 }
956
957 if (m_nbInfos >= 4) {
958 unsigned int old_maxIter = m_maxIter;
961 m_maxIter = old_maxIter;
962 }
963 else {
964 m_nbInfos = 0;
965 // std::cout << "[Warning] Unable to init with KLT" << std::endl;
966 }
967
969
970 unsigned int nbrow = 0;
971 computeVVS(m_I, m_nbInfos, nbrow);
972
973 if (postTracking(I_color, m_w_mbt, m_w_klt)) {
975
976 // AY : Removed as edge tracked, if necessary, is reinitialized in
977 // postTracking()
978
979 // initPyramid(I, Ipyramid);
980
981 // unsigned int i = (unsigned int)scales.size();
982 // do {
983 // i--;
984 // if(scales[i]){
985 // downScale(i);
986 // initMovingEdge(*Ipyramid[i], cMo);
987 // upScale(i);
988 // }
989 // } while(i != 0);
990
991 // cleanPyramid(Ipyramid);
992 }
993
994 if (displayFeatures) {
996 }
997}
998
999unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, unsigned int lvl)
1000{
1004
1005 if (lvl >= scales.size() || !scales[lvl]) {
1006 throw vpException(vpException::dimensionError, "_lvl not used.");
1007 }
1008
1009 unsigned int nbrow = initMbtTracking(lvl);
1010
1011 if (nbrow == 0) {
1012 // throw vpTrackingException(vpTrackingException::notEnoughPointError,
1013 // "Error: not enough features in the interaction matrix...");
1014 return nbrow;
1015 }
1016
1017 factor.resize(nbrow, false);
1018 factor = 1;
1019
1020 unsigned int n = 0;
1021 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1022 if ((*it)->isTracked()) {
1023 l = *it;
1025
1026 double fac = 1;
1027 for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
1028 ++itindex) {
1029 int index = *itindex;
1030 if (l->hiddenface->isAppearing((unsigned int)index)) {
1031 fac = 0.2;
1032 break;
1033 }
1034 if (l->closeToImageBorder(I, 10)) {
1035 fac = 0.1;
1036 break;
1037 }
1038 }
1039
1040 for (size_t a = 0; a < l->meline.size(); a++) {
1041 std::list<vpMeSite>::const_iterator itListLine;
1042 if (l->meline[a] != NULL) {
1043 itListLine = l->meline[a]->getMeList().begin();
1044
1045 for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
1046 factor[n + i] = fac;
1047 vpMeSite site = *itListLine;
1048 if (site.getState() != vpMeSite::NO_SUPPRESSION)
1049 factor[n + i] = 0.2;
1050 ++itListLine;
1051 }
1052 n += l->nbFeature[a];
1053 }
1054 }
1055 }
1056 }
1057
1058 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1059 ++it) {
1060 if ((*it)->isTracked()) {
1061 cy = *it;
1063 double fac = 1.0;
1064
1065 std::list<vpMeSite>::const_iterator itCyl1;
1066 std::list<vpMeSite>::const_iterator itCyl2;
1067 if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
1068 itCyl1 = cy->meline1->getMeList().begin();
1069 itCyl2 = cy->meline2->getMeList().begin();
1070 }
1071
1072 for (unsigned int i = 0; i < cy->nbFeature; i++) {
1073 factor[n + i] = fac;
1074 vpMeSite site;
1075 if (i < cy->nbFeaturel1) {
1076 site = *itCyl1;
1077 ++itCyl1;
1078 }
1079 else {
1080 site = *itCyl2;
1081 ++itCyl2;
1082 }
1083 if (site.getState() != vpMeSite::NO_SUPPRESSION)
1084 factor[n + i] = 0.2;
1085 }
1086
1087 n += cy->nbFeature;
1088 }
1089 }
1090
1091 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1092 if ((*it)->isTracked()) {
1093 ci = *it;
1095 double fac = 1.0;
1096
1097 std::list<vpMeSite>::const_iterator itCir;
1098 if (ci->meEllipse != NULL) {
1099 itCir = ci->meEllipse->getMeList().begin();
1100 }
1101
1102 for (unsigned int i = 0; i < ci->nbFeature; i++) {
1103 factor[n + i] = fac;
1104 vpMeSite site = *itCir;
1105 if (site.getState() != vpMeSite::NO_SUPPRESSION)
1106 factor[n + i] = 0.2;
1107 ++itCir;
1108 }
1109
1110 n += ci->nbFeature;
1111 }
1112 }
1113
1114 return nbrow;
1115}
1116
1118 const vpHomogeneousMatrix &cMo, unsigned int lvl)
1119{
1123
1124 unsigned int n = 0;
1125 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
1126 if ((*it)->isTracked()) {
1127 l = *it;
1129 for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
1130 for (unsigned int j = 0; j < 6; j++) {
1131 L[n + i][j] = l->L[i][j];
1132 error[n + i] = l->error[i];
1133 }
1134 }
1135 n += l->nbFeatureTotal;
1136 }
1137 }
1138
1139 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
1140 ++it) {
1141 if ((*it)->isTracked()) {
1142 cy = *it;
1144 for (unsigned int i = 0; i < cy->nbFeature; i++) {
1145 for (unsigned int j = 0; j < 6; j++) {
1146 L[n + i][j] = cy->L[i][j];
1147 error[n + i] = cy->error[i];
1148 }
1149 }
1150 n += cy->nbFeature;
1151 }
1152 }
1153 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
1154 if ((*it)->isTracked()) {
1155 ci = *it;
1157 for (unsigned int i = 0; i < ci->nbFeature; i++) {
1158 for (unsigned int j = 0; j < 6; j++) {
1159 L[n + i][j] = ci->L[i][j];
1160 error[n + i] = ci->error[i];
1161 }
1162 }
1163
1164 n += ci->nbFeature;
1165 }
1166 }
1167}
1168
1181
1204
1215void vpMbEdgeKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int idFace,
1216 const std::string &name)
1217{
1218 vpMbEdgeTracker::initCircle(p1, p2, p3, radius, idFace, name);
1219}
1220
1231void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1232 const std::string &name)
1233{
1234 vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
1235 vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
1236}
1237
1250 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1251 bool displayFullModel)
1252{
1253 std::vector<std::vector<double> > models =
1254 vpMbEdgeKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1255
1256 for (size_t i = 0; i < models.size(); i++) {
1257 if (vpMath::equal(models[i][0], 0)) {
1258 vpImagePoint ip1(models[i][1], models[i][2]);
1259 vpImagePoint ip2(models[i][3], models[i][4]);
1260 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1261 }
1262 else if (vpMath::equal(models[i][0], 1)) {
1263 vpImagePoint center(models[i][1], models[i][2]);
1264 double n20 = models[i][3];
1265 double n11 = models[i][4];
1266 double n02 = models[i][5];
1267 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1268 }
1269 }
1270
1271 if (displayFeatures) {
1272 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1276
1278 double id = m_featuresToBeDisplayedKlt[i][5];
1279 std::stringstream ss;
1280 ss << id;
1281 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1282 }
1283 }
1284 }
1285
1286#ifdef VISP_HAVE_OGRE
1287 if (useOgre)
1288 faces.displayOgre(cMo);
1289#endif
1290}
1291
1304 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1305 bool displayFullModel)
1306{
1307 std::vector<std::vector<double> > models =
1308 getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1309
1310 for (size_t i = 0; i < models.size(); i++) {
1311 if (vpMath::equal(models[i][0], 0)) {
1312 vpImagePoint ip1(models[i][1], models[i][2]);
1313 vpImagePoint ip2(models[i][3], models[i][4]);
1314 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1315 }
1316 else if (vpMath::equal(models[i][0], 1)) {
1317 vpImagePoint center(models[i][1], models[i][2]);
1318 double n20 = models[i][3];
1319 double n11 = models[i][4];
1320 double n02 = models[i][5];
1321 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1322 }
1323 }
1324
1325 if (displayFeatures) {
1326 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1330
1332 double id = m_featuresToBeDisplayedKlt[i][5];
1333 std::stringstream ss;
1334 ss << id;
1335 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1336 }
1337 }
1338 }
1339
1340#ifdef VISP_HAVE_OGRE
1341 if (useOgre)
1342 faces.displayOgre(cMo);
1343#endif
1344}
1345
1346std::vector<std::vector<double> > vpMbEdgeKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1347 const vpHomogeneousMatrix &cMo,
1348 const vpCameraParameters &cam,
1349 bool displayFullModel)
1350{
1351 std::vector<std::vector<double> > models;
1352
1353 for (unsigned int i = 0; i < scales.size(); i += 1) {
1354 if (scales[i]) {
1355 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
1356 ++it) {
1357 std::vector<std::vector<double> > currentModel =
1358 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1359 models.insert(models.end(), currentModel.begin(), currentModel.end());
1360 }
1361
1362 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
1363 it != cylinders[scaleLevel].end(); ++it) {
1364 std::vector<std::vector<double> > currentModel =
1365 (*it)->getModelForDisplay(width, height, cMo, cam, displayFullModel);
1366 models.insert(models.end(), currentModel.begin(), currentModel.end());
1367 }
1368
1369 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
1370 it != circles[scaleLevel].end(); ++it) {
1371 std::vector<double> paramsCircle = (*it)->getModelForDisplay(cMo, cam, displayFullModel);
1372 models.push_back(paramsCircle);
1373 }
1374
1375 break; // displaying model on one scale only
1376 }
1377 }
1378
1379#ifdef VISP_HAVE_OGRE
1380 if (useOgre)
1381 faces.displayOgre(cMo);
1382#endif
1383
1384 return models;
1385}
1386
1399void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1400 const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1401{
1402 // Reinit klt
1403
1404 // delete the Klt Polygon features
1405 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1406 vpMbtDistanceKltPoints *kltpoly = *it;
1407 if (kltpoly != NULL) {
1408 delete kltpoly;
1409 }
1410 kltpoly = NULL;
1411 }
1412 kltPolygons.clear();
1413
1414 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1415 ++it) {
1416 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1417 if (kltPolyCylinder != NULL) {
1418 delete kltPolyCylinder;
1419 }
1420 kltPolyCylinder = NULL;
1421 }
1422 kltCylinders.clear();
1423
1424 // delete the structures used to display circles
1426 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1427 ci = *it;
1428 if (ci != NULL) {
1429 delete ci;
1430 }
1431 ci = NULL;
1432 }
1433
1434 circles_disp.clear();
1435
1436 firstInitialisation = true;
1437
1438 // Reinit edge
1441
1442 for (unsigned int i = 0; i < scales.size(); i += 1) {
1443 if (scales[i]) {
1444 for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
1445 l = *it;
1446 if (l != NULL)
1447 delete l;
1448 l = NULL;
1449 }
1450
1451 for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
1452 ++it) {
1453 cy = *it;
1454 if (cy != NULL)
1455 delete cy;
1456 cy = NULL;
1457 }
1458
1459 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
1460 ci = *it;
1461 if (ci != NULL)
1462 delete ci;
1463 ci = NULL;
1464 }
1465
1466 lines[i].clear();
1467 cylinders[i].clear();
1468 circles[i].clear();
1469 }
1470 }
1471
1472 // compute_interaction=1;
1473 nline = 0;
1474 ncylinder = 0;
1475 ncircle = 0;
1476 // lambda = 1;
1478
1479 // Reinit common parts
1480 faces.reset();
1481
1482 loadModel(cad_name, verbose, T);
1483
1484 m_cMo = cMo;
1485 init(I);
1486}
1487
1488#elif !defined(VISP_BUILD_SHARED_LIBS)
1489// Work around to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
1490// no symbols
1491void dummy_vpMbEdgeKltTracker() { };
1492#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: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)
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 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 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
Implementation of an homography and operations on homographies.
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
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:116
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 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
double m_thresholdKLT
The threshold used in the robust estimation of KLT.
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
virtual void setClipping(const unsigned int &flags)
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 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 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)
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
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="")
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.
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
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.
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="")
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
bool computeProjError
vpHomogeneousMatrix m_cMo
The current pose.
vpCameraParameters m_cam
The camera parameters.
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="")
bool displayFeatures
If true, the features are displayed.
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
bool applyLodSettingInConfig
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
bool useScanLine
Use Scanline for visibility tests.
double angleAppears
Angle used to detect a face appearance.
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.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
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.
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.
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)
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.
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:65
@ 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
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ 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
Definition of the vpSubMatrix vpSubMatrix class provides a mask on a vpMatrix all properties of vpMat...
Definition vpSubMatrix.h:58
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 vpERROR_TRACE
Definition vpDebug.h:388