Visual Servoing Platform version 3.5.0
mbtGenericTracking2.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 * Example of Hybrid Tracking of MBT and MBT KTL.
33 *
34 * Authors:
35 * Aurelien Yol
36 * Souriya Trinh
37 *
38 *****************************************************************************/
39
47#include <iostream>
48#include <visp3/core/vpConfig.h>
49
50#if (defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)) \
51 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
52
53#include <visp3/core/vpDebug.h>
54#include <visp3/core/vpHomogeneousMatrix.h>
55#include <visp3/core/vpIoTools.h>
56#include <visp3/core/vpMath.h>
57#include <visp3/gui/vpDisplayD3D.h>
58#include <visp3/gui/vpDisplayGDI.h>
59#include <visp3/gui/vpDisplayGTK.h>
60#include <visp3/gui/vpDisplayOpenCV.h>
61#include <visp3/gui/vpDisplayX.h>
62#include <visp3/io/vpImageIo.h>
63#include <visp3/io/vpParseArgv.h>
64#include <visp3/io/vpVideoReader.h>
65#include <visp3/mbt/vpMbGenericTracker.h>
66
67#define GETOPTARGS "x:m:i:n:de:chtfColwvpT:"
68
69#define USE_XML 0
70
71void usage(const char *name, const char *badparam)
72{
73 fprintf(stdout, "\n\
74Example of tracking based on the 3D model.\n\
75\n\
76SYNOPSIS\n\
77 %s [-i <test image path>] [-x <config file>]\n\
78 [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
79 [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n\
80 [-T <tracker type>]\n", name);
81
82 fprintf(stdout, "\n\
83OPTIONS: \n\
84 -i <input image path> \n\
85 Set image input path.\n\
86 From this path read images \n\
87 \"mbt/cube/image%%04d.ppm\". These \n\
88 images come from ViSP-images-x.y.z.tar.gz available \n\
89 on the ViSP website.\n\
90 Setting the VISP_INPUT_IMAGE_PATH environment\n\
91 variable produces the same behavior than using\n\
92 this option.\n\
93\n\
94 -x <config file> \n\
95 Set the config file (the xml file) to use.\n\
96 The config file is used to specify the parameters of the tracker.\n\
97\n\
98 -m <model name> \n\
99 Specify the name of the file of the model\n\
100 The model can either be a vrml model (.wrl) or a .cao file.\n\
101\n\
102 -e <last frame index> \n\
103 Specify the index of the last frame. Once reached, the tracking is stopped\n\
104\n\
105 -f \n\
106 Do not use the vrml model, use the .cao one. These two models are \n\
107 equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
108 website. However, the .cao model allows to use the 3d model based tracker \n\
109 without Coin.\n\
110\n\
111 -C \n\
112 Track only the cube (not the cylinder). In this case the models files are\n\
113 cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
114 cube_and_cylinder.wrl.\n\
115\n\
116 -n <initialisation file base name> \n\
117 Base name of the initialisation file. The file will be 'base_name'.init .\n\
118 This base name is also used for the optional picture specifying where to \n\
119 click (a .ppm picture).\n\
120\n\
121 -t \n\
122 Turn off the display of the the moving edges and Klt points. \n\
123\n\
124 -d \n\
125 Turn off the display.\n\
126\n\
127 -c\n\
128 Disable the mouse click. Useful to automate the \n\
129 execution of this program without human intervention.\n\
130\n\
131 -o\n\
132 Use Ogre3D for visibility tests\n\
133\n\
134 -w\n\
135 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
136\n\
137 -l\n\
138 Use the scanline for visibility tests.\n\
139\n\
140 -v\n\
141 Compute covariance matrix.\n\
142\n\
143 -p\n\
144 Compute gradient projection error.\n\
145\n\
146 -T <tracker type>\n\
147 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (EdgeKlt)>).\n\
148\n\
149 -h \n\
150 Print the help.\n\n");
151
152 if (badparam)
153 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
154}
155
156bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
157 std::string &initFile, long &lastFrame, bool &displayFeatures, bool &click_allowed, bool &display,
158 bool &cao3DModel, bool &trackCylinder, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline,
159 bool &computeCovariance, bool &projectionError, int &trackerType)
160{
161 const char *optarg_;
162 int c;
163 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
164
165 switch (c) {
166 case 'e':
167 lastFrame = atol(optarg_);
168 break;
169 case 'i':
170 ipath = optarg_;
171 break;
172 case 'x':
173 configFile = optarg_;
174 break;
175 case 'm':
176 modelFile = optarg_;
177 break;
178 case 'n':
179 initFile = optarg_;
180 break;
181 case 't':
182 displayFeatures = false;
183 break;
184 case 'f':
185 cao3DModel = true;
186 break;
187 case 'c':
188 click_allowed = false;
189 break;
190 case 'd':
191 display = false;
192 break;
193 case 'C':
194 trackCylinder = false;
195 break;
196 case 'o':
197 useOgre = true;
198 break;
199 case 'l':
200 useScanline = true;
201 break;
202 case 'w':
203 showOgreConfigDialog = true;
204 break;
205 case 'v':
206 computeCovariance = true;
207 break;
208 case 'p':
209 projectionError = true;
210 break;
211 case 'T':
212 trackerType = atoi(optarg_);
213 break;
214 case 'h':
215 usage(argv[0], NULL);
216 return false;
217 break;
218
219 default:
220 usage(argv[0], optarg_);
221 return false;
222 break;
223 }
224 }
225
226 if ((c == 1) || (c == -1)) {
227 // standalone param or error
228 usage(argv[0], NULL);
229 std::cerr << "ERROR: " << std::endl;
230 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
231 return false;
232 }
233
234 return true;
235}
236
237int main(int argc, const char **argv)
238{
239 try {
240 std::string env_ipath;
241 std::string opt_ipath;
242 std::string ipath;
243 std::string opt_configFile;
244 std::string opt_modelFile;
245 std::string modelFile;
246 std::string opt_initFile;
247 std::string initFile;
248 long opt_lastFrame = -1;
249 bool displayFeatures = true;
250 bool opt_click_allowed = true;
251 bool opt_display = true;
252 bool cao3DModel = false;
253 bool trackCylinder = true;
254 bool useOgre = false;
255 bool showOgreConfigDialog = false;
256 bool useScanline = false;
257 bool computeCovariance = false;
258 bool projectionError = false;
259 int trackerType = vpMbGenericTracker::EDGE_TRACKER;
260
261 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
262 // environment variable value
264
265 // Set the default input path
266 if (!env_ipath.empty())
267 ipath = env_ipath;
268
269 // Read the command line options
270 if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures,
271 opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog,
272 useScanline, computeCovariance, projectionError, trackerType)) {
273 return EXIT_FAILURE;
274 }
275
276 // Test if an input path is set
277 if (opt_ipath.empty() && env_ipath.empty()) {
278 usage(argv[0], NULL);
279 std::cerr << std::endl << "ERROR:" << std::endl;
280 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
281 << " environment variable to specify the location of the " << std::endl
282 << " image path where test images are located." << std::endl
283 << std::endl;
284
285 return EXIT_FAILURE;
286 }
287
288 // Get the option values
289 if (!opt_ipath.empty())
290 ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
291 else
292 ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
293
294#if USE_XML
295 std::string configFile;
296 if (!opt_configFile.empty())
297 configFile = opt_configFile;
298 else if (!opt_ipath.empty())
299 configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
300 else
301 configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
302#endif
303
304 if (!opt_modelFile.empty()) {
305 modelFile = opt_modelFile;
306 } else {
307 std::string modelFileCao;
308 std::string modelFileWrl;
309 if (trackCylinder) {
310 modelFileCao = "mbt/cube_and_cylinder.cao";
311 modelFileWrl = "mbt/cube_and_cylinder.wrl";
312 } else {
313 modelFileCao = "mbt/cube.cao";
314 modelFileWrl = "mbt/cube.wrl";
315 }
316
317 if (!opt_ipath.empty()) {
318 if (cao3DModel) {
319 modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
320 } else {
321#ifdef VISP_HAVE_COIN3D
322 modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
323#else
324 std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
325 modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
326#endif
327 }
328 } else {
329 if (cao3DModel) {
330 modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
331 } else {
332#ifdef VISP_HAVE_COIN3D
333 modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
334#else
335 std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
336 modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
337#endif
338 }
339 }
340 }
341
342 if (!opt_initFile.empty())
343 initFile = opt_initFile;
344 else if (!opt_ipath.empty())
345 initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
346 else
347 initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
348
349 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
350 vpImage<unsigned char> I1, I2, I3;
351 vpVideoReader reader;
352
353 reader.setFileName(ipath);
354 try {
355 reader.open(I1);
356 I2 = I1;
357 I3 = I1;
358 } catch (...) {
359 std::cerr << "Cannot open sequence: " << ipath << std::endl;
360 return EXIT_FAILURE;
361 }
362
363 if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
364 reader.setLastFrameIndex(opt_lastFrame);
365
366 reader.acquire(I1);
367 I2 = I1;
368 I3 = I1;
369
370 mapOfImages["Camera1"] = &I1;
371 mapOfImages["Camera2"] = &I2;
372 mapOfImages["Camera3"] = &I3;
373
374// initialise a display
375#if defined VISP_HAVE_X11
376 vpDisplayX display1, display2, display3;
377#elif defined VISP_HAVE_GDI
378 vpDisplayGDI display1, display2, display3;
379#elif defined VISP_HAVE_OPENCV
380 vpDisplayOpenCV display1, display2, display3;
381#elif defined VISP_HAVE_D3D9
382 vpDisplayD3D display1, display2, display3;
383#elif defined VISP_HAVE_GTK
384 vpDisplayGTK display1, display2, display3;
385#else
386 opt_display = false;
387#endif
388
389 if (opt_display) {
390#if defined(VISP_HAVE_DISPLAY)
394
395 display1.init(I1, 100, 100, "Test tracking (Cam1)");
396 display2.init(I2, (int)(I1.getWidth() / vpDisplay::getDownScalingFactor(I1)) + 110, 100, "Test tracking (Cam2)");
397 display3.init(I3, 100, (int)(I1.getHeight() / vpDisplay::getDownScalingFactor(I1)) + 110, "Test tracking (Cam3)");
398#endif
402
406 }
407
408 // Object pointer to check that inheritance is ok
409 vpMbTracker *tracker = new vpMbGenericTracker(3, trackerType);
410 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
411 std::map<std::string, vpCameraParameters> mapOfCameraParams;
412
413// Initialise the tracker: camera parameters, moving edge and KLT settings
414#if USE_XML
415 // From the xml file
416 std::map<std::string, std::string> mapOfConfigFiles;
417 mapOfConfigFiles["Camera1"] = configFile;
418 mapOfConfigFiles["Camera2"] = configFile;
419 mapOfConfigFiles["Camera3"] = configFile;
420 dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(mapOfConfigFiles);
421#else
422 // By setting the parameters:
424 cam.initPersProjWithoutDistortion(547, 542, 338, 234);
425 mapOfCameraParams["Camera1"] = cam;
426 mapOfCameraParams["Camera2"] = cam;
427 mapOfCameraParams["Camera3"] = cam;
428
429 vpMe me;
430 me.setMaskSize(5);
431 me.setMaskNumber(180);
432 me.setRange(7);
433 me.setThreshold(5000);
434 me.setMu1(0.5);
435 me.setMu2(0.5);
436 me.setSampleStep(4);
437 std::map<std::string, vpMe> mapOfMe;
438 mapOfMe["Camera1"] = me;
439 mapOfMe["Camera2"] = me;
440 mapOfMe["Camera3"] = me;
441
442#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
443 vpKltOpencv klt;
444 klt.setMaxFeatures(10000);
445 klt.setWindowSize(5);
446 klt.setQuality(0.01);
447 klt.setMinDistance(5);
448 klt.setHarrisFreeParameter(0.01);
449 klt.setBlockSize(3);
450 klt.setPyramidLevels(3);
451 std::map<std::string, vpKltOpencv> mapOfKlt;
452 mapOfKlt["Camera1"] = klt;
453 mapOfKlt["Camera2"] = klt;
454 mapOfKlt["Camera3"] = klt;
455
456 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(mapOfKlt);
457 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
458#endif
459
460 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
461 dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(mapOfMe);
462 tracker->setAngleAppear(vpMath::rad(65));
463 tracker->setAngleDisappear(vpMath::rad(75));
464
465 // Specify the clipping to
466 tracker->setNearClippingDistance(0.01);
467 tracker->setFarClippingDistance(0.90);
468
469 std::map<std::string, unsigned int> mapOfClippingFlags;
470 dynamic_cast<vpMbGenericTracker *>(tracker)->getClipping(mapOfClippingFlags);
471 for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end();
472 ++it) {
473 it->second = (it->second | vpMbtPolygon::FOV_CLIPPING);
474 }
475
476 dynamic_cast<vpMbGenericTracker *>(tracker)->setClipping(mapOfClippingFlags);
477// tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING
478// | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
479// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
480#endif
481
482 // Display the moving edges, and the Klt points
483 tracker->setDisplayFeatures(displayFeatures);
484
485 // Tells if the tracker has to use Ogre3D for visibility tests
486 tracker->setOgreVisibilityTest(useOgre);
487 if (useOgre)
488 tracker->setOgreShowConfigDialog(showOgreConfigDialog);
489
490 // Tells if the tracker has to use the scanline visibility tests
491 tracker->setScanLineVisibilityTest(useScanline);
492
493 // Tells if the tracker has to compute the covariance matrix
494 tracker->setCovarianceComputation(computeCovariance);
495
496 // Tells if the tracker has to compute the projection error
497 tracker->setProjectionErrorComputation(projectionError);
498
499 // Retrieve the camera parameters from the tracker
500 dynamic_cast<vpMbGenericTracker *>(tracker)->getCameraParameters(mapOfCameraParams);
501
502 // Loop to position the cube
503 if (opt_display && opt_click_allowed) {
504 while (!vpDisplay::getClick(I1, false)) {
506 vpDisplay::displayText(I1, 15, 10, "click after positioning the object", vpColor::red);
508 }
509 }
510
511 // Load the 3D model (either a vrml file or a .cao file)
512 tracker->loadModel(modelFile);
513
514 // Initialise the tracker by clicking on the image
515 // This function looks for
516 // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
517 // in the object basis) of the points used for the initialisation
518 // - a ./cube/cube.ppm file to display where the user have to click
519 // (optionnal, set by the third parameter)
520 if (opt_display && opt_click_allowed) {
521 std::map<std::string, std::string> mapOfInitFiles;
522 mapOfInitFiles["Camera1"] = initFile;
523
524 dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(mapOfImages, mapOfInitFiles, true);
525 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
526
527 // display the 3D model at the given pose
528 dynamic_cast<vpMbGenericTracker *>(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
530 } else {
531 vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
532 std::map<std::string, vpHomogeneousMatrix> mapOfInitPoses;
533 mapOfInitPoses["Camera1"] = c1Moi;
534
535 dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(mapOfImages, mapOfInitPoses);
536 }
537
538 // track the model
539 dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages);
540 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
541
542 if (opt_display) {
546 }
547
548 bool quit = false, click = false;
549 while (!reader.end() && !quit) {
550 // acquire a new image
551 reader.acquire(I1);
552 I2 = I1;
553 I3 = I1;
554 mapOfImages["Camera1"] = &I1;
555 mapOfImages["Camera2"] = &I2;
556 mapOfImages["Camera3"] = &I3;
557
558 // display the image
559 if (opt_display) {
563
564 std::stringstream ss;
565 ss << "Num frame: " << reader.getFrameIndex() << "/" << reader.getLastFrameIndex();
566 vpDisplay::displayText(I1, 40, 20, ss.str(), vpColor::red);
567 }
568
569 // Test to reset the tracker
570 if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 10) {
571 std::cout << "----------Test reset tracker----------" << std::endl;
572 if (opt_display) {
576 }
577
578 tracker->resetTracker();
579#if USE_XML
580 dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(mapOfConfigFiles);
581#else
582 // By setting the parameters:
583 cam.initPersProjWithoutDistortion(547, 542, 338, 234);
584 mapOfCameraParams["Camera1"] = cam;
585 mapOfCameraParams["Camera2"] = cam;
586 mapOfCameraParams["Camera3"] = cam;
587
588 me.setMaskSize(5);
589 me.setMaskNumber(180);
590 me.setRange(7);
591 me.setThreshold(5000);
592 me.setMu1(0.5);
593 me.setMu2(0.5);
594 me.setSampleStep(4);
595
596 mapOfMe["Camera1"] = me;
597 mapOfMe["Camera2"] = me;
598 mapOfMe["Camera3"] = me;
599
600#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
601 klt.setMaxFeatures(10000);
602 klt.setWindowSize(5);
603 klt.setQuality(0.01);
604 klt.setMinDistance(5);
605 klt.setHarrisFreeParameter(0.01);
606 klt.setBlockSize(3);
607 klt.setPyramidLevels(3);
608
609 mapOfKlt["Camera1"] = klt;
610 mapOfKlt["Camera2"] = klt;
611 mapOfKlt["Camera3"] = klt;
612
613 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(mapOfKlt);
614 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
615#endif
616
617 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
618 dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(mapOfMe);
619 tracker->setAngleAppear(vpMath::rad(65));
620 tracker->setAngleDisappear(vpMath::rad(75));
621
622 // Specify the clipping to
623 tracker->setNearClippingDistance(0.01);
624 tracker->setFarClippingDistance(0.90);
625
626 dynamic_cast<vpMbGenericTracker *>(tracker)->getClipping(mapOfClippingFlags);
627 for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin();
628 it != mapOfClippingFlags.end(); ++it) {
629 it->second = (it->second | vpMbtPolygon::FOV_CLIPPING);
630 }
631
632 dynamic_cast<vpMbGenericTracker *>(tracker)->setClipping(mapOfClippingFlags);
633// tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING
634// | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
635// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
636#endif
637 tracker->loadModel(modelFile);
638 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
639 tracker->setOgreVisibilityTest(useOgre);
640 tracker->setScanLineVisibilityTest(useScanline);
641 tracker->setCovarianceComputation(computeCovariance);
642 tracker->setProjectionErrorComputation(projectionError);
643 dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(mapOfImages, mapOfCameraPoses);
644 }
645
646 // Test to set an initial pose
647 if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
649 c1Moi.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946);
650 std::map<std::string, vpHomogeneousMatrix> mapOfSetPoses;
651 mapOfSetPoses["Camera1"] = c1Moi;
652
653 std::cout << "Test set pose" << std::endl;
654 dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(mapOfImages, mapOfSetPoses);
655 }
656
657 // track the object: stop tracking from frame 40 to 50
658 if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
659 reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
660 dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages);
661 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
662 if (opt_display) {
663 // display the 3D model
664 if (reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
665 std::map<std::string, const vpImage<unsigned char> *> mapOfSubImages;
666 mapOfSubImages["Camera1"] = &I1;
667 mapOfSubImages["Camera2"] = &I2;
668
669 dynamic_cast<vpMbGenericTracker *>(tracker)->display(mapOfSubImages, mapOfCameraPoses, mapOfCameraParams,
670 vpColor::red, 3);
671 } else {
672 dynamic_cast<vpMbGenericTracker *>(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
673 vpColor::red, 3);
674 }
675 // display the frame
676 vpDisplay::displayFrame(I1, mapOfCameraPoses["Camera1"], mapOfCameraParams["Camera1"], 0.05);
677 vpDisplay::displayFrame(I2, mapOfCameraPoses["Camera2"], mapOfCameraParams["Camera2"], 0.05);
678 vpDisplay::displayFrame(I3, mapOfCameraPoses["Camera3"], mapOfCameraParams["Camera3"], 0.05);
679 }
680 }
681
682 if (opt_click_allowed && opt_display) {
683 vpDisplay::displayText(I1, 10, 10, "Click to quit", vpColor::red);
685 if (vpDisplay::getClick(I1, button, click)) {
686 switch (button) {
688 quit = !click;
689 break;
690
692 click = !click;
693 break;
694
695 default:
696 break;
697 }
698 }
699 }
700
701 if (computeCovariance) {
702 std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
703 }
704
705 if (projectionError) {
706 std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl;
707 }
708
709 if (opt_display) {
713 }
714 }
715
716 std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
717 std::cout << "\nFinal poses, c1Mo:\n"
718 << mapOfCameraPoses["Camera1"] << "\nc2Mo:\n"
719 << mapOfCameraPoses["Camera2"] << "\nc3Mo:\n"
720 << mapOfCameraPoses["Camera3"] << std::endl;
721
722 if (opt_click_allowed && !quit) {
724 }
725 reader.close();
726
727 delete tracker;
728 tracker = NULL;
729
730 return EXIT_SUCCESS;
731 } catch (const vpException &e) {
732 std::cout << "Catch an exception: " << e << std::endl;
733 return EXIT_FAILURE;
734 }
735}
736
737#elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
738int main()
739{
740 std::cout << "Cannot run this example: visp_mbt, visp_gui modules are required."
741 << std::endl;
742 return EXIT_SUCCESS;
743}
744#else
745int main()
746{
747 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
748 return EXIT_SUCCESS;
749}
750#endif
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static const vpColor red
Definition: vpColor.h:217
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
virtual void setDownScalingFactor(unsigned int scale)
Definition: vpDisplay.cpp:231
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
@ SCALE_AUTO
Definition: vpDisplay.h:183
unsigned int getDownScalingFactor()
Definition: vpDisplay.h:235
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
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getHeight() const
Definition: vpImage.h:188
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
Definition: vpMath.h:110
Real-time 6D object pose tracking using its CAD model.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:105
virtual void resetTracker()=0
virtual void setOgreShowConfigDialog(bool showConfigDialog)
Definition: vpMbTracker.h:643
virtual void setDisplayFeatures(bool displayF)
Definition: vpMbTracker.h:517
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:480
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:499
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:601
virtual void setOgreVisibilityTest(const bool &v)
virtual vpMatrix getCovarianceMatrix() const
Definition: vpMbTracker.h:265
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual double getProjectionError() const
Definition: vpMbTracker.h:310
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:584
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:469
Definition: vpMe.h:61
void setMu1(const double &mu_1)
Definition: vpMe.h:241
void setSampleStep(const double &s)
Definition: vpMe.h:278
void setRange(const unsigned int &r)
Definition: vpMe.h:271
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:459
void setMu2(const double &mu_2)
Definition: vpMe.h:248
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:452
void setThreshold(const double &t)
Definition: vpMe.h:300
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void setLastFrameIndex(const long last_frame)
long getLastFrameIndex()
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
long getFirstFrameIndex()
long getFrameIndex() const