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