44#include <visp3/core/vpConfig.h>
46#if (defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)) && \
47 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
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>
63#define GETOPTARGS "x:m:i:n:de:chtfColwvpT:"
67void usage(
const char *name,
const char *badparam)
69#if VISP_HAVE_DATASET_VERSION >= 0x030600
70 std::string ext(
"png");
72 std::string ext(
"pgm");
75Example of tracking based on the 3D model.\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",
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\
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\
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\
104 -e <last frame index> \n\
105 Specify the index of the last frame. Once reached, the tracking is stopped\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\
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\
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\
124 Turn off the display of the the moving edges and Klt points. \n\
127 Turn off the display.\n\
130 Disable the mouse click. Useful to automate the \n\
131 execution of this program without human intervention.\n\
134 Use Ogre3D for visibility tests\n\
137 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
140 Use the scanline for visibility tests.\n\
143 Compute covariance matrix.\n\
146 Compute gradient projection error.\n\
149 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (EdgeKlt)>).\n\
152 Print the help.\n\n", ext.c_str());
155 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
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)
169 lastFrame = atol(optarg_);
175 configFile = optarg_;
184 displayFeatures =
false;
190 click_allowed =
false;
196 trackCylinder =
false;
205 showOgreConfigDialog =
true;
208 computeCovariance =
true;
211 projectionError =
true;
214 trackerType = atoi(optarg_);
217 usage(argv[0], NULL);
222 usage(argv[0], optarg_);
228 if ((c == 1) || (c == -1)) {
230 usage(argv[0], NULL);
231 std::cerr <<
"ERROR: " << std::endl;
232 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
239int main(
int argc,
const char **argv)
242 std::string env_ipath;
243 std::string opt_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;
263#if VISP_HAVE_DATASET_VERSION >= 0x030600
264 std::string ext(
"png");
266 std::string ext(
"pgm");
274 if (!env_ipath.empty())
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)) {
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
297 if (!opt_ipath.empty())
303 std::string configFile;
304 if (!opt_configFile.empty())
305 configFile = opt_configFile;
306 else if (!opt_ipath.empty())
312 if (!opt_modelFile.empty()) {
313 modelFile = opt_modelFile;
316 std::string modelFileCao;
317 std::string modelFileWrl;
319 modelFileCao =
"mbt/cube_and_cylinder.cao";
320 modelFileWrl =
"mbt/cube_and_cylinder.wrl";
323 modelFileCao =
"mbt/cube.cao";
324 modelFileWrl =
"mbt/cube.wrl";
327 if (!opt_ipath.empty()) {
332#ifdef VISP_HAVE_COIN3D
335 std::cerr <<
"Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
345#ifdef VISP_HAVE_COIN3D
348 std::cerr <<
"Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
355 if (!opt_initFile.empty())
356 initFile = opt_initFile;
357 else if (!opt_ipath.empty())
362 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
373 std::cerr <<
"Cannot open sequence: " << ipath << std::endl;
384 mapOfImages[
"Camera1"] = &I1;
385 mapOfImages[
"Camera2"] = &I2;
386 mapOfImages[
"Camera3"] = &I3;
389#if defined(VISP_HAVE_X11)
391#elif defined(VISP_HAVE_GDI)
393#elif defined(HAVE_OPENCV_HIGHGUI)
395#elif defined(VISP_HAVE_D3D9)
397#elif defined(VISP_HAVE_GTK)
404#if defined(VISP_HAVE_DISPLAY)
409 display1.
init(I1, 100, 100,
"Test tracking (Cam1)");
424 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
425 std::map<std::string, vpCameraParameters> mapOfCameraParams;
430 std::map<std::string, std::string> mapOfConfigFiles;
431 mapOfConfigFiles[
"Camera1"] = configFile;
432 mapOfConfigFiles[
"Camera2"] = configFile;
433 mapOfConfigFiles[
"Camera3"] = configFile;
439 mapOfCameraParams[
"Camera1"] = cam;
440 mapOfCameraParams[
"Camera2"] = cam;
441 mapOfCameraParams[
"Camera3"] = cam;
452 std::map<std::string, vpMe> mapOfMe;
453 mapOfMe[
"Camera1"] = me;
454 mapOfMe[
"Camera2"] = me;
455 mapOfMe[
"Camera3"] = me;
457#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
466 std::map<std::string, vpKltOpencv> mapOfKlt;
467 mapOfKlt[
"Camera1"] = klt;
468 mapOfKlt[
"Camera2"] = klt;
469 mapOfKlt[
"Camera3"] = klt;
484 std::map<std::string, unsigned int> mapOfClippingFlags;
486 for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end();
518 if (opt_display && opt_click_allowed) {
535 if (opt_display && opt_click_allowed) {
536 std::map<std::string, std::string> mapOfInitFiles;
537 mapOfInitFiles[
"Camera1"] = initFile;
539 dynamic_cast<vpMbGenericTracker *
>(tracker)->initClick(mapOfImages, mapOfInitFiles,
true);
543 dynamic_cast<vpMbGenericTracker *
>(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
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;
551 dynamic_cast<vpMbGenericTracker *
>(tracker)->initFromPose(mapOfImages, mapOfInitPoses);
564 bool quit =
false, click =
false;
565 while (!reader.
end() && !quit) {
570 mapOfImages[
"Camera1"] = &I1;
571 mapOfImages[
"Camera2"] = &I2;
572 mapOfImages[
"Camera3"] = &I3;
580 std::stringstream ss;
587 std::cout <<
"----------Test reset tracker----------" << std::endl;
600 mapOfCameraParams[
"Camera1"] = cam;
601 mapOfCameraParams[
"Camera2"] = cam;
602 mapOfCameraParams[
"Camera3"] = cam;
613 mapOfMe[
"Camera1"] = me;
614 mapOfMe[
"Camera2"] = me;
615 mapOfMe[
"Camera3"] = me;
617#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
626 mapOfKlt[
"Camera1"] = klt;
627 mapOfKlt[
"Camera2"] = klt;
628 mapOfKlt[
"Camera3"] = klt;
644 for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin();
645 it != mapOfClippingFlags.end(); ++it) {
660 dynamic_cast<vpMbGenericTracker *
>(tracker)->initFromPose(mapOfImages, mapOfCameraPoses);
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;
670 std::cout <<
"Test set pose" << std::endl;
682 std::map<std::string, const vpImage<unsigned char> *> mapOfSubImages;
683 mapOfSubImages[
"Camera1"] = &I1;
684 mapOfSubImages[
"Camera2"] = &I2;
686 dynamic_cast<vpMbGenericTracker *
>(tracker)->display(mapOfSubImages, mapOfCameraPoses, mapOfCameraParams,
690 dynamic_cast<vpMbGenericTracker *
>(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
700 if (opt_click_allowed && opt_display) {
719 if (computeCovariance) {
720 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
723 if (projectionError) {
724 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
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;
740 if (opt_click_allowed && !quit) {
751 std::cout <<
"Catch an exception: " << e << std::endl;
756#elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
759 std::cout <<
"Cannot run this example: visp_mbt, visp_gui modules are required." << std::endl;
765 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
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...
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()
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.
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.
unsigned int getWidth() const
unsigned int getHeight() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
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)
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)
void setMu1(const double &mu_1)
void setSampleStep(const double &s)
void setRange(const unsigned int &r)
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
void setMaskSize(const unsigned int &a)
void setMu2(const double &mu_2)
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
void setMaskNumber(const unsigned int &a)
void setThreshold(const double &t)
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)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
long getFirstFrameIndex()
long getFrameIndex() const