Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMbKltTracker.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 * Model based tracker using only KLT
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpImageConvert.h>
37#include <visp3/core/vpTrackingException.h>
38#include <visp3/core/vpVelocityTwistMatrix.h>
39#include <visp3/mbt/vpMbKltTracker.h>
40#include <visp3/mbt/vpMbtXmlGenericParser.h>
41
42#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
43
44#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
45#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
46#endif
47
48namespace
49{
69vpMatrix homography2collineation(const vpMatrix &H, const vpCameraParameters &cam)
70{
71 vpMatrix G(3, 3);
72 double px = cam.get_px();
73 double py = cam.get_py();
74 double u0 = cam.get_u0();
75 double v0 = cam.get_v0();
76 double one_over_px = cam.get_px_inverse();
77 double one_over_py = cam.get_py_inverse();
78 double h00 = H[0][0], h01 = H[0][1], h02 = H[0][2];
79 double h10 = H[1][0], h11 = H[1][1], h12 = H[1][2];
80 double h20 = H[2][0], h21 = H[2][1], h22 = H[2][2];
81
82 double A = h00 * px + u0 * h20;
83 double B = h01 * px + u0 * h21;
84 double C = h02 * px + u0 * h22;
85 double D = h10 * py + v0 * h20;
86 double E = h11 * py + v0 * h21;
87 double F = h12 * py + v0 * h22;
88
89 G[0][0] = A * one_over_px;
90 G[1][0] = D * one_over_px;
91 G[2][0] = h20 * one_over_px;
92
93 G[0][1] = B * one_over_py;
94 G[1][1] = E * one_over_py;
95 G[2][1] = h21 * one_over_py;
96
97 double u0_one_over_px = u0 * one_over_px;
98 double v0_one_over_py = v0 * one_over_py;
99
100 G[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
101 G[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
102 G[2][2] = -h20 * u0_one_over_px - h21 * v0_one_over_py + h22;
103
104 return G;
105}
106} // namespace
107
109 :
110 cur(), c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
111 kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
112 m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
113{
116 tracker.setMaxFeatures(10000);
118 tracker.setQuality(0.01);
123
124#ifdef VISP_HAVE_OGRE
125 faces.getOgreContext()->setWindowName("MBT Klt");
126#endif
127
128 m_lambda = 0.8;
129 m_maxIter = 200;
130}
131
137{
138 // delete the Klt Polygon features
139 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
140 vpMbtDistanceKltPoints *kltpoly = *it;
141 if (kltpoly != NULL) {
142 delete kltpoly;
143 }
144 kltpoly = NULL;
145 }
146 kltPolygons.clear();
147
148 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
149 ++it) {
150 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
151 if (kltPolyCylinder != NULL) {
152 delete kltPolyCylinder;
153 }
154 kltPolyCylinder = NULL;
155 }
156 kltCylinders.clear();
157
158 // delete the structures used to display circles
159 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
160 vpMbtDistanceCircle *ci = *it;
161 if (ci != NULL) {
162 delete ci;
163 }
164 ci = NULL;
165 }
166
167 circles_disp.clear();
168}
169
171{
172 if (!modelInitialised) {
173 throw vpException(vpException::fatalError, "model not initialized");
174 }
175
176 bool reInitialisation = false;
177 if (!useOgre)
179 else {
180#ifdef VISP_HAVE_OGRE
181 if (!faces.isOgreInitialised()) {
185 // Turn off Ogre config dialog display for the next call to this
186 // function since settings are saved in the ogre.cfg file and used
187 // during the next call
188 ogreShowConfigDialog = false;
189 }
190
192
193#else
195#endif
196 }
197 reinit(I);
198}
199
201{
202 c0Mo = m_cMo;
203 ctTc0.eye();
204
206
208
209 if (useScanLine) {
212 }
213
214 // mask
215 cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
216
217 vpMbtDistanceKltPoints *kltpoly;
218 vpMbtDistanceKltCylinder *kltPolyCylinder;
219 if (useScanLine) {
221 } else {
222 unsigned char val = 255 /* - i*15*/;
223 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
224 kltpoly = *it;
225 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
226 // need to changeFrame when reinit() is called by postTracking
227 // other solution is
228 kltpoly->polygon->changeFrame(m_cMo);
229 kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
230 kltpoly->updateMask(mask, val, maskBorder);
231 }
232 }
233
234 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
235 ++it) {
236 kltPolyCylinder = *it;
237
238 if (kltPolyCylinder->isTracked()) {
239 for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
240 unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
241 if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
242 faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
243 }
244 }
245
246 kltPolyCylinder->updateMask(mask, val, maskBorder);
247 }
248 }
249 }
250
251 tracker.initTracking(cur, mask);
252 // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
253 // the points are valid for tracking and avoid too fast reinitialisations.
254 // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
255 // points" << std::endl;
256
257 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
258 kltpoly = *it;
259 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
260 kltpoly->init(tracker, m_mask);
261 }
262 }
263
264 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
265 ++it) {
266 kltPolyCylinder = *it;
267
268 if (kltPolyCylinder->isTracked())
269 kltPolyCylinder->init(tracker, m_cMo);
270 }
271}
272
278{
279 m_cMo.eye();
280
281 // delete the Klt Polygon features
282 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
283 vpMbtDistanceKltPoints *kltpoly = *it;
284 if (kltpoly != NULL) {
285 delete kltpoly;
286 }
287 kltpoly = NULL;
288 }
289 kltPolygons.clear();
290
291 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
292 ++it) {
293 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
294 if (kltPolyCylinder != NULL) {
295 delete kltPolyCylinder;
296 }
297 kltPolyCylinder = NULL;
298 }
299 kltCylinders.clear();
300
301 // delete the structures used to display circles
302 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
303 vpMbtDistanceCircle *ci = *it;
304 if (ci != NULL) {
305 delete ci;
306 }
307 ci = NULL;
308 }
309
310 circles_disp.clear();
311
313 firstInitialisation = true;
314 computeCovariance = false;
315
318
319 tracker.setMaxFeatures(10000);
321 tracker.setQuality(0.01);
326
329
331
332 maskBorder = 5;
333 threshold_outlier = 0.5;
334 percentGood = 0.6;
335
336 m_lambda = 0.8;
337 m_maxIter = 200;
338
339 faces.reset();
340
342
343 useScanLine = false;
344
345#ifdef VISP_HAVE_OGRE
346 useOgre = false;
347#endif
348}
349
358std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
359{
360 std::vector<vpImagePoint> kltPoints;
361 for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
362 long id;
363 float x_tmp, y_tmp;
364 tracker.getFeature((int)i, id, x_tmp, y_tmp);
365 kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
366 }
367
368 return kltPoints;
369}
370
379std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
380{
381 std::map<int, vpImagePoint> kltPoints;
382 for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
383 long id;
384 float x_tmp, y_tmp;
385 tracker.getFeature((int)i, id, x_tmp, y_tmp);
386#ifdef TARGET_OS_IPHONE
387 kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
388#else
389 kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
390#endif
391 }
392
393 return kltPoints;
394}
395
411
418{
419 // for (unsigned int i = 0; i < faces.size(); i += 1){
420 // faces[i]->setCameraParameters(camera);
421 // }
422
423 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
424 vpMbtDistanceKltPoints *kltpoly = *it;
425 kltpoly->setCameraParameters(cam);
426 }
427
428 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
429 ++it) {
430 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
431 kltPolyCylinder->setCameraParameters(cam);
432 }
433
434 m_cam = cam;
435}
436
437void vpMbKltTracker::setPose(const vpImage<unsigned char> *const I, const vpImage<vpRGBa> *const I_color,
438 const vpHomogeneousMatrix &cdMo)
439{
440 if (I_color) {
441 vpImageConvert::convert(*I_color, m_I);
442 }
443
444 if (!kltCylinders.empty()) {
445 std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
446 "This feature is not implemented yet."
447 << std::endl;
448 std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
449 m_cMo = cdMo;
450 if (I) {
451 init(*I);
452 } else {
453 init(m_I);
454 }
455 } else {
456 vpMbtDistanceKltPoints *kltpoly;
457
458 std::vector<cv::Point2f> init_pts;
459 std::vector<long> init_ids;
460 std::vector<cv::Point2f> guess_pts;
461
462 vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
463 vpHomogeneousMatrix cMcd = cdMc.inverse();
464
465 vpRotationMatrix cdRc;
467
468 cdMc.extract(cdRc);
469 cdMc.extract(cdtc);
470
471 // unsigned int nbCur = 0;
472
473 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
474 kltpoly = *it;
475
476 if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
477 kltpoly->polygon->changeFrame(cdMo);
478
479 // Get the normal to the face at the current state cMo
480 vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
481 plan.changeFrame(cMcd);
482
483 vpColVector Nc = plan.getNormal();
484 Nc.normalize();
485
486 double invDc = 1.0 / plan.getD();
487
488 // Create the homography
489 vpMatrix cdHc;
490 vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
491 cdHc /= cdHc[2][2];
492
493 // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
494 vpMatrix cdGc = homography2collineation(cdHc, m_cam);
495
496 // Points displacement
497 std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
498 // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
499 for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
500#ifdef TARGET_OS_IPHONE
501 if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
502 init_ids.end())
503#else
504 if (std::find(init_ids.begin(), init_ids.end(),
505 (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
506#endif
507 {
508 // KLT point already processed (a KLT point can exist in another
509 // vpMbtDistanceKltPoints due to possible overlapping faces)
510 continue;
511 }
512
513 vpColVector cdp(3);
514 cdp[0] = iter->second.get_j();
515 cdp[1] = iter->second.get_i();
516 cdp[2] = 1.0;
517
518 cv::Point2f p((float)cdp[0], (float)cdp[1]);
519 init_pts.push_back(p);
520#ifdef TARGET_OS_IPHONE
521 init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
522#else
523 init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
524#endif
525
526 double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
527
528 if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
529 cdp[0] = 0.0;
530 cdp[1] = 0.0;
531 throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
532 }
533
534 cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
535 cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
536
537 // Set value to the KLT tracker
538 cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
539 guess_pts.push_back(p_guess);
540 }
541 }
542 }
543
544 if (I) {
546 } else {
548 }
549
550 tracker.setInitialGuess(init_pts, guess_pts, init_ids);
551
552 bool reInitialisation = false;
553 if (!useOgre) {
554 if (I) {
555 faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
556 } else {
557 faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
558 }
559 } else {
560#ifdef VISP_HAVE_OGRE
561 if (I) {
563 reInitialisation);
564 } else {
566 reInitialisation);
567 }
568#else
569 if (I) {
570 faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
571 } else {
572 faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
573 }
574#endif
575 }
576
577 m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
578
579 if (useScanLine) {
582 }
583
584 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
585 kltpoly = *it;
586 if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
588 kltpoly->init(tracker, m_mask);
589 }
590 }
591
592 m_cMo = cdMo;
593 c0Mo = m_cMo;
594 ctTc0.eye();
595 }
596}
597
608{
609 vpMbKltTracker::setPose(&I, NULL, cdMo);
610}
611
622{
623 vpMbKltTracker::setPose(NULL, &I_color, cdMo);
624}
625
633{
635 kltPoly->setCameraParameters(m_cam);
636 kltPoly->polygon = &polygon;
637 kltPoly->hiddenface = &faces;
638 kltPoly->useScanLine = useScanLine;
639 kltPolygons.push_back(kltPoly);
640}
648{
650 kltPoly->setCameraParameters(m_cam);
651 kltPoly->polygon = &polygon;
652 kltPoly->hiddenface = &faces;
653 kltPoly->useScanLine = useScanLine;
654 kltPolygons.push_back(kltPoly);
655}
656
664{
667
668 m_nbInfos = 0;
669 m_nbFaceUsed = 0;
670 // for (unsigned int i = 0; i < faces.size(); i += 1){
671 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
672 vpMbtDistanceKltPoints *kltpoly = *it;
673 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
675 // faces[i]->ransac();
676 if (kltpoly->hasEnoughPoints()) {
677 m_nbInfos += kltpoly->getCurrentNumberPoints();
678 m_nbFaceUsed++;
679 }
680 }
681 }
682
683 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
684 ++it) {
685 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
686
687 if (kltPolyCylinder->isTracked()) {
688 kltPolyCylinder->computeNbDetectedCurrent(tracker);
689 if (kltPolyCylinder->hasEnoughPoints()) {
690 m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
691 m_nbFaceUsed++;
692 }
693 }
694 }
695}
696
701{
702 // # For a better Post Tracking, tracker should reinitialize if so faces
703 // don't have enough points but are visible. # Here we are not doing it for
704 // more speed performance.
705 bool reInitialisation = false;
706
707 unsigned int initialNumber = 0;
708 unsigned int currentNumber = 0;
709 unsigned int shift = 0;
710 // for (unsigned int i = 0; i < faces.size(); i += 1){
711 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
712 vpMbtDistanceKltPoints *kltpoly = *it;
713 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
714 initialNumber += kltpoly->getInitialNumberPoint();
715 if (kltpoly->hasEnoughPoints()) {
716 vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
717 shift += 2 * kltpoly->getCurrentNumberPoints();
718 kltpoly->removeOutliers(sub_w, threshold_outlier);
719
720 currentNumber += kltpoly->getCurrentNumberPoints();
721 }
722 // else{
723 // reInitialisation = true;
724 // break;
725 // }
726 }
727 }
728
729 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
730 ++it) {
731 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
732
733 if (kltPolyCylinder->isTracked()) {
734 initialNumber += kltPolyCylinder->getInitialNumberPoint();
735 if (kltPolyCylinder->hasEnoughPoints()) {
736 vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
737 shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
738 kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
739
740 currentNumber += kltPolyCylinder->getCurrentNumberPoints();
741 }
742 }
743 }
744
745 // if(!reInitialisation){
746 double value = percentGood * (double)initialNumber;
747 if ((double)currentNumber < value) {
748 // std::cout << "Too many point disappear : " << initialNumber << "/"
749 // << currentNumber << std::endl;
750 reInitialisation = true;
751 } else {
752 if (!useOgre)
754 else {
755#ifdef VISP_HAVE_OGRE
757#else
759#endif
760 }
761 }
762 // }
763
764 if (reInitialisation)
765 return true;
766
767 return false;
768}
769
774{
775 vpMatrix L_true; // interaction matrix without weighting
776 vpMatrix LVJ_true;
777 vpColVector v; // "speed" for VVS
778
779 vpMatrix LTL;
780 vpColVector LTR;
781 vpHomogeneousMatrix cMoPrev;
782 vpHomogeneousMatrix ctTc0_Prev;
783 vpColVector error_prev;
784 double mu = m_initialMu;
785
786 double normRes = 0;
787 double normRes_1 = -1;
788 unsigned int iter = 0;
789
790 bool isoJoIdentity = m_isoJoIdentity; // Backup since it can be modified if L is not full rank
791 if (isoJoIdentity)
792 oJo.eye();
793
795
796 while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
798
799 bool reStartFromLastIncrement = false;
800 computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
801 if (reStartFromLastIncrement) {
802 ctTc0 = ctTc0_Prev;
803 }
804
805 if (!reStartFromLastIncrement) {
807
808 if (computeCovariance) {
809 L_true = m_L_klt;
810 if (!isoJoIdentity) {
812 cVo.buildFrom(m_cMo);
813 LVJ_true = (m_L_klt * cVo * oJo);
814 }
815 }
816
817 normRes_1 = normRes;
818 normRes = 0.0;
819
820 for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
822 normRes += m_weightedError_klt[i];
823 }
824
825 if ((iter == 0) || m_computeInteraction) {
826 for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
827 for (unsigned int j = 0; j < 6; j++) {
828 m_L_klt[i][j] *= m_w_klt[i];
829 }
830 }
831 }
832
834
835 cMoPrev = m_cMo;
836 ctTc0_Prev = ctTc0;
838 m_cMo = ctTc0 * c0Mo;
839 } // endif(!reStartFromLastIncrement)
840
841 iter++;
842 }
843
845}
846
848{
849 unsigned int nbFeatures = 2 * m_nbInfos;
850
851 m_L_klt.resize(nbFeatures, 6, false, false);
852 m_error_klt.resize(nbFeatures, false);
853
854 m_weightedError_klt.resize(nbFeatures, false);
855 m_w_klt.resize(nbFeatures, false);
856 m_w_klt = 1;
857
859}
860
862{
863 unsigned int shift = 0;
864 vpHomography H;
865
866 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
867 vpMbtDistanceKltPoints *kltpoly = *it;
868 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
869 kltpoly->hasEnoughPoints()) {
870 vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
871 vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
872
873 try {
874 kltpoly->computeHomography(ctTc0, H);
875 kltpoly->computeInteractionMatrixAndResidu(subR, subL);
876 } catch (...) {
877 throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
878 }
879
880 shift += 2 * kltpoly->getCurrentNumberPoints();
881 }
882 }
883
884 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
885 ++it) {
886 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
887
888 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
889 vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
890 vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
891
892 try {
893 kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
894 } catch (...) {
895 throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
896 }
897
898 shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
899 }
900 }
901}
902
911{
912 preTracking(I);
913
914 if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
915 throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
916 }
917
918 computeVVS();
919
920 if (postTracking(I, m_w_klt))
921 reinit(I);
922
923 if (displayFeatures) {
925 }
926}
927
936{
939
940 if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
941 throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
942 }
943
944 computeVVS();
945
947 reinit(m_I);
948}
949
993void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
994{
995 // Load projection error config
996 vpMbTracker::loadConfigFile(configFile, verbose);
997
999 xmlp.setVerbose(verbose);
1000 xmlp.setKltMaxFeatures(10000);
1001 xmlp.setKltWindowSize(5);
1002 xmlp.setKltQuality(0.01);
1003 xmlp.setKltMinDistance(5);
1004 xmlp.setKltHarrisParam(0.01);
1005 xmlp.setKltBlockSize(3);
1006 xmlp.setKltPyramidLevels(3);
1010
1011 try {
1012 if (verbose) {
1013 std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1014 }
1015 xmlp.parse(configFile.c_str());
1016 } catch (...) {
1017 vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1018 throw vpException(vpException::ioError, "problem to parse configuration file.");
1019 }
1020
1021 vpCameraParameters camera;
1022 xmlp.getCameraParameters(camera);
1023 setCameraParameters(camera);
1024
1035
1036 // if(useScanLine)
1037 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1038
1039 if (xmlp.hasNearClippingDistance())
1041
1042 if (xmlp.hasFarClippingDistance())
1044
1045 if (xmlp.getFovClipping())
1047
1048 useLodGeneral = xmlp.getLodState();
1051
1053 if (this->getNbPolygon() > 0) {
1058 }
1059}
1060
1073 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1074 bool displayFullModel)
1075{
1076 std::vector<std::vector<double> > models =
1077 vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1078
1079 for (size_t i = 0; i < models.size(); i++) {
1080 if (vpMath::equal(models[i][0], 0)) {
1081 vpImagePoint ip1(models[i][1], models[i][2]);
1082 vpImagePoint ip2(models[i][3], models[i][4]);
1083 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1084 } else if (vpMath::equal(models[i][0], 1)) {
1085 vpImagePoint center(models[i][1], models[i][2]);
1086 double n20 = models[i][3];
1087 double n11 = models[i][4];
1088 double n02 = models[i][5];
1089 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1090 }
1091 }
1092
1093 if (displayFeatures) {
1094 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1098
1100 double id = m_featuresToBeDisplayedKlt[i][5];
1101 std::stringstream ss;
1102 ss << id;
1103 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1104 }
1105 }
1106 }
1107
1108#ifdef VISP_HAVE_OGRE
1109 if (useOgre)
1110 faces.displayOgre(cMo);
1111#endif
1112}
1113
1126 const vpColor &col, unsigned int thickness, bool displayFullModel)
1127{
1128 std::vector<std::vector<double> > models =
1129 vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1130
1131 for (size_t i = 0; i < models.size(); i++) {
1132 if (vpMath::equal(models[i][0], 0)) {
1133 vpImagePoint ip1(models[i][1], models[i][2]);
1134 vpImagePoint ip2(models[i][3], models[i][4]);
1135 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1136 } else if (vpMath::equal(models[i][0], 1)) {
1137 vpImagePoint center(models[i][1], models[i][2]);
1138 double n20 = models[i][3];
1139 double n11 = models[i][4];
1140 double n02 = models[i][5];
1141 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1142 }
1143 }
1144
1145 if (displayFeatures) {
1146 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1150
1152 double id = m_featuresToBeDisplayedKlt[i][5];
1153 std::stringstream ss;
1154 ss << id;
1155 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1156 }
1157 }
1158 }
1159
1160#ifdef VISP_HAVE_OGRE
1161 if (useOgre)
1162 faces.displayOgre(cMo);
1163#endif
1164}
1165
1166std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1167{
1168 std::vector<std::vector<double> > features;
1169
1170 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1171 vpMbtDistanceKltPoints *kltpoly = *it;
1172
1173 if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1174 std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1175 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1176 }
1177 }
1178
1179 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1180 ++it) {
1181 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1182
1183 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1184 std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1185 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1186 }
1187 }
1188
1189 return features;
1190}
1191
1207std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1208 const vpHomogeneousMatrix &cMo,
1209 const vpCameraParameters &cam,
1210 bool displayFullModel)
1211{
1212 std::vector<std::vector<double> > models;
1213
1214 vpCameraParameters c = cam;
1215
1216 if (clippingFlag > 3) // Contains at least one FOV constraint
1217 c.computeFov(width, height);
1218
1219 // vpMbtDistanceKltPoints *kltpoly;
1220 // vpMbtDistanceKltCylinder *kltPolyCylinder;
1221
1222 // Previous version 12/08/2015
1223 // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1224 // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1225 // kltpoly = *it;
1226 // kltpoly->polygon->changeFrame(cMo_);
1227 // kltpoly->polygon->computePolygonClipped(c);
1228 // }
1230
1231 if (useScanLine && !displayFullModel)
1232 faces.computeScanLineRender(m_cam, width, height);
1233
1234 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1235 vpMbtDistanceKltPoints *kltpoly = *it;
1236 std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1237 models.insert(models.end(), modelLines.begin(), modelLines.end());
1238 }
1239
1240 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1241 ++it) {
1242 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1243 std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1244 models.insert(models.end(), modelLines.begin(), modelLines.end());
1245 }
1246
1247 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1248 vpMbtDistanceCircle *displayCircle = *it;
1249 std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1250 if (!paramsCircle.empty()) {
1251 models.push_back(paramsCircle);
1252 }
1253 }
1254
1255 return models;
1256}
1257
1265{
1266 unsigned int nbTotalPoints = 0;
1267 // for (unsigned int i = 0; i < faces.size(); i += 1){
1268 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1269 vpMbtDistanceKltPoints *kltpoly = *it;
1270 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1271 kltpoly->hasEnoughPoints()) {
1272 nbTotalPoints += kltpoly->getCurrentNumberPoints();
1273 }
1274 }
1275
1276 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1277 ++it) {
1278 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1279 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1280 nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1281 }
1282
1283 if (nbTotalPoints < 10) {
1284 std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1286 "test tracking failed (too few points to realize a good tracking).");
1287 }
1288}
1289
1300void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1301 const std::string & /*name*/)
1302{
1304 kltPoly->setCameraParameters(m_cam);
1305
1306 kltPoly->buildFrom(p1, p2, radius);
1307
1308 // Add the Cylinder BBox to the list of polygons
1309 kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1310 kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1311 kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1312 kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1313
1314 kltPoly->hiddenface = &faces;
1315 kltPoly->useScanLine = useScanLine;
1316 kltCylinders.push_back(kltPoly);
1317}
1318
1330void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius, int /*idFace*/,
1331 const std::string &name)
1332{
1333 addCircle(p1, p2, p3, radius, name);
1334}
1335
1344void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1345 const std::string &name)
1346{
1347 bool already_here = false;
1348
1349 // for(std::list<vpMbtDistanceCircle*>::const_iterator
1350 // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1351 // ci = *it;
1352 // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1353 // samePoint(*(ci->p3),P3)) ||
1354 // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1355 // samePoint(*(ci->p3),P2)) ){
1356 // already_here = (std::fabs(ci->radius - r) <
1357 // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1358 // r));
1359 // }
1360 // }
1361
1362 if (!already_here) {
1364
1366 ci->setName(name);
1367 ci->buildFrom(P1, P2, P3, r);
1368 circles_disp.push_back(ci);
1369 }
1370}
1371
1384void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1385 const vpHomogeneousMatrix &cMo, bool verbose, const vpHomogeneousMatrix &T)
1386{
1387 m_cMo.eye();
1388
1389 firstInitialisation = true;
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
1412 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1413 vpMbtDistanceCircle *ci = *it;
1414 if (ci != NULL) {
1415 delete ci;
1416 }
1417 ci = NULL;
1418 }
1419
1420 faces.reset();
1421
1422 loadModel(cad_name, verbose, T);
1423 initFromPose(I, cMo);
1424}
1425
1433void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1434{
1435 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1436 vpMbtDistanceKltPoints *kltpoly = *it;
1437 if (kltpoly->polygon->getName() == name) {
1438 kltpoly->setTracked(useKltTracking);
1439 }
1440 }
1441}
1442
1443#elif !defined(VISP_BUILD_SHARED_LIBS)
1444// Work around to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1445// symbols
1446void dummy_vpMbKltTracker(){};
1447#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)
double get_px_inverse() const
double get_py_inverse() const
Implementation of column vector and the associated operations.
vpColVector & normalize()
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
@ fatalError
Fatal error.
Definition vpException.h:84
@ divideByZeroError
Division by zero.
Definition vpException.h:82
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) 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 getCols() const
Definition vpImage.h:175
unsigned int getHeight() const
Definition vpImage.h:184
unsigned int getRows() const
Definition vpImage.h:214
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition vpKltOpencv.h:73
double getQuality() const
int getMaxFeatures() const
Get the list of lost feature.
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
void setTrackerId(int tid)
int getWindowSize() const
Get the window size used to refine the corner locations.
int getNbFeatures() const
Get the number of current features.
void setHarrisFreeParameter(double harris_k)
void getFeature(const int &index, long &id, float &x, float &y) const
double getHarrisFreeParameter() const
Get the free parameter of the Harris detector.
void setMaxFeatures(int maxCount)
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
double getMinDistance() const
void setMinDistance(double minDistance)
int getBlockSize() const
Get the size of the averaging block used to track the features.
int getPyramidLevels() const
Get the list of features id.
void setUseHarris(int useHarrisDetector)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition vpMath.h:116
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
void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int setVisibleOgre(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
void initOgre(const vpCameraParameters &cam=vpCameraParameters())
unsigned int setVisible(unsigned int width, unsigned int height, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h)
vpAROgre * getOgreContext()
void displayOgre(const vpHomogeneousMatrix &cMo)
vpMbScanLine & getMbScanLineRenderer()
void setOgreShowConfigDialog(bool showConfigDialog)
std::list< vpMbtDistanceKltCylinder * > kltCylinders
virtual 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_klt
(s - s*)
vpHomogeneousMatrix c0Mo
Initial pose.
virtual void track(const vpImage< unsigned char > &I)
vpHomogeneousMatrix ctTc0
std::list< vpMbtDistanceKltPoints * > kltPolygons
virtual std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void initCylinder(const vpPoint &, const vpPoint &, double, int, const std::string &name="")
cv::Mat cur
Temporary OpenCV image for fast conversion.
std::map< int, vpImagePoint > getKltImagePointsWithId() const
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
std::list< vpMbtDistanceCircle * > circles_disp
Vector of the circles used here only to display the full model.
virtual void testTracking()
vpColVector m_weightedError_klt
Weighted error.
virtual ~vpMbKltTracker()
vpKltOpencv tracker
Points tracker.
virtual std::vector< std::vector< double > > getFeaturesForDisplayKlt()
void preTracking(const vpImage< unsigned char > &I)
void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r, const std::string &name="")
virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, double, int, const std::string &name="")
unsigned int m_nbInfos
bool postTracking(const vpImage< unsigned char > &I, vpColVector &w)
vpRobust m_robust_klt
Robust.
virtual void computeVVSInteractionMatrixAndResidu()
virtual void reinit(const vpImage< unsigned char > &I)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
std::vector< vpImagePoint > getKltImagePoints() const
unsigned int maskBorder
Erosion of the mask.
unsigned int m_nbFaceUsed
virtual void initFaceFromCorners(vpMbtPolygon &polygon)
vpColVector m_w_klt
Robust weights.
void setCameraParameters(const vpCameraParameters &cam)
std::vector< std::vector< double > > m_featuresToBeDisplayedKlt
Display features.
vpMatrix m_L_klt
Interaction matrix.
void setUseKltTracking(const std::string &name, const bool &useKltTracking)
virtual void computeVVSInit()
virtual void initFaceFromLines(vpMbtPolygon &polygon)
virtual void init(const vpImage< unsigned char > &I)
double m_lambda
Gain of the virtual visual servoing stage.
bool modelInitialised
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
virtual void computeVVSPoseEstimation(const bool isoJoIdentity, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
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.
vpHomogeneousMatrix m_cMo
The current pose.
vpCameraParameters m_cam
The camera parameters.
bool useOgre
Use Ogre3d for visibility tests.
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
virtual void setLod(bool useLod, const std::string &name="")
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
bool displayFeatures
If true, the features are displayed.
double angleDisappears
Angle used to detect a face disappearance.
virtual unsigned int getNbPolygon() const
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
virtual void setFarClippingDistance(const double &dist)
bool m_isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
bool useScanLine
Use Scanline for visibility tests.
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
bool ogreShowConfigDialog
unsigned int clippingFlag
Flags specifying which clipping to used.
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void setCameraParameters(const vpCameraParameters &camera)
void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, double r)
std::vector< double > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
void setName(const std::string &circle_name)
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
virtual void setCameraParameters(const vpCameraParameters &_cam)
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
std::vector< std::vector< double > > getFeaturesForDisplay()
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
bool useScanLine
Use scanline rendering.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
unsigned int getCurrentNumberPoints() const
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
unsigned int getInitialNumberPoint() const
Implementation of a polygon of the model containing points of interest. It is used by the model-based...
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
bool useScanLine
Use scanline rendering.
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
std::vector< std::vector< double > > getFeaturesForDisplay()
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
virtual void setCameraParameters(const vpCameraParameters &_cam)
unsigned int getCurrentNumberPoints() const
std::vector< std::vector< double > > getModelForDisplay(const vpCameraParameters &cam, bool displayFullModel=false)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
std::map< int, int > & getCurrentPointsInd()
unsigned int getInitialNumberPoint() const
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
std::map< int, vpImagePoint > & getCurrentPoints()
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
void setTracked(const bool &track)
Implementation of a polygon of the model used by the model-based tracker.
std::string getName() const
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 setKltMaskBorder(const unsigned int &mb)
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
double getLodMinPolygonAreaThreshold() const
This class defines the container for a plane geometrical structure.
Definition vpPlane.h:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition vpPlane.cpp:361
double getD() const
Definition vpPlane.h:106
vpColVector getNormal() const
Definition vpPlane.cpp:245
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getNbPoint() const
vpPoint * p
corners in the object frame
Definition vpPolygon3D.h:76
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
void setMinMedianAbsoluteDeviation(double mad_min)
Definition vpRobust.h:155
Implementation of a rotation matrix and operations on such kind of matrices.
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.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition vpDebug.h:388