Visual Servoing Platform version 3.5.0
vpMbKltTracker.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Model based tracker using only KLT
33 *
34 * Authors:
35 * Romain Tallonneau
36 * Aurelien Yol
37 *
38 *****************************************************************************/
39
40#include <visp3/core/vpImageConvert.h>
41#include <visp3/core/vpTrackingException.h>
42#include <visp3/core/vpVelocityTwistMatrix.h>
43#include <visp3/mbt/vpMbKltTracker.h>
44#include <visp3/mbt/vpMbtXmlGenericParser.h>
45
46#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
47
48#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
49#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
50#endif
51
52namespace {
72vpMatrix homography2collineation(const vpMatrix &H, const vpCameraParameters &cam)
73{
74 vpMatrix G(3, 3);
75 double px = cam.get_px();
76 double py = cam.get_py();
77 double u0 = cam.get_u0();
78 double v0 = cam.get_v0();
79 double one_over_px = cam.get_px_inverse();
80 double one_over_py = cam.get_py_inverse();
81 double h00 = H[0][0], h01 = H[0][1], h02 = H[0][2];
82 double h10 = H[1][0], h11 = H[1][1], h12 = H[1][2];
83 double h20 = H[2][0], h21 = H[2][1], h22 = H[2][2];
84
85 double A = h00 * px + u0 * h20;
86 double B = h01 * px + u0 * h21;
87 double C = h02 * px + u0 * h22;
88 double D = h10 * py + v0 * h20;
89 double E = h11 * py + v0 * h21;
90 double F = h12 * py + v0 * h22;
91
92 G[0][0] = A * one_over_px;
93 G[1][0] = D * one_over_px;
94 G[2][0] = h20 * one_over_px;
95
96 G[0][1] = B * one_over_py;
97 G[1][1] = E * one_over_py;
98 G[2][1] = h21 * one_over_py;
99
100 double u0_one_over_px = u0 * one_over_px;
101 double v0_one_over_py = v0 * one_over_py;
102
103 G[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
104 G[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
105 G[2][2] = - h20 * u0_one_over_px - h21 * v0_one_over_py + h22;
106
107 return G;
108}
109}
110
112 :
113#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
114 cur(),
115#else
116 cur(NULL),
117#endif
118 c0Mo(), firstInitialisation(true), maskBorder(5), threshold_outlier(0.5), percentGood(0.6), ctTc0(), tracker(),
119 kltPolygons(), kltCylinders(), circles_disp(), m_nbInfos(0), m_nbFaceUsed(0), m_L_klt(), m_error_klt(), m_w_klt(),
120 m_weightedError_klt(), m_robust_klt(), m_featuresToBeDisplayedKlt()
121{
124 tracker.setMaxFeatures(10000);
126 tracker.setQuality(0.01);
131
132#ifdef VISP_HAVE_OGRE
133 faces.getOgreContext()->setWindowName("MBT Klt");
134#endif
135
136 m_lambda = 0.8;
137 m_maxIter = 200;
138}
139
145{
146#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
147 if (cur != NULL) {
148 cvReleaseImage(&cur);
149 cur = NULL;
150 }
151#endif
152
153 // delete the Klt Polygon features
154 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
155 vpMbtDistanceKltPoints *kltpoly = *it;
156 if (kltpoly != NULL) {
157 delete kltpoly;
158 }
159 kltpoly = NULL;
160 }
161 kltPolygons.clear();
162
163 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
164 ++it) {
165 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
166 if (kltPolyCylinder != NULL) {
167 delete kltPolyCylinder;
168 }
169 kltPolyCylinder = NULL;
170 }
171 kltCylinders.clear();
172
173 // delete the structures used to display circles
174 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
175 vpMbtDistanceCircle *ci = *it;
176 if (ci != NULL) {
177 delete ci;
178 }
179 ci = NULL;
180 }
181
182 circles_disp.clear();
183}
184
186{
187 if (!modelInitialised) {
188 throw vpException(vpException::fatalError, "model not initialized");
189 }
190
191 bool reInitialisation = false;
192 if (!useOgre)
194 else {
195#ifdef VISP_HAVE_OGRE
196 if (!faces.isOgreInitialised()) {
200 // Turn off Ogre config dialog display for the next call to this
201 // function since settings are saved in the ogre.cfg file and used
202 // during the next call
203 ogreShowConfigDialog = false;
204 }
205
207
208#else
210#endif
211 }
212 reinit(I);
213}
214
216{
217 c0Mo = m_cMo;
218 ctTc0.eye();
219
221
223
224 if (useScanLine) {
227 }
228
229// mask
230#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
231 cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
232#else
233 IplImage *mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
234 cvZero(mask);
235#endif
236
237 vpMbtDistanceKltPoints *kltpoly;
238 vpMbtDistanceKltCylinder *kltPolyCylinder;
239 if (useScanLine) {
241 } else {
242 unsigned char val = 255 /* - i*15*/;
243 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
244 kltpoly = *it;
245 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
246 // need to changeFrame when reinit() is called by postTracking
247 // other solution is
248 kltpoly->polygon->changeFrame(m_cMo);
249 kltpoly->polygon->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
250 kltpoly->updateMask(mask, val, maskBorder);
251 }
252 }
253
254 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
255 ++it) {
256 kltPolyCylinder = *it;
257
258 if (kltPolyCylinder->isTracked()) {
259 for (unsigned int k = 0; k < kltPolyCylinder->listIndicesCylinderBBox.size(); k++) {
260 unsigned int indCylBBox = (unsigned int)kltPolyCylinder->listIndicesCylinderBBox[k];
261 if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
262 faces[indCylBBox]->computePolygonClipped(m_cam); // Might not be necessary when scanline is activated
263 }
264 }
265
266 kltPolyCylinder->updateMask(mask, val, maskBorder);
267 }
268 }
269 }
270
271 tracker.initTracking(cur, mask);
272 // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
273 // the points are valid for tracking and avoid too fast reinitialisations.
274 // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
275 // points" << std::endl;
276
277 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
278 kltpoly = *it;
279 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
280 kltpoly->init(tracker, m_mask);
281 }
282 }
283
284 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
285 ++it) {
286 kltPolyCylinder = *it;
287
288 if (kltPolyCylinder->isTracked())
289 kltPolyCylinder->init(tracker, m_cMo);
290 }
291
292#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
293 cvReleaseImage(&mask);
294#endif
295}
296
302{
303 m_cMo.eye();
304
305#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
306 if (cur != NULL) {
307 cvReleaseImage(&cur);
308 cur = NULL;
309 }
310#endif
311
312 // delete the Klt Polygon features
313 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
314 vpMbtDistanceKltPoints *kltpoly = *it;
315 if (kltpoly != NULL) {
316 delete kltpoly;
317 }
318 kltpoly = NULL;
319 }
320 kltPolygons.clear();
321
322 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
323 ++it) {
324 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
325 if (kltPolyCylinder != NULL) {
326 delete kltPolyCylinder;
327 }
328 kltPolyCylinder = NULL;
329 }
330 kltCylinders.clear();
331
332 // delete the structures used to display circles
333 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
334 vpMbtDistanceCircle *ci = *it;
335 if (ci != NULL) {
336 delete ci;
337 }
338 ci = NULL;
339 }
340
341 circles_disp.clear();
342
344 firstInitialisation = true;
345 computeCovariance = false;
346
349
350 tracker.setMaxFeatures(10000);
352 tracker.setQuality(0.01);
357
360
362
363 maskBorder = 5;
364 threshold_outlier = 0.5;
365 percentGood = 0.6;
366
367 m_lambda = 0.8;
368 m_maxIter = 200;
369
370 faces.reset();
371
373
374 useScanLine = false;
375
376#ifdef VISP_HAVE_OGRE
377 useOgre = false;
378#endif
379}
380
389std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
390{
391 std::vector<vpImagePoint> kltPoints;
392 for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
393 long id;
394 float x_tmp, y_tmp;
395 tracker.getFeature((int)i, id, x_tmp, y_tmp);
396 kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
397 }
398
399 return kltPoints;
400}
401
410std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
411{
412 std::map<int, vpImagePoint> kltPoints;
413 for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
414 long id;
415 float x_tmp, y_tmp;
416 tracker.getFeature((int)i, id, x_tmp, y_tmp);
417#if TARGET_OS_IPHONE
418 kltPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
419#else
420 kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
421#endif
422 }
423
424 return kltPoints;
425}
426
433{
441}
442
449{
450 // for (unsigned int i = 0; i < faces.size(); i += 1){
451 // faces[i]->setCameraParameters(camera);
452 // }
453
454 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
455 vpMbtDistanceKltPoints *kltpoly = *it;
456 kltpoly->setCameraParameters(cam);
457 }
458
459 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
460 ++it) {
461 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
462 kltPolyCylinder->setCameraParameters(cam);
463 }
464
465 m_cam = cam;
466}
467
468void vpMbKltTracker::setPose(const vpImage<unsigned char> * const I, const vpImage<vpRGBa> * const I_color,
469 const vpHomogeneousMatrix &cdMo)
470{
471 if (I_color) {
472 vpImageConvert::convert(*I_color, m_I);
473 }
474
475 if (!kltCylinders.empty()) {
476 std::cout << "WARNING: Cannot set pose when model contains cylinder(s). "
477 "This feature is not implemented yet."
478 << std::endl;
479 std::cout << "Tracker will be reinitialized with the given pose." << std::endl;
480 m_cMo = cdMo;
481 if (I) {
482 init(*I);
483 } else {
484 init(m_I);
485 }
486 } else {
487 vpMbtDistanceKltPoints *kltpoly;
488
489#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
490 std::vector<cv::Point2f> init_pts;
491 std::vector<long> init_ids;
492 std::vector<cv::Point2f> guess_pts;
493#else
494 unsigned int nbp = 0;
495 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
496 kltpoly = *it;
497 if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2)
498 nbp += (*it)->getCurrentNumberPoints();
499 }
500
501 CvPoint2D32f *init_pts = NULL;
502 init_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(init_pts[0]));
503 long *init_ids = (long *)cvAlloc((unsigned int)tracker.getMaxFeatures() * sizeof(long));
504 unsigned int iter_pts = 0;
505
506 CvPoint2D32f *guess_pts = NULL;
507 guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0]));
508#endif
509
510 vpHomogeneousMatrix cdMc = cdMo * m_cMo.inverse();
511 vpHomogeneousMatrix cMcd = cdMc.inverse();
512
513 vpRotationMatrix cdRc;
515
516 cdMc.extract(cdRc);
517 cdMc.extract(cdtc);
518
519 // unsigned int nbCur = 0;
520
521 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
522 kltpoly = *it;
523
524 if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
525 kltpoly->polygon->changeFrame(cdMo);
526
527 // Get the normal to the face at the current state cMo
528 vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
529 plan.changeFrame(cMcd);
530
531 vpColVector Nc = plan.getNormal();
532 Nc.normalize();
533
534 double invDc = 1.0 / plan.getD();
535
536 // Create the homography
537 vpMatrix cdHc;
538 vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
539 cdHc /= cdHc[2][2];
540
541 // Compute homography in the pixel space cdGc = K * cdHc * K^{-1}
542 vpMatrix cdGc = homography2collineation(cdHc, m_cam);
543
544 // Points displacement
545 std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
546 // nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
547 for (; iter != kltpoly->getCurrentPoints().end(); ++iter) {
548#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
549#if TARGET_OS_IPHONE
550 if (std::find(init_ids.begin(), init_ids.end(), (long)(kltpoly->getCurrentPointsInd())[(int)iter->first]) !=
551 init_ids.end())
552#else
553 if (std::find(init_ids.begin(), init_ids.end(),
554 (long)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]) != init_ids.end())
555#endif
556 {
557 // KLT point already processed (a KLT point can exist in another
558 // vpMbtDistanceKltPoints due to possible overlapping faces)
559 continue;
560 }
561#endif
562
563 vpColVector cdp(3);
564 cdp[0] = iter->second.get_j();
565 cdp[1] = iter->second.get_i();
566 cdp[2] = 1.0;
567
568#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
569 cv::Point2f p((float)cdp[0], (float)cdp[1]);
570 init_pts.push_back(p);
571#if TARGET_OS_IPHONE
572 init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
573#else
574 init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
575#endif
576#else
577 init_pts[iter_pts].x = (float)cdp[0];
578 init_pts[iter_pts].y = (float)cdp[1];
579 init_ids[iter_pts] = (kltpoly->getCurrentPointsInd())[(size_t)iter->first];
580#endif
581
582 double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
583
584 if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
585 cdp[0] = 0.0;
586 cdp[1] = 0.0;
587 throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
588 }
589
590 cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
591 cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
592
593// Set value to the KLT tracker
594#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
595 cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
596 guess_pts.push_back(p_guess);
597#else
598 guess_pts[iter_pts].x = (float)cdp[0];
599 guess_pts[iter_pts++].y = (float)cdp[1];
600#endif
601 }
602 }
603 }
604
605 if (I) {
607 } else {
609 }
610
611#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
612 tracker.setInitialGuess(init_pts, guess_pts, init_ids);
613#else
614 tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
615
616 if (init_pts)
617 cvFree(&init_pts);
618 init_pts = NULL;
619
620 if (guess_pts)
621 cvFree(&guess_pts);
622 guess_pts = NULL;
623
624 if (init_ids)
625 cvFree(&init_ids);
626 init_ids = NULL;
627#endif
628
629 bool reInitialisation = false;
630 if (!useOgre) {
631 if (I) {
632 faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
633 } else {
634 faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
635 }
636 } else {
637#ifdef VISP_HAVE_OGRE
638 if (I) {
639 faces.setVisibleOgre(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
640 } else {
642 }
643#else
644 if (I) {
645 faces.setVisible(I->getWidth(), I->getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
646 } else {
647 faces.setVisible(m_I.getWidth(), m_I.getHeight(), m_cam, cdMo, angleAppears, angleDisappears, reInitialisation);
648 }
649#endif
650 }
651
652 m_cam.computeFov(I ? I->getWidth() : m_I.getWidth(), I ? I->getHeight() : m_I.getHeight());
653
654 if (useScanLine) {
657 }
658
659 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
660 kltpoly = *it;
661 if (kltpoly->polygon->isVisible() && kltpoly->polygon->getNbPoint() > 2) {
663 kltpoly->init(tracker, m_mask);
664 }
665 }
666
667 m_cMo = cdMo;
668 c0Mo = m_cMo;
669 ctTc0.eye();
670 }
671}
672
683{
684 vpMbKltTracker::setPose(&I, NULL, cdMo);
685}
686
697{
698 vpMbKltTracker::setPose(NULL, &I_color, cdMo);
699}
700
708{
710 kltPoly->setCameraParameters(m_cam);
711 kltPoly->polygon = &polygon;
712 kltPoly->hiddenface = &faces;
713 kltPoly->useScanLine = useScanLine;
714 kltPolygons.push_back(kltPoly);
715}
723{
725 kltPoly->setCameraParameters(m_cam);
726 kltPoly->polygon = &polygon;
727 kltPoly->hiddenface = &faces;
728 kltPoly->useScanLine = useScanLine;
729 kltPolygons.push_back(kltPoly);
730}
731
739{
742
743 m_nbInfos = 0;
744 m_nbFaceUsed = 0;
745 // for (unsigned int i = 0; i < faces.size(); i += 1){
746 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
747 vpMbtDistanceKltPoints *kltpoly = *it;
748 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
750 // faces[i]->ransac();
751 if (kltpoly->hasEnoughPoints()) {
752 m_nbInfos += kltpoly->getCurrentNumberPoints();
753 m_nbFaceUsed++;
754 }
755 }
756 }
757
758 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
759 ++it) {
760 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
761
762 if (kltPolyCylinder->isTracked()) {
763 kltPolyCylinder->computeNbDetectedCurrent(tracker);
764 if (kltPolyCylinder->hasEnoughPoints()) {
765 m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
766 m_nbFaceUsed++;
767 }
768 }
769 }
770}
771
776{
777 // # For a better Post Tracking, tracker should reinitialize if so faces
778 // don't have enough points but are visible. # Here we are not doing it for
779 // more speed performance.
780 bool reInitialisation = false;
781
782 unsigned int initialNumber = 0;
783 unsigned int currentNumber = 0;
784 unsigned int shift = 0;
785 // for (unsigned int i = 0; i < faces.size(); i += 1){
786 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
787 vpMbtDistanceKltPoints *kltpoly = *it;
788 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
789 initialNumber += kltpoly->getInitialNumberPoint();
790 if (kltpoly->hasEnoughPoints()) {
791 vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
792 shift += 2 * kltpoly->getCurrentNumberPoints();
793 kltpoly->removeOutliers(sub_w, threshold_outlier);
794
795 currentNumber += kltpoly->getCurrentNumberPoints();
796 }
797 // else{
798 // reInitialisation = true;
799 // break;
800 // }
801 }
802 }
803
804 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
805 ++it) {
806 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
807
808 if (kltPolyCylinder->isTracked()) {
809 initialNumber += kltPolyCylinder->getInitialNumberPoint();
810 if (kltPolyCylinder->hasEnoughPoints()) {
811 vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
812 shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
813 kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
814
815 currentNumber += kltPolyCylinder->getCurrentNumberPoints();
816 }
817 }
818 }
819
820 // if(!reInitialisation){
821 double value = percentGood * (double)initialNumber;
822 if ((double)currentNumber < value) {
823 // std::cout << "Too many point disappear : " << initialNumber << "/"
824 // << currentNumber << std::endl;
825 reInitialisation = true;
826 } else {
827 if (!useOgre)
829 else {
830#ifdef VISP_HAVE_OGRE
832#else
834#endif
835 }
836 }
837 // }
838
839 if (reInitialisation)
840 return true;
841
842 return false;
843}
844
849{
850 vpMatrix L_true; // interaction matrix without weighting
851 vpMatrix LVJ_true;
852 vpColVector v; // "speed" for VVS
853
854 vpMatrix LTL;
855 vpColVector LTR;
856 vpHomogeneousMatrix cMoPrev;
857 vpHomogeneousMatrix ctTc0_Prev;
858 vpColVector error_prev;
859 double mu = m_initialMu;
860
861 double normRes = 0;
862 double normRes_1 = -1;
863 unsigned int iter = 0;
864
866
867 while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
869
870 bool reStartFromLastIncrement = false;
871 computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
872 if (reStartFromLastIncrement) {
873 ctTc0 = ctTc0_Prev;
874 }
875
876 if (!reStartFromLastIncrement) {
878
879 if (computeCovariance) {
880 L_true = m_L_klt;
881 if (!isoJoIdentity) {
883 cVo.buildFrom(m_cMo);
884 LVJ_true = (m_L_klt * cVo * oJo);
885 }
886 }
887
888 normRes_1 = normRes;
889 normRes = 0.0;
890
891 for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
893 normRes += m_weightedError_klt[i];
894 }
895
896 if ((iter == 0) || m_computeInteraction) {
897 for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
898 for (unsigned int j = 0; j < 6; j++) {
899 m_L_klt[i][j] *= m_w_klt[i];
900 }
901 }
902 }
903
905 v);
906
907 cMoPrev = m_cMo;
908 ctTc0_Prev = ctTc0;
910 m_cMo = ctTc0 * c0Mo;
911 } // endif(!reStartFromLastIncrement)
912
913 iter++;
914 }
915
916 computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
917}
918
920{
921 unsigned int nbFeatures = 2 * m_nbInfos;
922
923 m_L_klt.resize(nbFeatures, 6, false, false);
924 m_error_klt.resize(nbFeatures, false);
925
926 m_weightedError_klt.resize(nbFeatures, false);
927 m_w_klt.resize(nbFeatures, false);
928 m_w_klt = 1;
929
931}
932
934{
935 unsigned int shift = 0;
936 vpHomography H;
937
938 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
939 vpMbtDistanceKltPoints *kltpoly = *it;
940 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
941 kltpoly->hasEnoughPoints()) {
942 vpSubColVector subR(m_error_klt, shift, 2 * kltpoly->getCurrentNumberPoints());
943 vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltpoly->getCurrentNumberPoints(), 6);
944
945 try {
946 kltpoly->computeHomography(ctTc0, H);
947 kltpoly->computeInteractionMatrixAndResidu(subR, subL);
948 } catch (...) {
949 throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
950 }
951
952 shift += 2 * kltpoly->getCurrentNumberPoints();
953 }
954 }
955
956 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
957 ++it) {
958 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
959
960 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
961 vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
962 vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
963
964 try {
965 kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
966 } catch (...) {
967 throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
968 }
969
970 shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
971 }
972 }
973}
974
983{
984 preTracking(I);
985
986 if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
987 throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
988 }
989
990 computeVVS();
991
992 if (postTracking(I, m_w_klt))
993 reinit(I);
994
995 if (displayFeatures) {
997 }
998}
999
1008{
1009 vpImageConvert::convert(I_color, m_I);
1011
1012 if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
1013 throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
1014 }
1015
1016 computeVVS();
1017
1018 if (postTracking(m_I, m_w_klt))
1019 reinit(m_I);
1020}
1021
1065void vpMbKltTracker::loadConfigFile(const std::string &configFile, bool verbose)
1066{
1067 // Load projection error config
1068 vpMbTracker::loadConfigFile(configFile, verbose);
1069
1071 xmlp.setVerbose(verbose);
1072 xmlp.setKltMaxFeatures(10000);
1073 xmlp.setKltWindowSize(5);
1074 xmlp.setKltQuality(0.01);
1075 xmlp.setKltMinDistance(5);
1076 xmlp.setKltHarrisParam(0.01);
1077 xmlp.setKltBlockSize(3);
1078 xmlp.setKltPyramidLevels(3);
1082
1083 try {
1084 if (verbose) {
1085 std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
1086 }
1087 xmlp.parse(configFile.c_str());
1088 } catch (...) {
1089 vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile.c_str());
1090 throw vpException(vpException::ioError, "problem to parse configuration file.");
1091 }
1092
1093 vpCameraParameters camera;
1094 xmlp.getCameraParameters(camera);
1095 setCameraParameters(camera);
1096
1107
1108 // if(useScanLine)
1109 faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
1110
1111 if (xmlp.hasNearClippingDistance())
1113
1114 if (xmlp.hasFarClippingDistance())
1116
1117 if (xmlp.getFovClipping())
1119
1120 useLodGeneral = xmlp.getLodState();
1123
1125 if (this->getNbPolygon() > 0) {
1130 }
1131}
1132
1145 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1146 bool displayFullModel)
1147{
1148 std::vector<std::vector<double> > models = vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1149
1150 for (size_t i = 0; i < models.size(); i++) {
1151 if (vpMath::equal(models[i][0], 0)) {
1152 vpImagePoint ip1(models[i][1], models[i][2]);
1153 vpImagePoint ip2(models[i][3], models[i][4]);
1154 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1155 } else if (vpMath::equal(models[i][0], 1)) {
1156 vpImagePoint center(models[i][1], models[i][2]);
1157 double n20 = models[i][3];
1158 double n11 = models[i][4];
1159 double n02 = models[i][5];
1160 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1161 }
1162 }
1163
1164 if (displayFeatures) {
1165 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1169
1171 double id = m_featuresToBeDisplayedKlt[i][5];
1172 std::stringstream ss;
1173 ss << id;
1174 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1175 }
1176 }
1177 }
1178
1179#ifdef VISP_HAVE_OGRE
1180 if (useOgre)
1181 faces.displayOgre(cMo);
1182#endif
1183}
1184
1197 const vpCameraParameters &cam, const vpColor &col, unsigned int thickness,
1198 bool displayFullModel)
1199{
1200 std::vector<std::vector<double> > models = vpMbKltTracker::getModelForDisplay(I.getWidth(), I.getHeight(), cMo, cam, displayFullModel);
1201
1202 for (size_t i = 0; i < models.size(); i++) {
1203 if (vpMath::equal(models[i][0], 0)) {
1204 vpImagePoint ip1(models[i][1], models[i][2]);
1205 vpImagePoint ip2(models[i][3], models[i][4]);
1206 vpDisplay::displayLine(I, ip1, ip2, col, thickness);
1207 } else if (vpMath::equal(models[i][0], 1)) {
1208 vpImagePoint center(models[i][1], models[i][2]);
1209 double n20 = models[i][3];
1210 double n11 = models[i][4];
1211 double n02 = models[i][5];
1212 vpDisplay::displayEllipse(I, center, n20, n11, n02, true, col, thickness);
1213 }
1214 }
1215
1216 if (displayFeatures) {
1217 for (size_t i = 0; i < m_featuresToBeDisplayedKlt.size(); i++) {
1221
1223 double id = m_featuresToBeDisplayedKlt[i][5];
1224 std::stringstream ss;
1225 ss << id;
1226 vpDisplay::displayText(I, ip2, ss.str(), vpColor::red);
1227 }
1228 }
1229 }
1230
1231#ifdef VISP_HAVE_OGRE
1232 if (useOgre)
1233 faces.displayOgre(cMo);
1234#endif
1235}
1236
1237std::vector<std::vector<double> > vpMbKltTracker::getFeaturesForDisplayKlt()
1238{
1239 std::vector<std::vector<double> > features;
1240
1241 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1242 vpMbtDistanceKltPoints *kltpoly = *it;
1243
1244 if (kltpoly->hasEnoughPoints() && kltpoly->polygon->isVisible() && kltpoly->isTracked()) {
1245 std::vector<std::vector<double> > currentFeatures = kltpoly->getFeaturesForDisplay();
1246 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1247 }
1248 }
1249
1250 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1251 ++it) {
1252 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1253
1254 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
1255 std::vector<std::vector<double> > currentFeatures = kltPolyCylinder->getFeaturesForDisplay();
1256 features.insert(features.end(), currentFeatures.begin(), currentFeatures.end());
1257 }
1258 }
1259
1260 return features;
1261}
1262
1277std::vector<std::vector<double> > vpMbKltTracker::getModelForDisplay(unsigned int width, unsigned int height,
1278 const vpHomogeneousMatrix &cMo,
1279 const vpCameraParameters &cam,
1280 bool displayFullModel)
1281{
1282 std::vector<std::vector<double> > models;
1283
1284 vpCameraParameters c = cam;
1285
1286 if (clippingFlag > 3) // Contains at least one FOV constraint
1287 c.computeFov(width, height);
1288
1289 // vpMbtDistanceKltPoints *kltpoly;
1290 // vpMbtDistanceKltCylinder *kltPolyCylinder;
1291
1292 // Previous version 12/08/2015
1293 // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
1294 // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
1295 // kltpoly = *it;
1296 // kltpoly->polygon->changeFrame(cMo_);
1297 // kltpoly->polygon->computePolygonClipped(c);
1298 // }
1300
1301 if (useScanLine && !displayFullModel)
1302 faces.computeScanLineRender(m_cam, width, height);
1303
1304 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1305 vpMbtDistanceKltPoints *kltpoly = *it;
1306 std::vector<std::vector<double> > modelLines = kltpoly->getModelForDisplay(cam, displayFullModel);
1307 models.insert(models.end(), modelLines.begin(), modelLines.end());
1308 }
1309
1310 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1311 ++it) {
1312 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1313 std::vector<std::vector<double> > modelLines = kltPolyCylinder->getModelForDisplay(cMo, cam);
1314 models.insert(models.end(), modelLines.begin(), modelLines.end());
1315 }
1316
1317 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1318 vpMbtDistanceCircle *displayCircle = *it;
1319 std::vector<double> paramsCircle = displayCircle->getModelForDisplay(cMo, cam, displayFullModel);
1320 if (!paramsCircle.empty()) {
1321 models.push_back(paramsCircle);
1322 }
1323 }
1324
1325 return models;
1326}
1327
1335{
1336 unsigned int nbTotalPoints = 0;
1337 // for (unsigned int i = 0; i < faces.size(); i += 1){
1338 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1339 vpMbtDistanceKltPoints *kltpoly = *it;
1340 if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
1341 kltpoly->hasEnoughPoints()) {
1342 nbTotalPoints += kltpoly->getCurrentNumberPoints();
1343 }
1344 }
1345
1346 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1347 ++it) {
1348 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1349 if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
1350 nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
1351 }
1352
1353 if (nbTotalPoints < 10) {
1354 std::cerr << "test tracking failed (too few points to realize a good tracking)." << std::endl;
1356 "test tracking failed (too few points to realize a good tracking).");
1357 }
1358}
1359
1370void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, double radius, int idFace,
1371 const std::string & /*name*/)
1372{
1374 kltPoly->setCameraParameters(m_cam);
1375
1376 kltPoly->buildFrom(p1, p2, radius);
1377
1378 // Add the Cylinder BBox to the list of polygons
1379 kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
1380 kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
1381 kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
1382 kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
1383
1384 kltPoly->hiddenface = &faces;
1385 kltPoly->useScanLine = useScanLine;
1386 kltCylinders.push_back(kltPoly);
1387}
1388
1400void vpMbKltTracker::initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, double radius,
1401 int /*idFace*/, const std::string &name)
1402{
1403 addCircle(p1, p2, p3, radius, name);
1404}
1405
1414void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, double r,
1415 const std::string &name)
1416{
1417 bool already_here = false;
1418
1419 // for(std::list<vpMbtDistanceCircle*>::const_iterator
1420 // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
1421 // ci = *it;
1422 // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
1423 // samePoint(*(ci->p3),P3)) ||
1424 // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
1425 // samePoint(*(ci->p3),P2)) ){
1426 // already_here = (std::fabs(ci->radius - r) <
1427 // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
1428 // r));
1429 // }
1430 // }
1431
1432 if (!already_here) {
1434
1436 ci->setName(name);
1437 ci->buildFrom(P1, P2, P3, r);
1438 circles_disp.push_back(ci);
1439 }
1440}
1441
1454void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
1455 const vpHomogeneousMatrix &cMo, bool verbose,
1456 const vpHomogeneousMatrix &T)
1457{
1458 m_cMo.eye();
1459
1460#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
1461 if (cur != NULL) {
1462 cvReleaseImage(&cur);
1463 cur = NULL;
1464 }
1465#endif
1466
1467 firstInitialisation = true;
1468
1469 // delete the Klt Polygon features
1470 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1471 vpMbtDistanceKltPoints *kltpoly = *it;
1472 if (kltpoly != NULL) {
1473 delete kltpoly;
1474 }
1475 kltpoly = NULL;
1476 }
1477 kltPolygons.clear();
1478
1479 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
1480 ++it) {
1481 vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
1482 if (kltPolyCylinder != NULL) {
1483 delete kltPolyCylinder;
1484 }
1485 kltPolyCylinder = NULL;
1486 }
1487 kltCylinders.clear();
1488
1489 // delete the structures used to display circles
1490 for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
1491 vpMbtDistanceCircle *ci = *it;
1492 if (ci != NULL) {
1493 delete ci;
1494 }
1495 ci = NULL;
1496 }
1497
1498 faces.reset();
1499
1500 loadModel(cad_name, verbose, T);
1501 initFromPose(I, cMo);
1502}
1503
1511void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
1512{
1513 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
1514 vpMbtDistanceKltPoints *kltpoly = *it;
1515 if (kltpoly->polygon->getName() == name) {
1516 kltpoly->setTracked(useKltTracking);
1517 }
1518 }
1519}
1520
1521#elif !defined(VISP_BUILD_SHARED_LIBS)
1522// Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
1523// symbols
1524void dummy_vpMbKltTracker(){};
1525#endif // VISP_HAVE_OPENCV
void setWindowName(const Ogre::String &n)
Definition: vpAROgre.h:269
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
double get_px_inverse() const
double get_py_inverse() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector & normalize()
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayEllipse(const vpImage< unsigned char > &I, const vpImagePoint &center, const double &coef1, const double &coef2, const double &coef3, bool use_normalized_centered_moments, const vpColor &color, unsigned int thickness=1, bool display_center=false, bool display_arc=false)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ fatalError
Fatal error.
Definition: vpException.h:96
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
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.
Definition: vpHomography.h:175
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getCols() const
Definition: vpImage.h:179
unsigned int getHeight() const
Definition: vpImage.h:188
unsigned int getRows() const
Definition: vpImage.h:218
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
double getQuality() const
Definition: vpKltOpencv.h:133
int getMaxFeatures() const
Get the list of lost feature.
Definition: vpKltOpencv.h:115
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
void setTrackerId(int tid)
Definition: vpKltOpencv.h:155
int getWindowSize() const
Get the window size used to refine the corner locations.
Definition: vpKltOpencv.h:135
int getNbFeatures() const
Get the number of current features.
Definition: vpKltOpencv.h:120
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.
Definition: vpKltOpencv.h:111
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
Definition: vpKltOpencv.h:118
void setMinDistance(double minDistance)
int getBlockSize() const
Get the size of the averaging block used to track the features.
Definition: vpKltOpencv.h:102
int getPyramidLevels() const
Get the list of features id.
Definition: vpKltOpencv.h:130
void setUseHarris(int useHarrisDetector)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:110
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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()
double threshold_outlier
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.
Definition: vpMbTracker.h:187
virtual void computeCovarianceMatrixVVS(const bool isoJoIdentity_, const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev, const vpMatrix &L_true, const vpMatrix &LVJ_true, const vpColVector &error)
bool modelInitialised
Definition: vpMbTracker.h:123
double minLineLengthThresholdGeneral
Minimum line length threshold for LOD mode (general setting)
Definition: vpMbTracker.h:177
virtual void setMinLineLengthThresh(double minLineLengthThresh, const std::string &name="")
vpImage< unsigned char > m_I
Grayscale image buffer, used when passing color images.
Definition: vpMbTracker.h:223
virtual void computeVVSCheckLevenbergMarquardt(unsigned int iter, vpColVector &error, const vpColVector &m_error_prev, const vpHomogeneousMatrix &cMoPrev, double &mu, bool &reStartFromLastIncrement, vpColVector *const w=NULL, const vpColVector *const m_w_prev=NULL)
bool useLodGeneral
True if LOD mode is enabled.
Definition: vpMbTracker.h:172
double minPolygonAreaThresholdGeneral
Minimum polygon area threshold for LOD mode (general setting)
Definition: vpMbTracker.h:179
bool m_computeInteraction
Definition: vpMbTracker.h:185
vpMatrix oJo
The Degrees of Freedom to estimate.
Definition: vpMbTracker.h:115
virtual void setMinPolygonAreaThresh(double minPolygonAreaThresh, const std::string &name="")
double m_initialMu
Initial Mu for Levenberg Marquardt optimization loop.
Definition: vpMbTracker.h:193
vpHomogeneousMatrix m_cMo
The current pose.
Definition: vpMbTracker.h:113
virtual void computeVVSPoseEstimation(const bool isoJoIdentity_, unsigned int iter, vpMatrix &L, vpMatrix &LTL, vpColVector &R, const vpColVector &error, vpColVector &error_prev, vpColVector &LTR, double &mu, vpColVector &v, const vpColVector *const w=NULL, vpColVector *const m_w_prev=NULL)
vpCameraParameters m_cam
The camera parameters.
Definition: vpMbTracker.h:111
bool useOgre
Use Ogre3d for visibility tests.
Definition: vpMbTracker.h:155
virtual void computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
vpMbHiddenFaces< vpMbtPolygon > faces
Set of faces describing the object.
Definition: vpMbTracker.h:143
bool isoJoIdentity
Boolean to know if oJo is identity (for fast computation)
Definition: vpMbTracker.h:117
virtual void setLod(bool useLod, const std::string &name="")
vpMbtOptimizationMethod m_optimizationMethod
Optimization method used.
Definition: vpMbTracker.h:140
bool displayFeatures
If true, the features are displayed.
Definition: vpMbTracker.h:138
double angleDisappears
Angle used to detect a face disappearance.
Definition: vpMbTracker.h:147
virtual unsigned int getNbPolygon() const
Definition: vpMbTracker.h:368
virtual void setNearClippingDistance(const double &dist)
bool applyLodSettingInConfig
Definition: vpMbTracker.h:175
virtual void setFarClippingDistance(const double &dist)
bool useScanLine
Use Scanline for visibility tests.
Definition: vpMbTracker.h:158
virtual void setClipping(const unsigned int &flags)
double angleAppears
Angle used to detect a face appearance.
Definition: vpMbTracker.h:145
const vpImage< bool > * m_mask
Mask used to disable tracking on a part of image.
Definition: vpMbTracker.h:221
virtual void initFromPose(const vpImage< unsigned char > &I, const std::string &initFile)
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
bool computeCovariance
Flag used to specify if the covariance matrix has to be computed or not.
Definition: vpMbTracker.h:128
unsigned int m_maxIter
Maximum number of iterations of the virtual visual servoing stage.
Definition: vpMbTracker.h:189
bool ogreShowConfigDialog
Definition: vpMbTracker.h:156
unsigned int clippingFlag
Flags specifying which clipping to used.
Definition: vpMbTracker.h:153
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
Manage a circle used in the model-based tracker.
void 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.
Definition: vpMbtPolygon.h:67
std::string getName() const
Definition: vpMbtPolygon.h:108
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:59
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
double getD() const
Definition: vpPlane.h:108
vpColVector getNormal() const
Definition: vpPlane.cpp:238
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void changeFrame(const vpHomogeneousMatrix &cMo)
unsigned int getNbPoint() const
Definition: vpPolygon3D.h:132
vpPoint * p
corners in the object frame
Definition: vpPolygon3D.h:81
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
void setMinMedianAbsoluteDeviation(double mad_min)
Definition: vpRobust.h:161
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:63
Error that can be emited by the vpTracker class and its derivates.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpERROR_TRACE
Definition: vpDebug.h:393