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