48 #include <visp3/core/vpConfig.h>
50 #if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpHomogeneousMatrix.h>
54 #include <visp3/core/vpIoTools.h>
55 #include <visp3/core/vpMath.h>
56 #include <visp3/gui/vpDisplayD3D.h>
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayGTK.h>
59 #include <visp3/gui/vpDisplayOpenCV.h>
60 #include <visp3/gui/vpDisplayX.h>
61 #include <visp3/io/vpImageIo.h>
62 #include <visp3/io/vpParseArgv.h>
63 #include <visp3/io/vpVideoReader.h>
64 #include <visp3/mbt/vpMbGenericTracker.h>
66 #define GETOPTARGS "x:m:i:n:de:chtfColwvpT:"
70 void usage(
const char *name,
const char *badparam)
73 Example of tracking based on the 3D model.\n\
76 %s [-i <test image path>] [-x <config file>]\n\
77 [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
78 [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n\
79 [-T <tracker type>]\n", name);
83 -i <input image path> \n\
84 Set image input path.\n\
85 From this path read images \n\
86 \"mbt/cube/image%%04d.ppm\". These \n\
87 images come from ViSP-images-x.y.z.tar.gz available \n\
88 on the ViSP website.\n\
89 Setting the VISP_INPUT_IMAGE_PATH environment\n\
90 variable produces the same behavior than using\n\
94 Set the config file (the xml file) to use.\n\
95 The config file is used to specify the parameters of the tracker.\n\
98 Specify the name of the file of the model\n\
99 The model can either be a vrml model (.wrl) or a .cao file.\n\
101 -e <last frame index> \n\
102 Specify the index of the last frame. Once reached, the tracking is stopped\n\
105 Do not use the vrml model, use the .cao one. These two models are \n\
106 equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
107 website. However, the .cao model allows to use the 3d model based tracker \n\
111 Track only the cube (not the cylinder). In this case the models files are\n\
112 cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
113 cube_and_cylinder.wrl.\n\
115 -n <initialisation file base name> \n\
116 Base name of the initialisation file. The file will be 'base_name'.init .\n\
117 This base name is also used for the optional picture specifying where to \n\
118 click (a .ppm picture).\n\
121 Turn off the display of the the moving edges and Klt points. \n\
124 Turn off the display.\n\
127 Disable the mouse click. Useful to automate the \n\
128 execution of this program without human intervention.\n\
131 Use Ogre3D for visibility tests\n\
134 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
137 Use the scanline for visibility tests.\n\
140 Compute covariance matrix.\n\
143 Compute gradient projection error.\n\
146 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (EdgeKlt)>).\n\
149 Print the help.\n\n");
152 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
155 bool getOptions(
int argc,
const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
156 std::string &initFile,
long &lastFrame,
bool &displayFeatures,
bool &click_allowed,
bool &display,
157 bool &cao3DModel,
bool &trackCylinder,
bool &useOgre,
bool &showOgreConfigDialog,
bool &useScanline,
158 bool &computeCovariance,
bool &projectionError,
int &trackerType)
166 lastFrame = atol(optarg_);
172 configFile = optarg_;
181 displayFeatures =
false;
187 click_allowed =
false;
193 trackCylinder =
false;
202 showOgreConfigDialog =
true;
205 computeCovariance =
true;
208 projectionError =
true;
211 trackerType = atoi(optarg_);
214 usage(argv[0], NULL);
219 usage(argv[0], optarg_);
225 if ((c == 1) || (c == -1)) {
227 usage(argv[0], NULL);
228 std::cerr <<
"ERROR: " << std::endl;
229 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
236 int main(
int argc,
const char **argv)
239 std::string env_ipath;
240 std::string opt_ipath;
242 std::string opt_configFile;
243 std::string opt_modelFile;
244 std::string modelFile;
245 std::string opt_initFile;
246 std::string initFile;
247 long opt_lastFrame = -1;
248 bool displayFeatures =
true;
249 bool opt_click_allowed =
true;
250 bool opt_display =
true;
251 bool cao3DModel =
false;
252 bool trackCylinder =
true;
253 bool useOgre =
false;
254 bool showOgreConfigDialog =
false;
255 bool useScanline =
false;
256 bool computeCovariance =
false;
257 bool projectionError =
false;
265 if (!env_ipath.empty())
269 if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures,
270 opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog,
271 useScanline, computeCovariance, projectionError, trackerType)) {
276 if (opt_ipath.empty() && env_ipath.empty()) {
277 usage(argv[0], NULL);
278 std::cerr << std::endl <<
"ERROR:" << std::endl;
279 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
280 <<
" environment variable to specify the location of the " << std::endl
281 <<
" image path where test images are located." << std::endl
288 if (!opt_ipath.empty())
293 #if defined(VISP_HAVE_XML2) && USE_XML
294 std::string configFile;
295 if (!opt_configFile.empty())
296 configFile = opt_configFile;
297 else if (!opt_ipath.empty())
303 if (!opt_modelFile.empty()) {
304 modelFile = opt_modelFile;
306 std::string modelFileCao;
307 std::string modelFileWrl;
309 modelFileCao =
"mbt/cube_and_cylinder.cao";
310 modelFileWrl =
"mbt/cube_and_cylinder.wrl";
312 modelFileCao =
"mbt/cube.cao";
313 modelFileWrl =
"mbt/cube.wrl";
316 if (!opt_ipath.empty()) {
320 #ifdef VISP_HAVE_COIN3D
323 std::cerr <<
"Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
331 #ifdef VISP_HAVE_COIN3D
334 std::cerr <<
"Coin is not detected in ViSP. Use the .cao model instead." << std::endl;
341 if (!opt_initFile.empty())
342 initFile = opt_initFile;
343 else if (!opt_ipath.empty())
348 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
358 std::cerr <<
"Cannot open sequence: " << ipath << std::endl;
369 mapOfImages[
"Camera1"] = &I1;
370 mapOfImages[
"Camera2"] = &I2;
371 mapOfImages[
"Camera3"] = &I3;
374 #if defined VISP_HAVE_X11
376 #elif defined VISP_HAVE_GDI
378 #elif defined VISP_HAVE_OPENCV
380 #elif defined VISP_HAVE_D3D9
382 #elif defined VISP_HAVE_GTK
389 #if defined(VISP_HAVE_DISPLAY)
394 display1.
init(I1, 100, 100,
"Test tracking (Cam1)");
409 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
410 std::map<std::string, vpCameraParameters> mapOfCameraParams;
413 #if defined(VISP_HAVE_XML2) && USE_XML
415 std::map<std::string, std::string> mapOfConfigFiles;
416 mapOfConfigFiles[
"Camera1"] = configFile;
417 mapOfConfigFiles[
"Camera2"] = configFile;
418 mapOfConfigFiles[
"Camera3"] = configFile;
419 dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(mapOfConfigFiles);
424 mapOfCameraParams[
"Camera1"] = cam;
425 mapOfCameraParams[
"Camera2"] = cam;
426 mapOfCameraParams[
"Camera3"] = cam;
436 std::map<std::string, vpMe> mapOfMe;
437 mapOfMe[
"Camera1"] = me;
438 mapOfMe[
"Camera2"] = me;
439 mapOfMe[
"Camera3"] = me;
441 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
450 std::map<std::string, vpKltOpencv> mapOfKlt;
451 mapOfKlt[
"Camera1"] = klt;
452 mapOfKlt[
"Camera2"] = klt;
453 mapOfKlt[
"Camera3"] = klt;
455 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(mapOfKlt);
456 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
459 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
460 dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(mapOfMe);
468 std::map<std::string, unsigned int> mapOfClippingFlags;
469 dynamic_cast<vpMbGenericTracker *>(tracker)->getClipping(mapOfClippingFlags);
470 for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end();
475 dynamic_cast<vpMbGenericTracker *>(tracker)->setClipping(mapOfClippingFlags);
499 dynamic_cast<vpMbGenericTracker *>(tracker)->getCameraParameters(mapOfCameraParams);
502 if (opt_display && opt_click_allowed) {
519 if (opt_display && opt_click_allowed) {
520 std::map<std::string, std::string> mapOfInitFiles;
521 mapOfInitFiles[
"Camera1"] = initFile;
523 dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(mapOfImages, mapOfInitFiles,
true);
524 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
527 dynamic_cast<vpMbGenericTracker *>(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
530 vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
531 std::map<std::string, vpHomogeneousMatrix> mapOfInitPoses;
532 mapOfInitPoses[
"Camera1"] = c1Moi;
534 dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(mapOfImages, mapOfInitPoses);
538 dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages);
539 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
547 bool quit =
false, click =
false;
548 while (!reader.
end() && !quit) {
553 mapOfImages[
"Camera1"] = &I1;
554 mapOfImages[
"Camera2"] = &I2;
555 mapOfImages[
"Camera3"] = &I3;
563 std::stringstream ss;
570 std::cout <<
"----------Test reset tracker----------" << std::endl;
578 #if defined(VISP_HAVE_XML2) && USE_XML
579 dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(mapOfConfigFiles);
584 mapOfCameraParams[
"Camera1"] = cam;
585 mapOfCameraParams[
"Camera2"] = cam;
586 mapOfCameraParams[
"Camera3"] = cam;
596 std::map<std::string, vpMe> mapOfMe;
597 mapOfMe[
"Camera1"] = me;
598 mapOfMe[
"Camera2"] = me;
599 mapOfMe[
"Camera3"] = me;
601 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
610 std::map<std::string, vpKltOpencv> mapOfKlt;
611 mapOfKlt[
"Camera1"] = klt;
612 mapOfKlt[
"Camera2"] = klt;
613 mapOfKlt[
"Camera3"] = klt;
615 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(mapOfKlt);
616 dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
619 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
620 dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(mapOfMe);
628 std::map<std::string, unsigned int> mapOfClippingFlags;
629 dynamic_cast<vpMbGenericTracker *>(tracker)->getClipping(mapOfClippingFlags);
630 for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin();
631 it != mapOfClippingFlags.end(); ++it) {
635 dynamic_cast<vpMbGenericTracker *>(tracker)->setClipping(mapOfClippingFlags);
641 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
646 dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(mapOfImages, mapOfCameraPoses);
652 c1Moi.
buildFrom(0.0439540832, 0.0845870108, 0.5477322481, 2.179498458, 0.8611798108, -0.3491961946);
653 std::map<std::string, vpHomogeneousMatrix> mapOfSetPoses;
654 mapOfSetPoses[
"Camera1"] = c1Moi;
656 std::cout <<
"Test set pose" << std::endl;
657 dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(mapOfImages, mapOfSetPoses);
663 dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages);
664 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
668 std::map<std::string, const vpImage<unsigned char> *> mapOfSubImages;
669 mapOfSubImages[
"Camera1"] = &I1;
670 mapOfSubImages[
"Camera2"] = &I2;
672 dynamic_cast<vpMbGenericTracker *>(tracker)->
display(mapOfSubImages, mapOfCameraPoses, mapOfCameraParams,
675 dynamic_cast<vpMbGenericTracker *>(tracker)->
display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
685 if (opt_click_allowed && opt_display) {
704 if (computeCovariance) {
705 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
708 if (projectionError) {
709 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
719 std::cout <<
"Reached last frame: " << reader.
getFrameIndex() << std::endl;
720 std::cout <<
"\nFinal poses, c1Mo:\n"
721 << mapOfCameraPoses[
"Camera1"] <<
"\nc2Mo:\n"
722 << mapOfCameraPoses[
"Camera2"] <<
"\nc3Mo:\n"
723 << mapOfCameraPoses[
"Camera3"] << std::endl;
725 if (opt_click_allowed && !quit) {
733 #if defined(VISP_HAVE_XML2) && USE_XML
739 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
748 std::cout <<
"Catch an exception: " << e << std::endl;
757 std::cout <<
"visp_mbt, visp_gui modules and OpenCV are required to run "