Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
testGenericTracker.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  * Regression test for MBT.
33  *
34  *****************************************************************************/
35 
42 #include <cstdlib>
43 #include <iostream>
44 #include <visp3/core/vpConfig.h>
45 
46 #if defined(VISP_HAVE_MODULE_MBT) \
47  && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
48 
49 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
50 #include <type_traits>
51 #endif
52 
53 #include <visp3/core/vpIoTools.h>
54 #include <visp3/core/vpImageDraw.h>
55 #include <visp3/core/vpFont.h>
56 #include <visp3/io/vpParseArgv.h>
57 #include <visp3/io/vpImageIo.h>
58 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/gui/vpDisplayGDI.h>
60 #include <visp3/gui/vpDisplayOpenCV.h>
61 #include <visp3/gui/vpDisplayD3D.h>
62 #include <visp3/gui/vpDisplayGTK.h>
63 #include <visp3/mbt/vpMbGenericTracker.h>
64 
65 #define GETOPTARGS "i:dsclt:e:DmCh"
66 
67 namespace
68 {
69  void usage(const char *name, const char *badparam)
70  {
71  fprintf(stdout, "\n\
72  Regression test for vpGenericTracker.\n\
73  \n\
74  SYNOPSIS\n\
75  %s [-i <test image path>] [-c] [-d] [-s] [-h] [-l] \n\
76  [-t <tracker type>] [-e <last frame index>] [-D] [-m] [-C]\n", name);
77 
78  fprintf(stdout, "\n\
79  OPTIONS: \n\
80  -i <input image path> \n\
81  Set image input path.\n\
82  These images come from ViSP-images-x.y.z.tar.gz available \n\
83  on the ViSP website.\n\
84  Setting the VISP_INPUT_IMAGE_PATH environment\n\
85  variable produces the same behavior than using\n\
86  this option.\n\
87  \n\
88  -d \n\
89  Turn off the display.\n\
90  \n\
91  -s \n\
92  If display is turn off, tracking results are saved in a video folder.\n\
93  \n\
94  -c\n\
95  Disable the mouse click. Useful to automate the \n\
96  execution of this program without human intervention.\n\
97  \n\
98  -t <tracker type>\n\
99  Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
100  \n\
101  -l\n\
102  Use the scanline for visibility tests.\n\
103  \n\
104  -e <last frame index>\n\
105  Specify the index of the last frame. Once reached, the tracking is stopped.\n\
106  \n\
107  -D \n\
108  Use depth.\n\
109  \n\
110  -m \n\
111  Set a tracking mask.\n\
112  \n\
113  -C \n\
114  Use color images.\n\
115  \n\
116  -h \n\
117  Print the help.\n\n");
118 
119  if (badparam)
120  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
121  }
122 
123  bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, bool &save,
124  bool &useScanline, int &trackerType, int &lastFrame, bool &use_depth, bool &use_mask,
125  bool &use_color_image)
126  {
127  const char *optarg_;
128  int c;
129  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
130 
131  switch (c) {
132  case 'i':
133  ipath = optarg_;
134  break;
135  case 'c':
136  click_allowed = false;
137  break;
138  case 'd':
139  display = false;
140  break;
141  case 's':
142  save = true;
143  break;
144  case 'l':
145  useScanline = true;
146  break;
147  case 't':
148  trackerType = atoi(optarg_);
149  break;
150  case 'e':
151  lastFrame = atoi(optarg_);
152  break;
153  case 'D':
154  use_depth = true;
155  break;
156  case 'm':
157  use_mask = true;
158  break;
159  case 'C':
160  use_color_image = true;
161  break;
162  case 'h':
163  usage(argv[0], NULL);
164  return false;
165  break;
166 
167  default:
168  usage(argv[0], optarg_);
169  return false;
170  break;
171  }
172  }
173 
174  if ((c == 1) || (c == -1)) {
175  // standalone param or error
176  usage(argv[0], NULL);
177  std::cerr << "ERROR: " << std::endl;
178  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
179  return false;
180  }
181 
182  return true;
183  }
184 
185  template <typename Type>
186  bool read_data(const std::string &input_directory, int cpt, const vpCameraParameters &cam_depth,
187  vpImage<Type> &I, vpImage<uint16_t> &I_depth,
188  std::vector<vpColVector> &pointcloud, vpHomogeneousMatrix &cMo)
189  {
190 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
191  static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
192  "Template function supports only unsigned char and vpRGBa images!");
193 #endif
194  char buffer[256];
195  sprintf(buffer, std::string(input_directory + "/Images/Image_%04d.pgm").c_str(), cpt);
196  std::string image_filename = buffer;
197 
198  sprintf(buffer, std::string(input_directory + "/Depth/Depth_%04d.bin").c_str(), cpt);
199  std::string depth_filename = buffer;
200 
201  sprintf(buffer, std::string(input_directory + "/CameraPose/Camera_%03d.txt").c_str(), cpt);
202  std::string pose_filename = buffer;
203 
204  if (!vpIoTools::checkFilename(image_filename) || !vpIoTools::checkFilename(depth_filename)
205  || !vpIoTools::checkFilename(pose_filename))
206  return false;
207 
208  vpImageIo::read(I, image_filename);
209 
210  unsigned int depth_width = 0, depth_height = 0;
211  std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
212  if (!file_depth.is_open())
213  return false;
214 
215  vpIoTools::readBinaryValueLE(file_depth, depth_height);
216  vpIoTools::readBinaryValueLE(file_depth, depth_width);
217  I_depth.resize(depth_height, depth_width);
218  pointcloud.resize(depth_height*depth_width);
219 
220  const float depth_scale = 0.000030518f;
221  for (unsigned int i = 0; i < I_depth.getHeight(); i++) {
222  for (unsigned int j = 0; j < I_depth.getWidth(); j++) {
223  vpIoTools::readBinaryValueLE(file_depth, I_depth[i][j]);
224  double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
225  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
226  vpColVector pt3d(4, 1.0);
227  pt3d[0] = x*Z;
228  pt3d[1] = y*Z;
229  pt3d[2] = Z;
230  pointcloud[i*I_depth.getWidth()+j] = pt3d;
231  }
232  }
233 
234  std::ifstream file_pose(pose_filename.c_str());
235  if (!file_pose.is_open()) {
236  return false;
237  }
238 
239  for (unsigned int i = 0; i < 4; i++) {
240  for (unsigned int j = 0; j < 4; j++) {
241  file_pose >> cMo[i][j];
242  }
243  }
244 
245  return true;
246  }
247 
248  void convert(const vpImage<vpRGBa> &src, vpImage<vpRGBa> &dst)
249  {
250  dst = src;
251  }
252 
253  void convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> &dst)
254  {
255  vpImageConvert::convert(src, dst);
256  }
257 
258  template <typename Type>
259  bool run(const std::string &input_directory,
260  bool opt_click_allowed, bool opt_display, bool useScanline, int trackerType_image,
261  int opt_lastFrame, bool use_depth, bool use_mask, bool save) {
262 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
263  static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
264  "Template function supports only unsigned char and vpRGBa images!");
265 #endif
266  // Initialise a display
267 #if defined VISP_HAVE_X11
268  vpDisplayX display1, display2;
269 #elif defined VISP_HAVE_GDI
270  vpDisplayGDI display1, display2;
271 #elif defined VISP_HAVE_OPENCV
272  vpDisplayOpenCV display1, display2;
273 #elif defined VISP_HAVE_D3D9
274  vpDisplayD3D display1, display2;
275 #elif defined VISP_HAVE_GTK
276  vpDisplayGTK display1, display2;
277 #else
278  opt_display = false;
279 #endif
280 
281  std::vector<int> tracker_type(2);
282  tracker_type[0] = trackerType_image;
283  tracker_type[1] = vpMbGenericTracker::DEPTH_DENSE_TRACKER;
284  vpMbGenericTracker tracker(tracker_type);
285  std::string configFileCam1 = input_directory + std::string("/Config/chateau.xml");
286  std::string configFileCam2 = input_directory + std::string("/Config/chateau_depth.xml");
287  std::cout << "Load config file for camera 1: " << configFileCam1 << std::endl;
288  std::cout << "Load config file for camera 2: " << configFileCam2 << std::endl;
289  tracker.loadConfigFile(configFileCam1, configFileCam2);
290 #if 0
291  // Corresponding parameters manually set to have an example code
292  {
293  vpCameraParameters cam_color, cam_depth;
294  cam_color.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0);
295  cam_depth.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0);
296  tracker.setCameraParameters(cam_color, cam_depth);
297  }
298 
299  // Edge
300  vpMe me;
301  me.setMaskSize(5);
302  me.setMaskNumber(180);
303  me.setRange(8);
304  me.setThreshold(10000);
305  me.setMu1(0.5);
306  me.setMu2(0.5);
307  me.setSampleStep(5);
308  tracker.setMovingEdge(me);
309 
310  // Klt
311 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
312  vpKltOpencv klt;
313  tracker.setKltMaskBorder(5);
314  klt.setMaxFeatures(10000);
315  klt.setWindowSize(5);
316  klt.setQuality(0.01);
317  klt.setMinDistance(5);
318  klt.setHarrisFreeParameter(0.02);
319  klt.setBlockSize(3);
320  klt.setPyramidLevels(3);
321 
322  tracker.setKltOpencv(klt);
323 #endif
324 
325  // Depth
326  tracker.setDepthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION);
327  tracker.setDepthNormalPclPlaneEstimationMethod(2);
328  tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
329  tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
330  tracker.setDepthNormalSamplingStep(2, 2);
331 
332  tracker.setDepthDenseSamplingStep(4, 4);
333 
334  tracker.setAngleAppear(vpMath::rad(85.0));
335  tracker.setAngleDisappear(vpMath::rad(89.0));
336  tracker.setNearClippingDistance(0.01);
337  tracker.setFarClippingDistance(2.0);
338  tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
339 #endif
340 
341 #ifdef VISP_HAVE_COIN3D
342  tracker.loadModel(input_directory + "/Models/chateau.wrl", input_directory + "/Models/chateau.cao");
343 #else
344  tracker.loadModel(input_directory + "/Models/chateau.cao", input_directory + "/Models/chateau.cao");
345 #endif
347  T[0][0] = -1;
348  T[0][3] = -0.2;
349  T[1][1] = 0;
350  T[1][2] = 1;
351  T[1][3] = 0.12;
352  T[2][1] = 1;
353  T[2][2] = 0;
354  T[2][3] = -0.15;
355  tracker.loadModel(input_directory + "/Models/cube.cao", false, T);
356  vpCameraParameters cam_color, cam_depth;
357  tracker.getCameraParameters(cam_color, cam_depth);
358  tracker.setDisplayFeatures(true);
359  tracker.setScanLineVisibilityTest(useScanline);
360 
361  std::map<int, std::pair<double, double> > map_thresh;
362  //Take the highest thresholds between all CI machines
363 #ifdef VISP_HAVE_COIN3D
365  = useScanline ? std::pair<double, double>(0.005, 3.9) : std::pair<double, double>(0.007, 3.7);
366 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
368  = useScanline ? std::pair<double, double>(0.007, 1.9) : std::pair<double, double>(0.007, 1.8);
370  = useScanline ? std::pair<double, double>(0.005, 3.5) : std::pair<double, double>(0.006, 3.4);
371 #endif
373  = useScanline ? std::pair<double, double>(0.003, 1.7) : std::pair<double, double>(0.002, 0.8);
374 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
376  = std::pair<double, double>(0.002, 0.3);
378  = useScanline ? std::pair<double, double>(0.002, 1.8) : std::pair<double, double>(0.002, 0.7);
379 #endif
380 #else
382  = useScanline ? std::pair<double, double>(0.007, 2.3) : std::pair<double, double>(0.007, 2.1);
383 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
385  = useScanline ? std::pair<double, double>(0.006, 1.7) : std::pair<double, double>(0.005, 1.4);
387  = useScanline ? std::pair<double, double>(0.004, 1.2) : std::pair<double, double>(0.004, 1.2);
388 #endif
390  = useScanline ? std::pair<double, double>(0.002, 0.7) : std::pair<double, double>(0.001, 0.4);
391 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
393  = std::pair<double, double>(0.002, 0.3);
395  = useScanline ? std::pair<double, double>(0.001, 0.5) : std::pair<double, double>(0.001, 0.4);
396 #endif
397 #endif
398 
399  vpImage<Type> I, I_depth;
400  vpImage<uint16_t> I_depth_raw;
401  vpHomogeneousMatrix cMo_truth;
402  std::vector<vpColVector> pointcloud;
403  int cpt_frame = 1;
404  if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
405  std::cerr << "Cannot read first frame!" << std::endl;
406  return EXIT_FAILURE;
407  }
408 
409  vpImage<bool> mask(I.getHeight(), I.getWidth());
410  const double roi_step = 7.0;
411  const double roi_step2 = 6.0;
412  if (use_mask) {
413  mask = false;
414  for (unsigned int i = (unsigned int) (I.getRows()/roi_step); i < (unsigned int) (I.getRows()*roi_step2/roi_step); i++) {
415  for (unsigned int j = (unsigned int) (I.getCols()/roi_step); j < (unsigned int) (I.getCols()*roi_step2/roi_step); j++) {
416  mask[i][j] = true;
417  }
418  }
419  tracker.setMask(mask);
420  }
421 
422  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
423 
424  vpImage<vpRGBa> results(I.getHeight(), I.getWidth() + I_depth.getWidth());
425  vpImage<vpRGBa> resultsColor(I.getHeight(), I.getWidth());
426  vpImage<vpRGBa> resultsDepth(I_depth.getHeight(), I_depth.getWidth());
427  if (save) {
428  vpIoTools::makeDirectory("results");
429  }
430  if (opt_display) {
431 #ifdef VISP_HAVE_DISPLAY
432  display1.init(I, 0, 0, "Image");
433  display2.init(I_depth, (int) I.getWidth(), 0, "Depth");
434 #endif
435  }
436 
437  vpHomogeneousMatrix depth_M_color;
438  depth_M_color[0][3] = -0.05;
439  tracker.setCameraTransformationMatrix("Camera2", depth_M_color);
440  tracker.initFromPose(I, cMo_truth);
441 
442  vpFont font(24);
443  bool click = false, quit = false, correct_accuracy = true;
444  std::vector<double> vec_err_t, vec_err_tu;
445  std::vector<double> time_vec;
446  while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit
447  && (opt_lastFrame > 0 ? (int)cpt_frame <= opt_lastFrame : true)) {
448  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
449 
450  if (opt_display) {
452  vpDisplay::display(I_depth);
453  } else if (save) {
454  convert(I, resultsColor);
455  convert(I_depth, resultsDepth);
456  }
457 
458  double t = vpTime::measureTimeMs();
459  std::map<std::string, const vpImage<Type> *> mapOfImages;
460  mapOfImages["Camera1"] = &I;
461  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
462  mapOfPointclouds["Camera2"] = &pointcloud;
463  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
464  if (!use_depth) {
465  mapOfWidths["Camera2"] = 0;
466  mapOfHeights["Camera2"] = 0;
467  } else {
468  mapOfWidths["Camera2"] = I_depth.getWidth();
469  mapOfHeights["Camera2"] = I_depth.getHeight();
470  }
471 
472  tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
473  vpHomogeneousMatrix cMo = tracker.getPose();
474  t = vpTime::measureTimeMs() - t;
475  time_vec.push_back(t);
476 
477  if (opt_display) {
478  tracker.display(I, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth, vpColor::red, 3);
479  vpDisplay::displayFrame(I, cMo, cam_depth, 0.05, vpColor::none, 3);
480  vpDisplay::displayFrame(I_depth, depth_M_color*cMo, cam_depth, 0.05, vpColor::none, 3);
481 
482  std::stringstream ss;
483  ss << "Frame: " << cpt_frame;
484  vpDisplay::displayText(I_depth, 20, 20, ss.str(), vpColor::red);
485  ss.str("");
486  ss << "Nb features: " << tracker.getError().getRows();
487  vpDisplay::displayText(I_depth, 40, 20, ss.str(), vpColor::red);
488  } else if (save) {
490  std::map<std::string, std::vector<std::vector<double> > > mapOfModels;
491  std::map<std::string, unsigned int> mapOfW;
492  mapOfW["Camera1"] = I.getWidth();
493  mapOfW["Camera2"] = I.getHeight();
494  std::map<std::string, unsigned int> mapOfH;
495  mapOfH["Camera1"] = I_depth.getWidth();
496  mapOfH["Camera2"] = I_depth.getHeight();
497  std::map<std::string, vpHomogeneousMatrix> mapOfcMos;
498  mapOfcMos["Camera1"] = cMo;
499  mapOfcMos["Camera2"] = depth_M_color*cMo;
500  std::map<std::string, vpCameraParameters> mapOfCams;
501  mapOfCams["Camera1"] = cam_color;
502  mapOfCams["Camera2"] = cam_depth;
503  tracker.getModelForDisplay(mapOfModels, mapOfW, mapOfH, mapOfcMos, mapOfCams);
504  for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfModels.begin();
505  it != mapOfModels.end(); ++it) {
506  for (size_t i = 0; i < it->second.size(); i++) {
507  // test if it->second[i][0] = 0
508  if (std::fabs(it->second[i][0]) <= std::numeric_limits<double>::epsilon()) {
509  vpImageDraw::drawLine(it->first == "Camera1" ? resultsColor : resultsDepth, vpImagePoint(it->second[i][1], it->second[i][2]),
510  vpImagePoint(it->second[i][3], it->second[i][4]), vpColor::red, 3);
511  }
512  }
513  }
515 
517  std::map<std::string, std::vector<std::vector<double> > > mapOfFeatures;
518  tracker.getFeaturesForDisplay(mapOfFeatures);
519  for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfFeatures.begin();
520  it != mapOfFeatures.end(); ++it) {
521  for (size_t i = 0; i < it->second.size(); i++) {
522  if (std::fabs(it->second[i][0]) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][0] = 0 for ME
523  vpColor color = vpColor::yellow;
524  if (std::fabs(it->second[i][3]) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][3] = 0
525  color = vpColor::green;
526  } else if (std::fabs(it->second[i][3] - 1) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][3] = 1
527  color = vpColor::blue;
528  } else if (std::fabs(it->second[i][3] - 2) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][3] = 2
529  color = vpColor::purple;
530  } else if (std::fabs(it->second[i][3] - 3) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][3] = 3
531  color = vpColor::red;
532  } else if (std::fabs(it->second[i][3] - 4) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][3] = 4
533  color = vpColor::cyan;
534  }
535  vpImageDraw::drawCross(it->first == "Camera1" ? resultsColor : resultsDepth, vpImagePoint(it->second[i][1], it->second[i][2]),
536  3, color, 1);
537  } else if (std::fabs(it->second[i][0] - 1) <= std::numeric_limits<double>::epsilon()) { // test it->second[i][0] = 1 for KLT
538  vpImageDraw::drawCross(it->first == "Camera1" ? resultsColor : resultsDepth, vpImagePoint(it->second[i][1], it->second[i][2]),
539  10, vpColor::red, 1);
540  }
541  }
542  }
544 
545  //Computation time
546  std::ostringstream oss;
547  oss << "Tracking time: " << t << " ms";
548  font.drawText(resultsColor, oss.str(), vpImagePoint(20,20), vpColor::red);
549  }
550 
551  vpPoseVector pose_est(cMo);
552  vpPoseVector pose_truth(cMo_truth);
553  vpColVector t_est(3), t_truth(3);
554  vpColVector tu_est(3), tu_truth(3);
555  for (unsigned int i = 0; i < 3; i++) {
556  t_est[i] = pose_est[i];
557  t_truth[i] = pose_truth[i];
558  tu_est[i] = pose_est[i+3];
559  tu_truth[i] = pose_truth[i+3];
560  }
561 
562  vpColVector t_err = t_truth-t_est, tu_err = tu_truth-tu_est;
563  const double t_thresh = map_thresh[!use_depth ? trackerType_image : trackerType_image | vpMbGenericTracker::DEPTH_DENSE_TRACKER].first;
564  const double tu_thresh = map_thresh[!use_depth ? trackerType_image : trackerType_image | vpMbGenericTracker::DEPTH_DENSE_TRACKER].second;
565  double t_err2 = sqrt(t_err.sumSquare()), tu_err2 = vpMath::deg(sqrt(tu_err.sumSquare()));
566  vec_err_t.push_back( t_err2 );
567  vec_err_tu.push_back( tu_err2 );
568  if ( !use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh) ) { //no accuracy test with mask
569  std::cerr << "Pose estimated exceeds the threshold (t_thresh = " << t_thresh << " ; tu_thresh = " << tu_thresh << ")!" << std::endl;
570  std::cout << "t_err: " << t_err2 << " ; tu_err: " << tu_err2 << std::endl;
571  correct_accuracy = false;
572  }
573 
574  if (opt_display) {
575  if (use_mask) {
576  vpRect roi(vpImagePoint(I.getRows()/roi_step, I.getCols()/roi_step),
577  vpImagePoint(I.getRows()*roi_step2/roi_step, I.getCols()*roi_step2/roi_step));
580  }
581 
582  vpDisplay::flush(I);
583  vpDisplay::flush(I_depth);
584  } else if (save) {
586  char buffer[256];
587  std::ostringstream oss;
588  oss << "results/image_%04d.png";
589  sprintf(buffer, oss.str().c_str(), cpt_frame);
590 
591  results.insert(resultsColor, vpImagePoint());
592  results.insert(resultsDepth, vpImagePoint(0, resultsColor.getWidth()));
593 
594  vpImageIo::write(results, buffer);
596  }
597 
598  if (opt_display && opt_click_allowed) {
600  if (vpDisplay::getClick(I, button, click)) {
601  switch (button) {
603  quit = !click;
604  break;
605 
607  click = !click;
608  break;
609 
610  default:
611  break;
612  }
613  }
614  }
615 
616  cpt_frame++;
617  }
618 
619  if (!time_vec.empty())
620  std::cout << "Computation time, Mean: " << vpMath::getMean(time_vec) << " ms ; Median: " << vpMath::getMedian(time_vec)
621  << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms" << std::endl;
622 
623  if (!vec_err_t.empty())
624  std::cout << "Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
625 
626  if (!vec_err_tu.empty())
627  std::cout << "Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
628 
629  return correct_accuracy ? EXIT_SUCCESS : EXIT_FAILURE;
630  }
631 }
632 
633 int main(int argc, const char *argv[])
634 {
635  try {
636  std::string env_ipath;
637  std::string opt_ipath = "";
638  bool opt_click_allowed = true;
639  bool opt_display = true;
640  bool opt_save = false;
641  bool useScanline = false;
642  int trackerType_image = vpMbGenericTracker::EDGE_TRACKER;
643 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
644  // To avoid Debian test timeout
645  int opt_lastFrame = 5;
646 #else
647  int opt_lastFrame = -1;
648 #endif
649  bool use_depth = false;
650  bool use_mask = false;
651  bool use_color_image = false;
652 
653  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
654  // environment variable value
655  env_ipath = vpIoTools::getViSPImagesDataPath();
656 
657  // Read the command line options
658  if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_save,
659  useScanline, trackerType_image, opt_lastFrame, use_depth,
660  use_mask, use_color_image)) {
661  return EXIT_FAILURE;
662  }
663 
664  std::cout << "trackerType_image: " << trackerType_image << std::endl;
665  std::cout << "useScanline: " << useScanline << std::endl;
666  std::cout << "use_depth: " << use_depth << std::endl;
667  std::cout << "use_mask: " << use_mask << std::endl;
668  std::cout << "use_color_image: " << use_color_image << std::endl;
669 #ifdef VISP_HAVE_COIN3D
670  std::cout << "COIN3D available." << std::endl;
671 #endif
672 
673 #if !defined(VISP_HAVE_MODULE_KLT) || (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100))
674  if (trackerType_image & 2) {
675  std::cout << "KLT features cannot be used: ViSP is not built with "
676  "KLT module or OpenCV is not available.\nTest is not run."
677  << std::endl;
678  return EXIT_SUCCESS;
679  }
680 #endif
681 
682  // Test if an input path is set
683  if (opt_ipath.empty() && env_ipath.empty()) {
684  usage(argv[0], NULL);
685  std::cerr << std::endl << "ERROR:" << std::endl;
686  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
687  << " environment variable to specify the location of the " << std::endl
688  << " image path where test images are located." << std::endl
689  << std::endl;
690 
691  return EXIT_FAILURE;
692  }
693 
694  std::string input_directory = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/Castle-simu");
695  if (!vpIoTools::checkDirectory(input_directory)) {
696  std::cerr << "ViSP-images does not contain the folder: " << input_directory << "!" << std::endl;
697  return EXIT_SUCCESS;
698  }
699 
700  if (use_color_image) {
701  return run<vpRGBa>(input_directory, opt_click_allowed, opt_display, useScanline,
702  trackerType_image, opt_lastFrame, use_depth, use_mask, opt_save);
703  } else {
704  return run<unsigned char>(input_directory, opt_click_allowed, opt_display, useScanline,
705  trackerType_image, opt_lastFrame, use_depth, use_mask, opt_save);
706  }
707  } catch (const vpException &e) {
708  std::cout << "Catch an exception: " << e << std::endl;
709  return EXIT_FAILURE;
710  }
711 }
712 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
713 int main()
714 {
715  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
716  return EXIT_SUCCESS;
717 }
718 #else
719 int main() {
720  std::cout << "Enable MBT module (VISP_HAVE_MODULE_MBT) to launch this test." << std::endl;
721  return EXIT_SUCCESS;
722 }
723 #endif
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
void setMaxFeatures(int maxCount)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
void setHarrisFreeParameter(double harris_k)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getRows() const
Definition: vpImage.h:218
void setMaskNumber(const unsigned int &a)
Definition: vpMe.cpp:452
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:1993
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:261
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:157
void setSampleStep(const double &s)
Definition: vpMe.h:278
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:291
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:134
static const vpColor none
Definition: vpColor.h:229
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
Definition: vpException.h:71
Definition: vpMe.h:60
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
static const vpColor green
Definition: vpColor.h:220
Real-time 6D object pose tracking using its CAD model.
static void flush(const vpImage< unsigned char > &I)
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:293
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
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:217
void setQuality(double qualityLevel)
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:241
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:106
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
static const vpColor cyan
Definition: vpColor.h:226
void setMaskSize(const unsigned int &a)
Definition: vpMe.cpp:459
void setPyramidLevels(int pyrMaxLevel)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
unsigned int getCols() const
Definition: vpImage.h:179
Generic class defining intrinsic camera parameters.
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:134
void setWindowSize(int winSize)
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
Font drawing functions for image.
Definition: vpFont.h:55
static double rad(double deg)
Definition: vpMath.h:110
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
void setMu2(const double &mu_2)
Definition: vpMe.h:248
static double deg(double rad)
Definition: vpMath.h:103
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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))
static void drawCross(vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, unsigned char color, unsigned int thickness=1)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:78
double sumSquare() const
void setThreshold(const double &t)
Definition: vpMe.h:300
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:802
Defines a rectangle in the plane.
Definition: vpRect.h:79
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void setRange(const unsigned int &r)
Definition: vpMe.h:271
static const vpColor yellow
Definition: vpColor.h:225
static const vpColor purple
Definition: vpColor.h:228
unsigned int getWidth() const
Definition: vpImage.h:246
void setBlockSize(int blockSize)
static void drawLine(vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, unsigned char color, unsigned int thickness=1)
Definition of the vpImage class member functions.
Definition: vpImage.h:126
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static const vpColor blue
Definition: vpColor.h:223