Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
mbtGenericTrackingDepthOnly.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 tracking with vpGenericTracker on Castel.
33  *
34  *****************************************************************************/
35 
42 #include <cstdlib>
43 #include <iostream>
44 #include <visp3/core/vpConfig.h>
45 
46 #if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
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/vpMbGenericTracker.h>
61 
62 #define GETOPTARGS "X:M:i:n:dchfolwvpT:e:u:"
63 
64 #define USE_XML 1
65 #define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images
66 
67 namespace
68 {
69 void usage(const char *name, const char *badparam)
70 {
71  fprintf(stdout, "\n\
72  Example of tracking with vpGenericTracker.\n\
73  \n\
74  SYNOPSIS\n\
75  %s [-i <test image path>] [-X <config file depth>]\n\
76  [-M <model name depth>] [-n <initialisation file base name>]\n\
77  [-f] [-c] [-d] [-h] [-o] [-w] [-l] [-v] [-p]\n\
78  [-T <tracker type>] [-e <last frame index>]\n\
79  [-u <disable face>]\n", name);
80 
81  fprintf(stdout, "\n\
82  OPTIONS: \n\
83  -i <input image path> \n\
84  Set image input path.\n\
85  These images come from ViSP-images-x.y.z.tar.gz available \n\
86  on the ViSP website.\n\
87  Setting the VISP_INPUT_IMAGE_PATH environment\n\
88  variable produces the same behavior than using\n\
89  this option.\n\
90  \n\
91  -X <config file> \n\
92  Set the config file (the xml file) to use for the depth sensor.\n\
93  The config file is used to specify the parameters of the tracker.\n\
94  \n\
95  -M <model name> \n\
96  Specify the name of the file of the model for the depth sensor.\n\
97  The model can either be a vrml model (.wrl) or a .cao file.\n\
98  \n\
99  -n <initialisation file base name> \n\
100  Base name of the initialisation file. The file will be 'base_name'.init .\n\
101  This base name is also used for the optional picture specifying where to \n\
102  click (a .ppm picture).\n\
103  \n\
104  -f \n\
105  Turn off the display of the visual features. \n\
106  \n\
107  -d \n\
108  Turn off the display.\n\
109  \n\
110  -c\n\
111  Disable the mouse click. Useful to automate the \n\
112  execution of this program without human intervention.\n\
113  \n\
114  -o\n\
115  Use Ogre3D for visibility tests\n\
116  \n\
117  -w\n\
118  When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
119  \n\
120  -l\n\
121  Use the scanline for visibility tests.\n\
122  \n\
123  -v\n\
124  Compute covariance matrix.\n\
125  \n\
126  -p\n\
127  Compute gradient projection error.\n\
128  \n\
129  -T <tracker type>\n\
130  Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
131  \n\
132  -e <last frame index>\n\
133  Specify the index of the last frame. Once reached, the tracking is stopped.\n\
134  \n\
135  -u <disable>\n\
136  Disable castle element (1=floor, 2=front_door, 4=slope, 8=tower_front, 16=tower_left, 32=tower_right, 64=tower_back).\n\
137  \n\
138  -h \n\
139  Print the help.\n\n");
140 
141  if (badparam)
142  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
143 }
144 
145 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile_depth,
146  std::string &modelFile_depth, std::string &initFile, bool &displayFeatures, bool &click_allowed,
147  bool &display, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance,
148  bool &projectionError, int &tracker_type_depth, int &lastFrame, int &disable_castle_faces)
149 {
150  const char *optarg_;
151  int c;
152  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
153 
154  switch (c) {
155  case 'i':
156  ipath = optarg_;
157  break;
158  case 'X':
159  configFile_depth = optarg_;
160  break;
161  case 'M':
162  modelFile_depth = optarg_;
163  break;
164  case 'n':
165  initFile = optarg_;
166  break;
167  case 'f':
168  displayFeatures = false;
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 'p':
189  projectionError = true;
190  break;
191  case 'T':
192  tracker_type_depth = atoi(optarg_);
193  break;
194  case 'e':
195  lastFrame = atoi(optarg_);
196  break;
197  case 'u':
198  disable_castle_faces = atoi(optarg_);
199  break;
200 
201  case 'h':
202  usage(argv[0], NULL);
203  return false;
204  break;
205  default:
206  usage(argv[0], optarg_);
207  return false;
208  break;
209  }
210  }
211 
212  if ((c == 1) || (c == -1)) {
213  // standalone param or error
214  usage(argv[0], NULL);
215  std::cerr << "ERROR: " << std::endl;
216  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
217  return false;
218  }
219 
220  return true;
221 }
222 
223 struct rs_intrinsics {
224  float ppx;
226  float ppy;
228  float fx;
230  float fy;
232  float coeffs[5];
233 };
234 
235 void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth)
236 {
237  float x = (pixel[0] - intrin.ppx) / intrin.fx;
238  float y = (pixel[1] - intrin.ppy) / intrin.fy;
239 
240  float r2 = x * x + y * y;
241  float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
242  float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
243  float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
244 
245  x = ux;
246  y = uy;
247 
248  point[0] = depth * x;
249  point[1] = depth * y;
250  point[2] = depth;
251 }
252 
253 bool read_data(const unsigned int cpt, const std::string &input_directory, vpImage<unsigned char> &I, vpImage<uint16_t> &I_depth_raw,
254  std::vector<vpColVector> &pointcloud, unsigned int &pointcloud_width, unsigned int &pointcloud_height)
255 {
256  char buffer[256];
257 
258  // Read image
259  std::stringstream ss;
260  ss << input_directory << "/image_%04d.pgm";
261  sprintf(buffer, ss.str().c_str(), cpt);
262  std::string filename_image = buffer;
263 
264  if (!vpIoTools::checkFilename(filename_image)) {
265  std::cerr << "Cannot read: " << filename_image << std::endl;
266  return false;
267  }
268  vpImageIo::read(I, filename_image);
269 
270  // Read raw depth
271  ss.str("");
272  ss << input_directory << "/depth_image_%04d.bin";
273  sprintf(buffer, ss.str().c_str(), cpt);
274  std::string filename_depth = buffer;
275 
276  std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
277  if (!file_depth.is_open()) {
278  return false;
279  }
280 
281  unsigned int height = 0, width = 0;
282  vpIoTools::readBinaryValueLE(file_depth, height);
283  vpIoTools::readBinaryValueLE(file_depth, width);
284 
285  I_depth_raw.resize(height, width);
286 
287  uint16_t depth_value = 0;
288  for (unsigned int i = 0; i < height; i++) {
289  for (unsigned int j = 0; j < width; j++) {
290  vpIoTools::readBinaryValueLE(file_depth, depth_value);
291  I_depth_raw[i][j] = depth_value;
292  }
293  }
294 
295  // Transform pointcloud
296  pointcloud_width = width;
297  pointcloud_height = height;
298  pointcloud.resize((size_t)width * height);
299 
300  // Only for Creative SR300
301  const float depth_scale = 0.000124986647f;
302  rs_intrinsics depth_intrinsic;
303  depth_intrinsic.ppx = 311.484558f;
304  depth_intrinsic.ppy = 246.283234f;
305  depth_intrinsic.fx = 476.053619f;
306  depth_intrinsic.fy = 476.053497f;
307  depth_intrinsic.coeffs[0] = 0.165056542f;
308  depth_intrinsic.coeffs[1] = -0.0508309528f;
309  depth_intrinsic.coeffs[2] = 0.00435937941f;
310  depth_intrinsic.coeffs[3] = 0.00541406544f;
311  depth_intrinsic.coeffs[4] = 0.250085592f;
312 
313  for (unsigned int i = 0; i < height; i++) {
314  for (unsigned int j = 0; j < width; j++) {
315  float scaled_depth = I_depth_raw[i][j] * depth_scale;
316  float point[3];
317  float pixel[2] = {(float)j, (float)i};
318  rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
319 
320  vpColVector data_3D(3);
321  data_3D[0] = point[0];
322  data_3D[1] = point[1];
323  data_3D[2] = point[2];
324 
325  pointcloud[(size_t)(i * width + j)] = data_3D;
326  }
327  }
328 
329  return true;
330 }
331 
332 void loadConfiguration(vpMbTracker *const tracker, const std::string &
333 #if defined(VISP_HAVE_XML2) && USE_XML
334  configFile_depth
335 #endif
336 )
337 {
338 #if defined(VISP_HAVE_XML2) && USE_XML
339  // From the xml file
340  dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(configFile_depth);
341 #else
342  // Depth
343  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalFeatureEstimationMethod(
345  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
346  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
347  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
348  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalSamplingStep(2, 2);
349 
350  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthDenseSamplingStep(4, 4);
351 
352  vpCameraParameters cam;
353  cam.initPersProjWithoutDistortion(476.0536193848, 476.0534973145, 311.4845581055, 246.2832336426);
354 
355  dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam);
356 
357  tracker->setAngleAppear(vpMath::rad(70));
358  tracker->setAngleDisappear(vpMath::rad(80));
359 
360  // Specify the clipping to
361  tracker->setNearClippingDistance(0.01);
362  tracker->setFarClippingDistance(2.0);
363  tracker->setClipping(tracker->getClipping() | vpMbtPolygon::FOV_CLIPPING);
364 // tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING
365 // | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
366 // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
367 #endif
368 }
369 
370 std::vector<std::string> getCastleElementNames(const int element)
371 {
372  std::vector<std::string> element_names;
373 
374  if (element & 0x1)
375  element_names.push_back("floor");
376  if (element & 0x2)
377  element_names.push_back("front_door");
378  if (element & 0x4)
379  element_names.push_back("slope");
380  if (element & 0x8)
381  element_names.push_back("tower_front");
382  if (element & 0x10)
383  element_names.push_back("tower_left");
384  if (element & 0x20)
385  element_names.push_back("tower_right");
386  if (element & 0x40)
387  element_names.push_back("tower_back");
388 
389  return element_names;
390 }
391 }
392 
393 int main(int argc, const char **argv)
394 {
395  try {
396  std::string env_ipath;
397  std::string opt_ipath;
398  std::string ipath;
399  std::string opt_configFile_depth;
400  std::string opt_modelFile_depth;
401  std::string opt_initFile;
402  std::string initFile;
403  bool displayFeatures = true;
404  bool opt_click_allowed = true;
405  bool opt_display = true;
406  bool useOgre = false;
407  bool showOgreConfigDialog = false;
408  bool useScanline = false;
409  bool computeCovariance = false;
410  bool projectionError = false;
411  int trackerType_depth = vpMbGenericTracker::DEPTH_DENSE_TRACKER;
412 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
413  // To avoid Debian test timeout
414  int opt_lastFrame = 5;
415 #else
416  int opt_lastFrame = -1;
417 #endif
418  int disable_castle_faces = 0;
419 
420  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
421  // environment variable value
422  env_ipath = vpIoTools::getViSPImagesDataPath();
423 
424  // Set the default input path
425  if (!env_ipath.empty())
426  ipath = env_ipath;
427 
428  // Read the command line options
429  if (!getOptions(argc, argv, opt_ipath, opt_configFile_depth, opt_modelFile_depth, opt_initFile, displayFeatures,
430  opt_click_allowed, opt_display, useOgre, showOgreConfigDialog, useScanline, computeCovariance,
431  projectionError, trackerType_depth, opt_lastFrame, disable_castle_faces)) {
432  return EXIT_FAILURE;
433  }
434 
435  // Test if an input path is set
436  if (opt_ipath.empty() && env_ipath.empty()) {
437  usage(argv[0], NULL);
438  std::cerr << std::endl << "ERROR:" << std::endl;
439  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
440  << " environment variable to specify the location of the " << std::endl
441  << " image path where test images are located." << std::endl
442  << std::endl;
443 
444  return EXIT_FAILURE;
445  }
446 
447  // Get the option values
448  ipath = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/castel");
449 
450  std::string dir_path = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth");
451  if (!vpIoTools::checkDirectory(dir_path)) {
452  std::cerr << "ViSP-images does not contain the folder: " << dir_path << "!" << std::endl;
453  return EXIT_SUCCESS;
454  }
455 
456  std::string configFile_depth;
457  if (!opt_configFile_depth.empty())
458  configFile_depth = opt_configFile_depth;
459  else
460  configFile_depth =
461  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_depth.xml");
462 
463  std::string modelFile_depth;
464  if (!opt_modelFile_depth.empty())
465  modelFile_depth = opt_modelFile_depth;
466  else
467  modelFile_depth =
468  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao");
469 
470  std::string vrml_ext = ".wrl";
471  bool use_vrml =
472  (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
473 
474  if (use_vrml) {
475 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
476  std::cout << "use_vrml: " << use_vrml << std::endl;
477 #else
478  std::cerr << "Error: vrml model file is only supported if ViSP is "
479  "build with Coin3D 3rd party"
480  << std::endl;
481  return EXIT_FAILURE;
482 #endif
483  }
484 
485  if (!opt_initFile.empty())
486  initFile = opt_initFile;
487  else
488  initFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.init");
489 
490  vpImage<unsigned char> I, I_depth;
491  vpImage<uint16_t> I_depth_raw;
492  std::vector<vpColVector> pointcloud;
493  unsigned int pointcloud_width, pointcloud_height;
494  if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) {
495  std::cerr << "Cannot open sequence: " << ipath << std::endl;
496  return EXIT_FAILURE;
497  }
498 
499  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
500 
501 // initialise a display
502 #if defined VISP_HAVE_X11
503  vpDisplayX display, display2;
504 #elif defined VISP_HAVE_GDI
505  vpDisplayGDI display, display2;
506 #elif defined VISP_HAVE_OPENCV
507  vpDisplayOpenCV display, display2;
508 #elif defined VISP_HAVE_D3D9
509  vpDisplayD3D display, display2;
510 #elif defined VISP_HAVE_GTK
511  vpDisplayGTK display, display2;
512 #else
513  opt_display = false;
514 #endif
515  if (opt_display) {
516 #if defined(VISP_HAVE_DISPLAY)
518  display.init(I_depth, 100, 100, "Depth");
520  display2.init(I, I_depth.getWidth()+100, 100, "Image");
521 #endif
522  vpDisplay::display(I_depth);
524  vpDisplay::flush(I_depth);
525  vpDisplay::flush(I);
526  }
527 
528  // Object pointer to check that inheritance is ok
529  vpMbTracker *tracker = new vpMbGenericTracker(1, trackerType_depth);
531  vpCameraParameters cam;
532 
533  loadConfiguration(tracker, configFile_depth);
534 
535  // Display the moving edges, and the Klt points
536  tracker->setDisplayFeatures(displayFeatures);
537 
538  // Tells if the tracker has to use Ogre3D for visibility tests
539  tracker->setOgreVisibilityTest(useOgre);
540  if (useOgre)
541  tracker->setOgreShowConfigDialog(showOgreConfigDialog);
542 
543  // Tells if the tracker has to use the scanline visibility tests
544  tracker->setScanLineVisibilityTest(useScanline);
545 
546  // Tells if the tracker has to compute the covariance matrix
547  tracker->setCovarianceComputation(computeCovariance);
548 
549  // Tells if the tracker has to compute the projection error
550  tracker->setProjectionErrorComputation(projectionError);
551 
552  // For generic projection error computation
553  tracker->setProjectionErrorDisplay(true);
556 
557  // Retrieve the camera parameters from the tracker
558  dynamic_cast<vpMbGenericTracker *>(tracker)->getCameraParameters(cam);
559 
560  vpCameraParameters cam_color;
561  cam_color.initPersProjWithoutDistortion(615.1674804688, 615.1675415039, 312.1889953613, 243.4373779297);
562  vpHomogeneousMatrix depth_M_color;
563  std::string depth_M_color_filename =
564  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/depth_M_color.txt");
565  {
566  std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
567  depth_M_color.load(depth_M_color_file);
568  }
569 
570  // Loop to position the object
571  if (opt_display && opt_click_allowed) {
572  while (!vpDisplay::getClick(I_depth, false)) {
573  vpDisplay::display(I_depth);
574  vpDisplay::displayText(I_depth, 15, 10, "click after positioning the object", vpColor::red);
575  vpDisplay::flush(I_depth);
576  }
577  }
578 
579  // Load the 3D model (either a vrml file or a .cao file)
580  dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile_depth);
581 
582  if (opt_display && opt_click_allowed) {
583  // Initialise the tracker by clicking on the image
584  dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(I_depth, initFile, true);
585  dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(cMo);
586  // display the 3D model at the given pose
587  dynamic_cast<vpMbGenericTracker *>(tracker)->display(I_depth, cMo, cam, vpColor::red);
588  } else {
589  vpHomogeneousMatrix cMoi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
590  dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(I_depth, cMoi);
591  }
592 
593  // track the model
594  {
595  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
596  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
597  mapOfPointclouds["Camera"] = &pointcloud;
598  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
599  mapOfWidths["Camera"] = pointcloud_width;
600  mapOfHeights["Camera"] = pointcloud_height;
601 
602  dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
603  }
604  dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(cMo);
605 
606  if (opt_display) {
607  vpDisplay::flush(I_depth);
608  }
609 
610  bool quit = false, click = false;
611  unsigned int frame_index = 0;
612  std::vector<double> time_vec;
613  while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
614  (opt_lastFrame > 0 ? (int)frame_index <= opt_lastFrame : true)) {
615  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
616 
617  if (opt_display) {
618  vpDisplay::display(I_depth);
620 
621  std::stringstream ss;
622  ss << "Num frame: " << frame_index;
623  vpDisplay::displayText(I_depth, 40, 20, ss.str(), vpColor::red);
624  }
625 
626  // Test reset the tracker
627  if (frame_index == 10) {
628  std::cout << "----------Test reset tracker----------" << std::endl;
629  if (opt_display) {
630  vpDisplay::display(I_depth);
631  }
632 
633  tracker->resetTracker();
634 
635  loadConfiguration(tracker, configFile_depth);
636  dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile_depth);
637  dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam);
638  tracker->setOgreVisibilityTest(useOgre);
639  tracker->setScanLineVisibilityTest(useScanline);
640  tracker->setCovarianceComputation(computeCovariance);
641  tracker->setProjectionErrorComputation(projectionError);
642  dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(I_depth, cMo);
643  }
644 
645  // Test set an initial pose
646 #if USE_SMALL_DATASET
647  if (frame_index == 20) {
648  cMo.buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
649 #else
650  if (frame_index == 50) {
651  cMo.buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
652 #endif
653  std::cout << "Test set pose" << std::endl;
654  dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(I_depth, cMo);
655  }
656 
657 #if USE_SMALL_DATASET
658  // track the object: stop tracking from frame 15 to 20
659  if (frame_index < 15 || frame_index >= 20) {
660 #else
661  // track the object: stop tracking from frame 30 to 50
662  if (frame_index < 30 || frame_index >= 50) {
663 #endif
664  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
665  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
666  mapOfPointclouds["Camera"] = &pointcloud;
667  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
668  mapOfWidths["Camera"] = pointcloud_width;
669  mapOfHeights["Camera"] = pointcloud_height;
670 
671  if (disable_castle_faces) {
672  std::vector<std::string> element_names = getCastleElementNames(disable_castle_faces);
673  std::cout << "Disable: ";
674  for (size_t idx = 0; idx < element_names.size(); idx++) {
675  std::cout << element_names[idx];
676  if (idx + 1 < element_names.size())
677  std::cout << ", ";
678 
679  if (trackerType_depth & vpMbGenericTracker::DEPTH_DENSE_TRACKER)
680  dynamic_cast<vpMbGenericTracker *>(tracker)->setUseDepthDenseTracking(element_names[idx], false);
681  if (trackerType_depth & vpMbGenericTracker::DEPTH_NORMAL_TRACKER)
682  dynamic_cast<vpMbGenericTracker *>(tracker)->setUseDepthNormalTracking(element_names[idx], false);
683  }
684  std::cout << std::endl;
685  }
686 
687  double t = vpTime::measureTimeMs();
688  dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
689  t = vpTime::measureTimeMs() - t;
690  time_vec.push_back(t);
691 
692  dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(cMo);
693 
694  if (opt_display) {
695  // display the 3D model
696  dynamic_cast<vpMbGenericTracker *>(tracker)->display(I_depth, cMo, cam, vpColor::darkRed);
697  // display the frame
698  vpDisplay::displayFrame(I_depth, cMo, cam, 0.05);
699  // computation time
700  std::stringstream ss;
701  ss << "Computation time: " << t << " ms";
702  vpDisplay::displayText(I_depth, 60, 20, ss.str(), vpColor::red);
703  // nb features
704  ss.str("");
705  ss << "nb features: " << tracker->getError().getRows();
706  vpDisplay::displayText(I_depth, 80, 20, ss.str(), vpColor::red);
707 
708  // generic projection error computed on image from the RGB camera
709  double projection_error = tracker->computeCurrentProjectionError(I, depth_M_color.inverse()*cMo, cam_color);
710  ss.str("");
711  ss << "Projection error: " << projection_error;
712  vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
713  }
714  }
715 
716  if (opt_click_allowed && opt_display) {
717  vpDisplay::displayText(I_depth, 10, 10, "Click to quit", vpColor::red);
719  if (vpDisplay::getClick(I_depth, button, click)) {
720  switch (button) {
722  quit = !click;
723  break;
724 
726  click = !click;
727  break;
728 
729  default:
730  break;
731  }
732  }
733  }
734 
735  if (computeCovariance) {
736  std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
737  }
738 
739  if (projectionError) {
740  std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl;
741  }
742 
743  if (opt_display) {
744  vpDisplay::flush(I_depth);
745  vpDisplay::flush(I);
746  }
747 
748  frame_index++;
749  }
750 
751  std::cout << "\nFinal poses, cMo:\n" << cMo << std::endl;
752  std::cout << "\nComputation time, Mean: " << vpMath::getMean(time_vec)
753  << " ms ; Median: " << vpMath::getMedian(time_vec) << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms"
754  << std::endl;
755 
756  if (opt_click_allowed && !quit) {
757  vpDisplay::getClick(I_depth);
758  }
759 
760  delete tracker;
761  tracker = NULL;
762 
763 #if defined(VISP_HAVE_XML2) && USE_XML
764  // Cleanup memory allocated by xml library used to parse the xml config
765  // file in vpMbGenericTracker::loadConfigFile()
767 #endif
768 
769 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
770  // Cleanup memory allocated by Coin library used to load a vrml model in
771  // vpMbGenericTracker::loadModel() We clean only if Coin was used.
772  if (use_vrml)
773  SoDB::finish();
774 #endif
775 
776  return EXIT_SUCCESS;
777  } catch (const vpException &e) {
778  std::cout << "Catch an exception: " << e << std::endl;
779  return EXIT_FAILURE;
780  }
781 }
782 
783 #else
784 int main()
785 {
786  std::cerr << "visp_mbt, visp_gui modules and OpenCV are required to run "
787  "this example."
788  << std::endl;
789  return EXIT_SUCCESS;
790 }
791 #endif
virtual void setDisplayFeatures(const bool displayF)
Definition: vpMbTracker.h:503
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:485
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
Definition: vpMbTracker.h:629
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static double getStdev(const std::vector< double > &v, const bool useBesselCorrection=false)
Definition: vpMath.cpp:252
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:467
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1316
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:256
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:466
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:1864
virtual void setDownScalingFactor(unsigned int scale)
Definition: vpDisplay.cpp:232
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
static const vpColor darkRed
Definition: vpColor.h:181
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
error that can be emited by ViSP classes.
Definition: vpException.h:71
Real-time 6D object pose tracking using its CAD model.
virtual void resetTracker()=0
static void flush(const vpImage< unsigned char > &I)
void load(std::ifstream &f)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:88
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:180
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:675
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:202
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:107
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1541
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
virtual double getProjectionError() const
Definition: vpMbTracker.h:310
Generic class defining intrinsic camera parameters.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:110
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:866
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:455
virtual void setProjectionErrorDisplay(const bool display)
Definition: vpMbTracker.h:575
unsigned int getRows() const
Definition: vpArray2D.h:156
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:102
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
static void cleanup()
Definition: vpXmlParser.h:310
virtual void setOgreVisibilityTest(const bool &v)
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:207
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
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))
virtual void setProjectionErrorDisplayArrowLength(const unsigned int length)
Definition: vpMbTracker.h:580
vpHomogeneousMatrix inverse() const
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:587
virtual void setClipping(const unsigned int &flags)
virtual void setProjectionErrorDisplayArrowThickness(const unsigned int thickness)
Definition: vpMbTracker.h:585
virtual void setFarClippingDistance(const double &dist)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:570
virtual vpColVector getError() const =0
virtual void setNearClippingDistance(const double &dist)
virtual vpMatrix getCovarianceMatrix() const
Definition: vpMbTracker.h:265