Visual Servoing Platform  version 3.1.0
mbtGenericTrackingDepth.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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:X:m:M:i:n:dchfolwvpt:T:e:"
63 
64 #define USE_XML 1
65 #define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images
66 
67 // Detect endianness of the host machine
68 // Reference: http://www.boost.org/doc/libs/1_36_0/boost/detail/endian.hpp
69 #if defined(__GLIBC__)
70 #include <endian.h>
71 #if (__BYTE_ORDER == __LITTLE_ENDIAN)
72 #define VISP_LITTLE_ENDIAN
73 #elif (__BYTE_ORDER == __BIG_ENDIAN)
74 #define VISP_BIG_ENDIAN
75 #elif (__BYTE_ORDER == __PDP_ENDIAN)
76 // Currently not supported when reading / writing binary file
77 #define VISP_PDP_ENDIAN
78 #else
79 #error Unknown machine endianness detected.
80 #endif
81 #elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN) || defined(__BIG_ENDIAN__) && !defined(__LITTLE_ENDIAN__)
82 #define VISP_BIG_ENDIAN
83 #elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN) || defined(__LITTLE_ENDIAN__) && !defined(__BIG_ENDIAN__)
84 #define VISP_LITTLE_ENDIAN
85 #elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || defined(__powerpc__) || defined(__ppc__) || \
86  defined(__hpux) || defined(_MIPSEB) || defined(_POWER) || defined(__s390__)
87 
88 #define VISP_BIG_ENDIAN
89 #elif defined(__i386__) || defined(__alpha__) || defined(__ia64) || defined(__ia64__) || defined(_M_IX86) || \
90  defined(_M_IA64) || defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) || defined(_M_AMD64) || \
91  defined(__x86_64) || defined(__x86_64__) || defined(_M_X64)
92 
93 #define VISP_LITTLE_ENDIAN
94 #else
95 #error Cannot detect host machine endianness.
96 #endif
97 
98 namespace
99 {
100 #ifdef VISP_BIG_ENDIAN
101 // Swap 16 bits by shifting to the right the first byte and by shifting to the
102 // left the second byte
103 uint16_t swap16bits(const uint16_t val) { return (((val >> 8) & 0x00FF) | ((val << 8) & 0xFF00)); }
104 
105 // Swap 32 bits by shifting to the right the first 2 bytes and by shifting to
106 // the left the last 2 bytes
107 uint32_t swap32bits(const uint32_t val)
108 {
109  return (((val >> 24) & 0x000000FF) | ((val >> 8) & 0x0000FF00) | ((val << 8) & 0x00FF0000) |
110  ((val << 24) & 0xFF000000));
111 }
112 #endif
113 
114 // Read an int stored in little endian
115 void readBinaryUIntLE(std::ifstream &file, unsigned int &uint_value)
116 {
117  // Read
118  file.read((char *)(&uint_value), sizeof(uint_value));
119 
120 #ifdef VISP_BIG_ENDIAN
121  // Swap bytes order from little endian to big endian
122  if (sizeof(uint_value) == 4) {
123  uint_value = (unsigned int)swap32bits((uint32_t)uint_value);
124  } else {
125  uint_value = swap16bits((uint16_t)uint_value);
126  }
127 #endif
128 }
129 
130 // Read an int stored in little endian
131 void readBinaryUIntLE(std::ifstream &file, uint16_t &uint_value)
132 {
133  // Read
134  file.read((char *)(&uint_value), sizeof(uint_value));
135 
136 #ifdef VISP_BIG_ENDIAN
137  // Swap bytes order from little endian to big endian
138  uint_value = swap16bits((uint16_t)uint_value);
139 #endif
140 }
141 
142 void usage(const char *name, const char *badparam)
143 {
144  fprintf(stdout, "\n\
145  Example of tracking with vpGenericTracker.\n\
146  \n\
147  SYNOPSIS\n\
148  %s [-i <test image path>] [-x <config file>] [-X <config file depth>]\n\
149  [-m <model name>] [-M <model name depth>] [-n <initialisation file base name>]\n\
150  [-f] [-c] [-d] [-h] [-o] [-w] [-l] [-v] [-p]\n\
151  [-t <tracker type>] [-T <tracker type>] [-e <last frame index>]\n", name);
152 
153  fprintf(stdout, "\n\
154  OPTIONS: \n\
155  -i <input image path> \n\
156  Set image input path.\n\
157  These images come from ViSP-images-x.y.z.tar.gz available \n\
158  on the ViSP website.\n\
159  Setting the VISP_INPUT_IMAGE_PATH environment\n\
160  variable produces the same behavior than using\n\
161  this option.\n\
162  \n\
163  -x <config file> \n\
164  Set the config file (the xml file) to use.\n\
165  The config file is used to specify the parameters of the tracker.\n\
166  \n\
167  -X <config file> \n\
168  Set the config file (the xml file) to use for the depth sensor.\n\
169  The config file is used to specify the parameters of the tracker.\n\
170  \n\
171  -m <model name> \n\
172  Specify the name of the file of the model.\n\
173  The model can either be a vrml model (.wrl) or a .cao file.\n\
174  \n\
175  -M <model name> \n\
176  Specify the name of the file of the model for the depth sensor.\n\
177  The model can either be a vrml model (.wrl) or a .cao file.\n\
178  \n\
179  -n <initialisation file base name> \n\
180  Base name of the initialisation file. The file will be 'base_name'.init .\n\
181  This base name is also used for the optional picture specifying where to \n\
182  click (a .ppm picture).\n\
183  \n\
184  -f \n\
185  Turn off the display of the the moving edges and Klt points. \n\
186  \n\
187  -d \n\
188  Turn off the display.\n\
189  \n\
190  -c\n\
191  Disable the mouse click. Useful to automate the \n\
192  execution of this program without human intervention.\n\
193  \n\
194  -o\n\
195  Use Ogre3D for visibility tests\n\
196  \n\
197  -w\n\
198  When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
199  \n\
200  -l\n\
201  Use the scanline for visibility tests.\n\
202  \n\
203  -v\n\
204  Compute covariance matrix.\n\
205  \n\
206  -p\n\
207  Compute gradient projection error.\n\
208  \n\
209  -t <tracker type>\n\
210  Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
211  \n\
212  -T <tracker type>\n\
213  Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
214  \n\
215  -e <last frame index>\n\
216  Specify the index of the last frame. Once reached, the tracking is stopped.\n\
217  \n\
218  -h \n\
219  Print the help.\n\n");
220 
221  if (badparam)
222  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
223 }
224 
225 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &configFile_depth,
226  std::string &modelFile, std::string &modelFile_depth, std::string &initFile, bool &displayFeatures,
227  bool &click_allowed, bool &display, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline,
228  bool &computeCovariance, bool &projectionError, int &trackerType, int &tracker_type_depth,
229  int &lastFrame)
230 {
231  const char *optarg_;
232  int c;
233  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
234 
235  switch (c) {
236  case 'i':
237  ipath = optarg_;
238  break;
239  case 'x':
240  configFile = optarg_;
241  break;
242  case 'X':
243  configFile_depth = optarg_;
244  break;
245  case 'm':
246  modelFile = optarg_;
247  break;
248  case 'M':
249  modelFile_depth = optarg_;
250  break;
251  case 'n':
252  initFile = optarg_;
253  break;
254  case 'f':
255  displayFeatures = false;
256  break;
257  case 'c':
258  click_allowed = false;
259  break;
260  case 'd':
261  display = false;
262  break;
263  case 'o':
264  useOgre = true;
265  break;
266  case 'l':
267  useScanline = true;
268  break;
269  case 'w':
270  showOgreConfigDialog = true;
271  break;
272  case 'v':
273  computeCovariance = true;
274  break;
275  case 'p':
276  projectionError = true;
277  break;
278  case 't':
279  trackerType = atoi(optarg_);
280  break;
281  case 'T':
282  tracker_type_depth = atoi(optarg_);
283  break;
284  case 'e':
285  lastFrame = atoi(optarg_);
286  break;
287  case 'h':
288  usage(argv[0], NULL);
289  return false;
290  break;
291 
292  default:
293  usage(argv[0], optarg_);
294  return false;
295  break;
296  }
297  }
298 
299  if ((c == 1) || (c == -1)) {
300  // standalone param or error
301  usage(argv[0], NULL);
302  std::cerr << "ERROR: " << std::endl;
303  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
304  return false;
305  }
306 
307  return true;
308 }
309 
310 struct rs_intrinsics {
311  float ppx;
313  float ppy;
315  float fx;
317  float fy;
319  float coeffs[5];
320 };
321 
322 void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth)
323 {
324  float x = (pixel[0] - intrin.ppx) / intrin.fx;
325  float y = (pixel[1] - intrin.ppy) / intrin.fy;
326 
327  float r2 = x * x + y * y;
328  float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
329  float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
330  float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
331 
332  x = ux;
333  y = uy;
334 
335  point[0] = depth * x;
336  point[1] = depth * y;
337  point[2] = depth;
338 }
339 
340 bool read_data(const unsigned int cpt, const std::string &input_directory, vpImage<unsigned char> &I,
341  vpImage<uint16_t> &I_depth_raw, std::vector<vpColVector> &pointcloud, unsigned int &pointcloud_width,
342  unsigned int &pointcloud_height)
343 {
344  char buffer[256];
345 
346  // Read image
347  std::stringstream ss;
348  ss << input_directory << "/image_%04d.pgm";
349  sprintf(buffer, ss.str().c_str(), cpt);
350  std::string filename_image = buffer;
351 
352  if (!vpIoTools::checkFilename(filename_image)) {
353  std::cerr << "Cannot read: " << filename_image << std::endl;
354  return false;
355  }
356  vpImageIo::read(I, filename_image);
357 
358  // Read raw depth
359  ss.str("");
360  ss << input_directory << "/depth_image_%04d.bin";
361  sprintf(buffer, ss.str().c_str(), cpt);
362  std::string filename_depth = buffer;
363 
364  std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
365  if (!file_depth.is_open()) {
366  return false;
367  }
368 
369  unsigned int height = 0, width = 0;
370  readBinaryUIntLE(file_depth, height);
371  readBinaryUIntLE(file_depth, width);
372 
373  I_depth_raw.resize(height, width);
374 
375  uint16_t depth_value = 0;
376  for (unsigned int i = 0; i < height; i++) {
377  for (unsigned int j = 0; j < width; j++) {
378  readBinaryUIntLE(file_depth, depth_value);
379  I_depth_raw[i][j] = depth_value;
380  }
381  }
382 
383  // Transform pointcloud
384  pointcloud_width = width;
385  pointcloud_height = height;
386  pointcloud.resize((size_t)width * height);
387 
388  // Only for Creative SR300
389  const float depth_scale = 0.000124986647f;
390  rs_intrinsics depth_intrinsic;
391  depth_intrinsic.ppx = 311.484558f;
392  depth_intrinsic.ppy = 246.283234f;
393  depth_intrinsic.fx = 476.053619f;
394  depth_intrinsic.fy = 476.053497f;
395  depth_intrinsic.coeffs[0] = 0.165056542f;
396  depth_intrinsic.coeffs[1] = -0.0508309528f;
397  depth_intrinsic.coeffs[2] = 0.00435937941f;
398  depth_intrinsic.coeffs[3] = 0.00541406544f;
399  depth_intrinsic.coeffs[4] = 0.250085592f;
400 
401  for (unsigned int i = 0; i < height; i++) {
402  for (unsigned int j = 0; j < width; j++) {
403  float scaled_depth = I_depth_raw[i][j] * depth_scale;
404  float point[3];
405  float pixel[2] = {(float)j, (float)i};
406  rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
407 
408  vpColVector data_3D(3);
409  data_3D[0] = point[0];
410  data_3D[1] = point[1];
411  data_3D[2] = point[2];
412 
413  pointcloud[(size_t)(i * width + j)] = data_3D;
414  }
415  }
416 
417  return true;
418 }
419 
420 void loadConfiguration(vpMbTracker *const tracker,
421  const std::string &
422 #if defined(VISP_HAVE_XML2) && USE_XML
423  configFile
424 #endif
425  ,
426  const std::string &
427 #if defined(VISP_HAVE_XML2) && USE_XML
428  configFile_depth
429 #endif
430 )
431 {
432 #if defined(VISP_HAVE_XML2) && USE_XML
433  // From the xml file
434  dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(configFile, configFile_depth);
435 #else
436  // Edge
437  vpMe me;
438  me.setMaskSize(5);
439  me.setMaskNumber(180);
440  me.setRange(8);
441  me.setThreshold(10000);
442  me.setMu1(0.5);
443  me.setMu2(0.5);
444  me.setSampleStep(4);
445  dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(me);
446 
447 // Klt
448 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
449  vpKltOpencv klt;
450  klt.setMaxFeatures(10000);
451  klt.setWindowSize(5);
452  klt.setQuality(0.01);
453  klt.setMinDistance(5);
454  klt.setHarrisFreeParameter(0.02);
455  klt.setBlockSize(3);
456  klt.setPyramidLevels(3);
457 
458  dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(klt);
459  dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
460 #endif
461 
462  // Depth
463  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalFeatureEstimationMethod(
465  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
466  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
467  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
468  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalSamplingStep(2, 2);
469 
470  dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthDenseSamplingStep(4, 4);
471 
472  vpCameraParameters cam1, cam2;
473  cam1.initPersProjWithoutDistortion(615.1674804688, 615.1675415039, 312.1889953613, 243.4373779297);
474  cam2.initPersProjWithoutDistortion(476.0536193848, 476.0534973145, 311.4845581055, 246.2832336426);
475 
476  dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
477 
478  tracker->setAngleAppear(vpMath::rad(70));
479  tracker->setAngleDisappear(vpMath::rad(80));
480 
481  // Specify the clipping to
482  tracker->setNearClippingDistance(0.01);
483  tracker->setFarClippingDistance(2.0);
484  tracker->setClipping(tracker->getClipping() | vpMbtPolygon::FOV_CLIPPING);
485 // tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING
486 // | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
487 // vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
488 #endif
489 }
490 }
491 
492 int main(int argc, const char **argv)
493 {
494  {
495  // Test TukeyEstimator
496  {
497  vpMbtTukeyEstimator<double> tukey_estimator;
498  std::vector<double> residues;
499  residues.push_back(0.5);
500  residues.push_back(0.1);
501  residues.push_back(0.15);
502  residues.push_back(0.14);
503  residues.push_back(0.12);
504  std::vector<double> weights(5, 1);
505 
506  tukey_estimator.MEstimator(residues, weights, 1e-3);
507 
508  for (size_t i = 0; i < weights.size(); i++) {
509  std::cout << "residues[" << i << "]=" << residues[i] << " ; weights[i" << i << "]=" << weights[i] << std::endl;
510  }
511  std::cout << std::endl;
512  }
513 
514  {
515  vpMbtTukeyEstimator<float> tukey_estimator;
516  std::vector<float> residues;
517  residues.push_back(0.5f);
518  residues.push_back(0.1f);
519  residues.push_back(0.15f);
520  residues.push_back(0.14f);
521  residues.push_back(0.12f);
522  std::vector<float> weights(5, 1);
523 
524  tukey_estimator.MEstimator(residues, weights, (float)1e-3);
525 
526  for (size_t i = 0; i < weights.size(); i++) {
527  std::cout << "residues[" << i << "]=" << residues[i] << " ; weights[i" << i << "]=" << weights[i] << std::endl;
528  }
529  std::cout << std::endl;
530  }
531  }
532 
533  try {
534  std::string env_ipath;
535  std::string opt_ipath;
536  std::string ipath;
537  std::string opt_configFile;
538  std::string opt_configFile_depth;
539  std::string opt_modelFile;
540  std::string opt_modelFile_depth;
541  std::string opt_initFile;
542  std::string initFile;
543  bool displayFeatures = true;
544  bool opt_click_allowed = true;
545  bool opt_display = true;
546  bool useOgre = false;
547  bool showOgreConfigDialog = false;
548  bool useScanline = false;
549  bool computeCovariance = false;
550  bool projectionError = false;
551  int trackerType_image = vpMbGenericTracker::EDGE_TRACKER;
552  int trackerType_depth = vpMbGenericTracker::DEPTH_DENSE_TRACKER;
553 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
554  // To avoid Debian test timeout
555  int opt_lastFrame = 5;
556 #else
557  int opt_lastFrame = -1;
558 #endif
559 
560  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
561  // environment variable value
562  env_ipath = vpIoTools::getViSPImagesDataPath();
563 
564  // Set the default input path
565  if (!env_ipath.empty())
566  ipath = env_ipath;
567 
568  // Read the command line options
569  if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_configFile_depth, opt_modelFile, opt_modelFile_depth,
570  opt_initFile, displayFeatures, opt_click_allowed, opt_display, useOgre, showOgreConfigDialog,
571  useScanline, computeCovariance, projectionError, trackerType_image, trackerType_depth,
572  opt_lastFrame)) {
573  return EXIT_FAILURE;
574  }
575 
576 #if !defined(VISP_HAVE_MODULE_KLT) || (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100))
577  if (trackerType_image == /*vpMbGenericTracker::KLT_TRACKER*/ 2) {
578  std::cout << "KLT only features cannot be used: ViSP is not built with "
579  "KLT module or OpenCV is not available."
580  << std::endl;
581  return EXIT_SUCCESS;
582  }
583 #endif
584 
585  // Test if an input path is set
586  if (opt_ipath.empty() && env_ipath.empty()) {
587  usage(argv[0], NULL);
588  std::cerr << std::endl << "ERROR:" << std::endl;
589  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
590  << " environment variable to specify the location of the " << std::endl
591  << " image path where test images are located." << std::endl
592  << std::endl;
593 
594  return EXIT_FAILURE;
595  }
596 
597  // Get the option values
598  ipath = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/castel");
599 
600  std::string dir_path = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth");
601  if (!vpIoTools::checkDirectory(dir_path)) {
602  std::cerr << "ViSP-images does not contain the folder: " << dir_path << "!" << std::endl;
603  return EXIT_SUCCESS;
604  }
605 
606  std::string configFile, configFile_depth;
607  if (!opt_configFile.empty())
608  configFile = opt_configFile;
609  else
610  configFile =
611  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.xml");
612 
613  if (!opt_configFile_depth.empty())
614  configFile_depth = opt_configFile_depth;
615  else
616  configFile_depth =
617  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_depth.xml");
618 
619  std::string modelFile, modelFile_depth;
620  if (!opt_modelFile.empty())
621  modelFile = opt_modelFile;
622  else {
623 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
624  modelFile =
625  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_gantry.wrl");
626 #else
627  modelFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao");
628 #endif
629  }
630 
631  if (!opt_modelFile_depth.empty())
632  modelFile_depth = opt_modelFile_depth;
633  else
634  modelFile_depth =
635  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao");
636 
637  std::string vrml_ext = ".wrl";
638  bool use_vrml =
639  (modelFile.compare(modelFile.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0) ||
640  (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
641 
642  if (use_vrml) {
643 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
644  std::cout << "use_vrml: " << use_vrml << std::endl;
645 #else
646  std::cerr << "Error: vrml model file is only supported if ViSP is "
647  "build with Coin3D 3rd party"
648  << std::endl;
649  return EXIT_FAILURE;
650 #endif
651  }
652 
653  if (!opt_initFile.empty())
654  initFile = opt_initFile;
655  else
656  initFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.init");
657 
658  vpImage<unsigned char> I, I_depth;
659  vpImage<uint16_t> I_depth_raw;
660  std::vector<vpColVector> pointcloud;
661  unsigned int pointcloud_width, pointcloud_height;
662  if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) {
663  std::cerr << "Cannot open sequence: " << ipath << std::endl;
664  return EXIT_FAILURE;
665  }
666 
667  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
668 
669 // initialise a display
670 #if defined VISP_HAVE_X11
671  vpDisplayX display1, display2;
672 #elif defined VISP_HAVE_GDI
673  vpDisplayGDI display1, display2;
674 #elif defined VISP_HAVE_OPENCV
675  vpDisplayOpenCV display1, display2;
676 #elif defined VISP_HAVE_D3D9
677  vpDisplayD3D display1, display2;
678 #elif defined VISP_HAVE_GTK
679  vpDisplayGTK display1, display2;
680 #else
681  opt_display = false;
682 #endif
683  if (opt_display) {
684 #if (defined VISP_HAVE_DISPLAY)
687  display1.init(I, 100, 100, "Test tracking (Left)");
688  display2.init(I_depth, (int)(I.getWidth() / vpDisplay::getDownScalingFactor(I)) + 110, 100,
689  "Test tracking (Right)");
690 #endif
692  vpDisplay::display(I_depth);
693  vpDisplay::flush(I);
694  vpDisplay::flush(I_depth);
695  }
696 
697  std::vector<int> trackerTypes(2);
698  trackerTypes[0] = trackerType_image;
699  trackerTypes[1] = trackerType_depth;
700  vpMbTracker *tracker = new vpMbGenericTracker(trackerTypes);
701  vpHomogeneousMatrix c1Mo, c2Mo;
702  vpCameraParameters cam1, cam2;
703 
704  loadConfiguration(tracker, configFile, configFile_depth);
705 
706  vpHomogeneousMatrix depth_M_color;
707  std::string depth_M_color_filename =
708  vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/depth_M_color.txt");
709  {
710  std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
711  depth_M_color.load(depth_M_color_file);
712  std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformationMatrices;
713  mapOfCameraTransformationMatrices["Camera2"] = depth_M_color;
714  dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrices);
715  }
716 
717  // Display the moving edges, and the Klt points
718  tracker->setDisplayFeatures(displayFeatures);
719 
720  // Tells if the tracker has to use Ogre3D for visibility tests
721  tracker->setOgreVisibilityTest(useOgre);
722  if (useOgre)
723  tracker->setOgreShowConfigDialog(showOgreConfigDialog);
724 
725  // Tells if the tracker has to use the scanline visibility tests
726  tracker->setScanLineVisibilityTest(useScanline);
727 
728  // Tells if the tracker has to compute the covariance matrix
729  tracker->setCovarianceComputation(computeCovariance);
730 
731  // Tells if the tracker has to compute the projection error
732  tracker->setProjectionErrorComputation(projectionError);
733 
734  // Retrieve the camera parameters from the tracker
735  dynamic_cast<vpMbGenericTracker *>(tracker)->getCameraParameters(cam1, cam2);
736 
737  // Loop to position the cube
738  if (opt_display && opt_click_allowed) {
739  while (!vpDisplay::getClick(I, false)) {
741  vpDisplay::displayText(I, 15, 10, "click after positioning the object", vpColor::red);
742  vpDisplay::flush(I);
743  }
744  }
745 
746  // Load the 3D model (either a vrml file or a .cao file)
747  dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile, modelFile_depth);
748 
749  if (opt_display && opt_click_allowed) {
750  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
751  mapOfImages["Camera1"] = &I;
752  mapOfImages["Camera2"] = &I_depth;
753  std::map<std::string, std::string> mapOfInitFiles;
754  mapOfInitFiles["Camera1"] = initFile;
755 
756  // Initialise the tracker by clicking on the image
757  dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(mapOfImages, mapOfInitFiles, true);
758  dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
759  // display the 3D model at the given pose
760  dynamic_cast<vpMbGenericTracker *>(tracker)->display(I, I_depth, c1Mo, c2Mo, cam1, cam2, vpColor::red);
761  } else {
762  vpHomogeneousMatrix c1Moi(0.06846423368, 0.09062570884, 0.3401096693, -2.671882598, 0.1174275908, -0.6011935263);
763  vpHomogeneousMatrix c2Moi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
764  dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(I, I_depth, c1Moi, c2Moi);
765  }
766 
767  // track the model
768  {
769  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
770  mapOfImages["Camera1"] = &I;
771  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
772  mapOfPointclouds["Camera2"] = &pointcloud;
773  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
774  mapOfWidths["Camera2"] = pointcloud_width;
775  mapOfHeights["Camera2"] = pointcloud_height;
776 
777  dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
778  }
779  dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
780 
781  if (opt_display) {
782  vpDisplay::flush(I);
783  vpDisplay::flush(I_depth);
784  }
785 
786  bool quit = false, click = false;
787  unsigned int frame_index = 0;
788  std::vector<double> time_vec;
789  while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
790  (opt_lastFrame > 0 ? (int)frame_index <= opt_lastFrame : true)) {
791  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
792 
793  if (opt_display) {
795  vpDisplay::display(I_depth);
796 
797  std::stringstream ss;
798  ss << "Num frame: " << frame_index;
799  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
800  }
801 
802  // Test reset the tracker
803  if (frame_index == 10) {
804  std::cout << "----------Test reset tracker----------" << std::endl;
805  if (opt_display) {
807  vpDisplay::display(I_depth);
808  }
809 
810  tracker->resetTracker();
811 
812  loadConfiguration(tracker, configFile, configFile_depth);
813  dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile, modelFile_depth);
814  dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
815  tracker->setOgreVisibilityTest(useOgre);
816  tracker->setScanLineVisibilityTest(useScanline);
817  tracker->setCovarianceComputation(computeCovariance);
818  tracker->setProjectionErrorComputation(projectionError);
819  dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(I, I_depth, c1Mo, c2Mo);
820  }
821 
822 // Test to set an initial pose
823 #if USE_SMALL_DATASET
824  if (frame_index == 20) {
825  c1Mo.buildFrom(0.07734634051, 0.08993639906, 0.342344402, -2.708409543, 0.0669276477, -0.3798958303);
826  c2Mo.buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
827 #else
828  if (frame_index == 50) {
829  c1Mo.buildFrom(0.09280663035, 0.09277655672, 0.330415149, -2.724431817, 0.0293932671, 0.02027966377);
830  c2Mo.buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
831 #endif
832  std::cout << "Test set pose" << std::endl;
833  dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(I, I_depth, c1Mo, c2Mo);
834  }
835 
836 #if USE_SMALL_DATASET
837  // track the object: stop tracking from frame 15 to 20
838  if (frame_index < 15 || frame_index >= 20) {
839 #else
840  // track the object: stop tracking from frame 30 to 50
841  if (frame_index < 30 || frame_index >= 50) {
842 #endif
843  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
844  mapOfImages["Camera1"] = &I;
845  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
846  mapOfPointclouds["Camera2"] = &pointcloud;
847  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
848  mapOfWidths["Camera2"] = pointcloud_width;
849  mapOfHeights["Camera2"] = pointcloud_height;
850 
851  double t = vpTime::measureTimeMs();
852  dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
853  t = vpTime::measureTimeMs() - t;
854  time_vec.push_back(t);
855 
856  dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
857 
858  if (opt_display) {
859  // display the 3D model
860  dynamic_cast<vpMbGenericTracker *>(tracker)->display(I, I_depth, c1Mo, c2Mo, cam1, cam2, vpColor::darkRed);
861  // display the frame
862  vpDisplay::displayFrame(I, c1Mo, cam1, 0.05);
863  vpDisplay::displayFrame(I_depth, c2Mo, cam2, 0.05);
864  // computation time
865  std::stringstream ss;
866  ss << "Computation time: " << t << " ms";
867  vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
868  }
869  }
870 
871  if (opt_click_allowed) {
872  vpDisplay::displayText(I, 10, 10, "Click to quit", vpColor::red);
874  if (vpDisplay::getClick(I, button, click)) {
875  switch (button) {
877  quit = !click;
878  break;
879 
881  click = !click;
882  break;
883 
884  default:
885  break;
886  }
887  }
888  }
889 
890  if (computeCovariance) {
891  std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
892  }
893 
894  if (projectionError) {
895  std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl;
896  }
897 
898  vpDisplay::flush(I);
899  vpDisplay::flush(I_depth);
900 
901  frame_index++;
902  }
903 
904  std::cout << "\nFinal poses, c1Mo:\n" << c1Mo << "\nc2Mo:\n" << c2Mo << std::endl;
905  std::cout << "\nComputation time, Mean: " << vpMath::getMean(time_vec)
906  << " ms ; Median: " << vpMath::getMedian(time_vec) << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms"
907  << std::endl;
908 
909  if (opt_click_allowed && !quit) {
911  }
912 
913  delete tracker;
914  tracker = NULL;
915 
916 #if defined(VISP_HAVE_XML2) && USE_XML
917  // Cleanup memory allocated by xml library used to parse the xml config
918  // file in vpMbGenericTracker::loadConfigFile()
920 #endif
921 
922 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
923  // Cleanup memory allocated by Coin library used to load a vrml model in
924  // vpMbGenericTracker::loadModel() We clean only if Coin was used.
925  if (use_vrml)
926  SoDB::finish();
927 #endif
928 
929  return EXIT_SUCCESS;
930  } catch (vpException &e) {
931  std::cout << "Catch an exception: " << e << std::endl;
932  return EXIT_FAILURE;
933  }
934 }
935 
936 #else
937 int main()
938 {
939  std::cerr << "visp_mbt, visp_gui modules and OpenCV are required to run "
940  "this example."
941  << std::endl;
942  return EXIT_SUCCESS;
943 }
944 #endif
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:223
virtual void setDisplayFeatures(const bool displayF)
Definition: vpMbTracker.h:470
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:452
virtual void setOgreShowConfigDialog(const bool showConfigDialog)
Definition: vpMbTracker.h:575
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:371
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1214
void setHarrisFreeParameter(double harris_k)
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:433
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:454
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
void setMaxFeatures(const int maxCount)
void setSampleStep(const double &s)
Definition: vpMe.h:278
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
virtual double getProjectionError() const
Definition: vpMbTracker.h:277
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Definition: vpMe.h:60
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
void setMu1(const double &mu_1)
Definition: vpMe.h:241
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 setQuality(double qualityLevel)
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
static bool checkFilename(const char *filename)
Definition: vpIoTools.cpp:577
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:1439
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:461
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:106
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:856
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:422
void setPyramidLevels(const int pyrMaxLevel)
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
void setMu2(const double &mu_2)
Definition: vpMe.h:248
void setWindowSize(const int winSize)
virtual void setOgreVisibilityTest(const bool &v)
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:207
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))
unsigned int getDownScalingFactor()
Definition: vpDisplay.h:229
virtual vpMatrix getCovarianceMatrix() const
Definition: vpMbTracker.h:232
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
void setBlockSize(const int blockSize)
void setThreshold(const double &t)
Definition: vpMe.h:300
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:533
virtual void setClipping(const unsigned int &flags)
void setRange(const unsigned int &r)
Definition: vpMe.h:271
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:531
virtual void setNearClippingDistance(const double &dist)