Visual Servoing Platform version 3.5.0
mbtKltTracking.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 MBT KLT Tracking.
33 *
34 * Authors:
35 * Aurelien Yol
36 *
37 *****************************************************************************/
38
45#include <iostream>
46#include <visp3/core/vpConfig.h>
47
48#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && \
49 defined(VISP_HAVE_DISPLAY) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
50
51#include <visp3/core/vpDebug.h>
52#include <visp3/core/vpHomogeneousMatrix.h>
53#include <visp3/core/vpIoTools.h>
54#include <visp3/core/vpMath.h>
55#include <visp3/gui/vpDisplayD3D.h>
56#include <visp3/gui/vpDisplayGDI.h>
57#include <visp3/gui/vpDisplayGTK.h>
58#include <visp3/gui/vpDisplayOpenCV.h>
59#include <visp3/gui/vpDisplayX.h>
60#include <visp3/io/vpImageIo.h>
61#include <visp3/io/vpParseArgv.h>
62#include <visp3/io/vpVideoReader.h>
63#include <visp3/mbt/vpMbKltTracker.h>
64
65#define GETOPTARGS "x:m:i:n:de:chtfolwv"
66
67void usage(const char *name, const char *badparam)
68{
69 fprintf(stdout, "\n\
70Example of tracking based on the 3D model.\n\
71\n\
72SYNOPSIS\n\
73 %s [-i <test image path>] [-x <config file>]\n\
74 [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
75 [-t] [-c] [-d] [-h] [-f] [-o] [-w] [-l] [-v]\n", name);
76
77 fprintf(stdout, "\n\
78OPTIONS: \n\
79 -i <input image path> \n\
80 Set image input path.\n\
81 From this path read images \n\
82 \"mbt/cube/image%%04d.ppm\". These \n\
83 images come from ViSP-images-x.y.z.tar.gz available \n\
84 on the ViSP website.\n\
85 Setting the VISP_INPUT_IMAGE_PATH environment\n\
86 variable produces the same behaviour than using\n\
87 this option.\n\
88\n\
89 -x <config file> \n\
90 Set the config file (the xml file) to use.\n\
91 The config file is used to specify the parameters of the tracker.\n\
92\n\
93 -m <model name> \n\
94 Specify the name of the file of the model\n\
95 The model can either be a vrml model (.wrl) or a .cao file.\n\
96\n\
97 -e <last frame index> \n\
98 Specify the index of the last frame. Once reached, the tracking is stopped\n\
99\n\
100 -f \n\
101 Do not use the vrml model, use the .cao one. These two models are \n\
102 equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
103 website. However, the .cao model allows to use the 3d model based tracker \n\
104 without Coin.\n\
105\n\
106 -n <initialisation file base name> \n\
107 Base name of the initialisation file. The file will be 'base_name'.init .\n\
108 This base name is also used for the optionnal picture specifying where to \n\
109 click (a .ppm picture).\n\
110\n\
111 -t \n\
112 Turn off the display of the the klt points. \n\
113\n\
114 -d \n\
115 Turn off the display.\n\
116\n\
117 -c\n\
118 Disable the mouse click. Useful to automaze the \n\
119 execution of this program without humain intervention.\n\
120\n\
121 -o\n\
122 Use Ogre3D for visibility tests\n\
123\n\
124 -w\n\
125 When Ogre3D is enable [-o] show Ogre3D configuration dialog thatallows to set the renderer.\n\
126\n\
127 -l\n\
128 Use the scanline for visibility tests.\n\
129\n\
130 -v\n\
131 Compute covariance matrix.\n\
132\n\
133 -h \n\
134 Print the help.\n\n");
135
136 if (badparam)
137 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
138}
139
140bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
141 std::string &initFile, long &lastFrame, bool &displayKltPoints, bool &click_allowed, bool &display,
142 bool &cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance)
143{
144 const char *optarg_;
145 int c;
146 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
147
148 switch (c) {
149 case 'e':
150 lastFrame = atol(optarg_);
151 break;
152 case 'i':
153 ipath = optarg_;
154 break;
155 case 'x':
156 configFile = optarg_;
157 break;
158 case 'm':
159 modelFile = optarg_;
160 break;
161 case 'n':
162 initFile = optarg_;
163 break;
164 case 't':
165 displayKltPoints = false;
166 break;
167 case 'f':
168 cao3DModel = true;
169 break;
170 case 'c':
171 click_allowed = false;
172 break;
173 case 'd':
174 display = false;
175 break;
176 case 'o':
177 useOgre = true;
178 break;
179 case 'l':
180 useScanline = true;
181 break;
182 case 'w':
183 showOgreConfigDialog = true;
184 break;
185 case 'v':
186 computeCovariance = true;
187 break;
188 case 'h':
189 usage(argv[0], NULL);
190 return false;
191 break;
192
193 default:
194 usage(argv[0], optarg_);
195 return false;
196 break;
197 }
198 }
199
200 if ((c == 1) || (c == -1)) {
201 // standalone param or error
202 usage(argv[0], NULL);
203 std::cerr << "ERROR: " << std::endl;
204 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
205 return false;
206 }
207
208 return true;
209}
210
211int main(int argc, const char **argv)
212{
213 try {
214 std::string env_ipath;
215 std::string opt_ipath;
216 std::string ipath;
217 std::string opt_configFile;
218 std::string configFile;
219 std::string opt_modelFile;
220 std::string modelFile;
221 std::string opt_initFile;
222 std::string initFile;
223 long opt_lastFrame = -1;
224 bool displayKltPoints = true;
225 bool opt_click_allowed = true;
226 bool opt_display = true;
227 bool cao3DModel = false;
228 bool useOgre = false;
229 bool showOgreConfigDialog = false;
230 bool useScanline = false;
231 bool computeCovariance = false;
232 bool quit = false;
233
234 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
235 // environment variable value
237
238 // Set the default input path
239 if (!env_ipath.empty())
240 ipath = env_ipath;
241
242 // Read the command line options
243 if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayKltPoints,
244 opt_click_allowed, opt_display, cao3DModel, useOgre, showOgreConfigDialog, useScanline,
245 computeCovariance)) {
246 return (-1);
247 }
248
249 // Test if an input path is set
250 if (opt_ipath.empty() && env_ipath.empty()) {
251 usage(argv[0], NULL);
252 std::cerr << std::endl << "ERROR:" << std::endl;
253 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
254 << " environment variable to specify the location of the " << std::endl
255 << " image path where test images are located." << std::endl
256 << std::endl;
257
258 return (-1);
259 }
260
261 // Get the option values
262 if (!opt_ipath.empty())
263 ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
264 else
265 ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
266
267 if (!opt_configFile.empty())
268 configFile = opt_configFile;
269 else if (!opt_ipath.empty())
270 configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
271 else
272 configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
273
274 if (!opt_modelFile.empty()) {
275 modelFile = opt_modelFile;
276 } else {
277 std::string modelFileCao = "mbt/cube.cao";
278 std::string modelFileWrl = "mbt/cube.wrl";
279
280 if (!opt_ipath.empty()) {
281 if (cao3DModel) {
282 modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
283 } else {
284#ifdef VISP_HAVE_COIN3D
285 modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
286#else
287 std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
288 modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
289#endif
290 }
291 } else {
292 if (cao3DModel) {
293 modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
294 } else {
295#ifdef VISP_HAVE_COIN3D
296 modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
297#else
298 std::cerr << "Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
299 modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
300#endif
301 }
302 }
303 }
304
305 if (!opt_initFile.empty())
306 initFile = opt_initFile;
307 else if (!opt_ipath.empty())
308 initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
309 else
310 initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
311
313 vpVideoReader reader;
314
315 reader.setFileName(ipath);
316 try {
317 reader.open(I);
318 } catch (...) {
319 std::cout << "Cannot open sequence: " << ipath << std::endl;
320 return -1;
321 }
322
323 if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
324 reader.setLastFrameIndex(opt_lastFrame);
325
326 reader.acquire(I);
327
328// initialise a display
329#if defined VISP_HAVE_X11
330 vpDisplayX display;
331#elif defined VISP_HAVE_GDI
332 vpDisplayGDI display;
333#elif defined VISP_HAVE_OPENCV
334 vpDisplayOpenCV display;
335#elif defined VISP_HAVE_D3D9
336 vpDisplayD3D display;
337#elif defined VISP_HAVE_GTK
338 vpDisplayGTK display;
339#else
340 opt_display = false;
341#endif
342 if (opt_display) {
343#if defined(VISP_HAVE_DISPLAY)
344 display.init(I, 100, 100, "Test tracking");
345#endif
348 }
349
350 vpMbKltTracker tracker;
352
353 // Load tracker config file (camera parameters and moving edge settings)
355 // From the xml file
356 tracker.loadConfigFile(configFile);
357#if 0
358 // Corresponding parameters manually set to have an example code
359 // By setting the parameters:
360 cam.initPersProjWithoutDistortion(547, 542, 338, 234);
361
362 vpKltOpencv klt;
363 klt.setMaxFeatures(10000);
364 klt.setWindowSize(5);
365 klt.setQuality(0.01);
366 klt.setMinDistance(5);
367 klt.setHarrisFreeParameter(0.01);
368 klt.setBlockSize(3);
369 klt.setPyramidLevels(3);
370
371 tracker.setCameraParameters(cam);
372 tracker.setKltOpencv(klt);
373 tracker.setKltMaskBorder(5);
374 tracker.setAngleAppear(vpMath::rad(65));
375 tracker.setAngleDisappear(vpMath::rad(75));
376
377 // Specify the clipping to use
378 tracker.setNearClippingDistance(0.01);
379 tracker.setFarClippingDistance(0.90);
381// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
382// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
383// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
384#endif
385
386 // Display the klt points
387 tracker.setDisplayFeatures(displayKltPoints);
388
389 // Tells if the tracker has to use Ogre3D for visibility tests
390 tracker.setOgreVisibilityTest(useOgre);
391 if (useOgre)
392 tracker.setOgreShowConfigDialog(showOgreConfigDialog);
393
394 // Tells if the tracker has to use the scanline visibility tests
395 tracker.setScanLineVisibilityTest(useScanline);
396
397 // Tells if the tracker has to compute the covariance matrix
398 tracker.setCovarianceComputation(computeCovariance);
399
400 // Retrieve the camera parameters from the tracker
401 tracker.getCameraParameters(cam);
402
403 // Loop to position the cube
404 if (opt_display && opt_click_allowed) {
405 while (!vpDisplay::getClick(I, false)) {
407 vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red);
409 }
410 }
411
412 // Load the 3D model (either a vrml file or a .cao file)
413 tracker.loadModel(modelFile);
414
415 // Initialise the tracker by clicking on the image
416 // This function looks for
417 // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
418 // in the object basis) of the points used for the initialisation
419 // - a ./cube/cube.ppm file to display where the user have to click
420 // (optionnal, set by the third parameter)
421 if (opt_display && opt_click_allowed) {
422 tracker.initClick(I, initFile, true);
423 tracker.getPose(cMo);
424 // display the 3D model at the given pose
425 tracker.display(I, cMo, cam, vpColor::red);
426 } else {
427 vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
428 tracker.initFromPose(I, cMoi);
429 }
430
431 // track the model
432 tracker.track(I);
433 tracker.getPose(cMo);
434
435 if (opt_display)
437
438 while (!reader.end()) {
439 // acquire a new image
440 reader.acquire(I);
441 // display the image
442 if (opt_display)
444
445 // Test to reset the tracker
446 if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 10) {
447 vpTRACE("Test reset tracker");
448 if (opt_display)
450 tracker.resetTracker();
451 tracker.loadConfigFile(configFile);
452#if 0
453 // Corresponding parameters manually set to have an example code
454 // By setting the parameters:
455 cam.initPersProjWithoutDistortion(547, 542, 338, 234);
456
457 vpKltOpencv klt;
458 klt.setMaxFeatures(10000);
459 klt.setWindowSize(5);
460 klt.setQuality(0.01);
461 klt.setMinDistance(5);
462 klt.setHarrisFreeParameter(0.01);
463 klt.setBlockSize(3);
464 klt.setPyramidLevels(3);
465
466 tracker.setCameraParameters(cam);
467 tracker.setKltOpencv(klt);
468 tracker.setKltMaskBorder(5);
469 tracker.setAngleAppear(vpMath::rad(65));
470 tracker.setAngleDisappear(vpMath::rad(75));
471
472 // Specify the clipping to use
473 tracker.setNearClippingDistance(0.01);
474 tracker.setFarClippingDistance(0.90);
476// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
477// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
478// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
479#endif
480 tracker.loadModel(modelFile);
481 tracker.setCameraParameters(cam);
482 tracker.setOgreVisibilityTest(useOgre);
483 tracker.setScanLineVisibilityTest(useScanline);
484 tracker.setCovarianceComputation(computeCovariance);
485 tracker.initFromPose(I, cMo);
486 }
487
488 // Test to set an initial pose
489 if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 50) {
490 cMo.buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946);
491 vpTRACE("Test set pose");
492 tracker.setPose(I, cMo);
493 // if (opt_display) {
494 // // display the 3D model
495 // tracker.display(I, cMo, cam, vpColor::darkRed);
496 // // display the frame
497 // vpDisplay::displayFrame (I, cMo, cam, 0.05);
502 // }
503 }
504
505 // track the object: stop tracking from frame 40 to 50
506 if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
507 reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
508 tracker.track(I);
509 tracker.getPose(cMo);
510 if (opt_display) {
511 // display the 3D model
512 tracker.display(I, cMo, cam, vpColor::darkRed);
513 // display the frame
514 vpDisplay::displayFrame(I, cMo, cam, 0.05);
515 }
516 }
517
518 if (opt_click_allowed) {
519 vpDisplay::displayText(I, 10, 10, "Click to quit", vpColor::red);
520 if (vpDisplay::getClick(I, false)) {
521 quit = true;
522 break;
523 }
524 }
525
526 if (computeCovariance) {
527 std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl;
528 }
529
530 if (opt_display)
532 }
533
534 std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
535
536 if (opt_click_allowed && !quit) {
538 }
539
540 reader.close();
541
542 return EXIT_SUCCESS;
543 } catch (const vpException &e) {
544 std::cout << "Catch an exception: " << e << std::endl;
545 return EXIT_FAILURE;
546 }
547}
548
549#else
550
551int main()
552{
553 std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
554 "this example."
555 << std::endl;
556 return EXIT_SUCCESS;
557}
558
559#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
static const vpColor darkRed
Definition: vpColor.h:218
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
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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))
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)
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
Model based tracker using only KLT.
virtual void setScanLineVisibilityTest(const bool &v)
virtual void track(const vpImage< unsigned char > &I)
virtual void setPose(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cdMo)
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
virtual void setOgreVisibilityTest(const bool &v)
void setCameraParameters(const vpCameraParameters &cam)
void setKltMaskBorder(const unsigned int &e)
virtual void setOgreShowConfigDialog(bool showConfigDialog)
Definition: vpMbTracker.h:643
virtual void getCameraParameters(vpCameraParameters &cam) const
Definition: vpMbTracker.h:248
virtual void setDisplayFeatures(bool displayF)
Definition: vpMbTracker.h:517
virtual void getPose(vpHomogeneousMatrix &cMo) const
Definition: vpMbTracker.h:414
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:480
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:499
virtual void initClick(const vpImage< unsigned char > &I, const std::string &initFile, bool displayHelp=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix())
virtual vpMatrix getCovarianceMatrix() const
Definition: vpMbTracker.h:265
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual void setClipping(const unsigned int &flags)
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())
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:469
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:256
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
#define vpTRACE
Definition: vpDebug.h:416