Visual Servoing Platform  version 3.2.0
testGenericTrackerDepth.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 depth 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 
48 #include <visp3/core/vpIoTools.h>
49 #include <visp3/io/vpParseArgv.h>
50 #include <visp3/io/vpImageIo.h>
51 #include <visp3/gui/vpDisplayX.h>
52 #include <visp3/gui/vpDisplayGDI.h>
53 #include <visp3/gui/vpDisplayOpenCV.h>
54 #include <visp3/gui/vpDisplayD3D.h>
55 #include <visp3/gui/vpDisplayGTK.h>
56 #include <visp3/mbt/vpMbGenericTracker.h>
57 
58 #define GETOPTARGS "i:dcle:mh"
59 
60 namespace
61 {
62  void usage(const char *name, const char *badparam)
63  {
64  fprintf(stdout, "\n\
65  Regression test for vpGenericTracker and depth.\n\
66  \n\
67  SYNOPSIS\n\
68  %s [-i <test image path>] [-c] [-d] [-h] [-l] \n\
69  [-e <last frame index>] [-m]\n", name);
70 
71  fprintf(stdout, "\n\
72  OPTIONS: \n\
73  -i <input image path> \n\
74  Set image input path.\n\
75  These images come from ViSP-images-x.y.z.tar.gz available \n\
76  on the ViSP website.\n\
77  Setting the VISP_INPUT_IMAGE_PATH environment\n\
78  variable produces the same behavior than using\n\
79  this option.\n\
80  \n\
81  -d \n\
82  Turn off the display.\n\
83  \n\
84  -c\n\
85  Disable the mouse click. Useful to automate the \n\
86  execution of this program without human intervention.\n\
87  \n\
88  -l\n\
89  Use the scanline for visibility tests.\n\
90  \n\
91  -e <last frame index>\n\
92  Specify the index of the last frame. Once reached, the tracking is stopped.\n\
93  \n\
94  -m \n\
95  Set a tracking mask.\n\
96  \n\
97  -h \n\
98  Print the help.\n\n");
99 
100  if (badparam)
101  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
102  }
103 
104  bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display,
105  bool &useScanline, int &lastFrame, bool &use_mask)
106  {
107  const char *optarg_;
108  int c;
109  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
110 
111  switch (c) {
112  case 'i':
113  ipath = optarg_;
114  break;
115  case 'c':
116  click_allowed = false;
117  break;
118  case 'd':
119  display = false;
120  break;
121  case 'l':
122  useScanline = true;
123  break;
124  case 'e':
125  lastFrame = atoi(optarg_);
126  break;
127  case 'm':
128  use_mask = true;
129  break;
130  case 'h':
131  usage(argv[0], NULL);
132  return false;
133  break;
134 
135  default:
136  usage(argv[0], optarg_);
137  return false;
138  break;
139  }
140  }
141 
142  if ((c == 1) || (c == -1)) {
143  // standalone param or error
144  usage(argv[0], NULL);
145  std::cerr << "ERROR: " << std::endl;
146  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
147  return false;
148  }
149 
150  return true;
151  }
152 
153  bool read_data(const std::string &input_directory, const int cpt, const vpCameraParameters &cam_depth,
155  std::vector<vpColVector> &pointcloud, vpHomogeneousMatrix &cMo)
156  {
157  char buffer[256];
158  sprintf(buffer, std::string(input_directory + "/Images/Image_%04d.pgm").c_str(), cpt);
159  std::string image_filename = buffer;
160 
161  sprintf(buffer, std::string(input_directory + "/Depth/Depth_%04d.bin").c_str(), cpt);
162  std::string depth_filename = buffer;
163 
164  sprintf(buffer, std::string(input_directory + "/CameraPose/Camera_%03d.txt").c_str(), cpt);
165  std::string pose_filename = buffer;
166 
167  if (!vpIoTools::checkFilename(image_filename) || !vpIoTools::checkFilename(depth_filename)
168  || !vpIoTools::checkFilename(pose_filename))
169  return false;
170 
171  vpImageIo::read(I, image_filename);
172 
173  unsigned int depth_width = 0, depth_height = 0;
174  std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
175  if (!file_depth.is_open())
176  return false;
177 
178  vpIoTools::readBinaryValueLE(file_depth, depth_height);
179  vpIoTools::readBinaryValueLE(file_depth, depth_width);
180  I_depth.resize(depth_height, depth_width);
181  pointcloud.resize(depth_height*depth_width);
182 
183  const float depth_scale = 0.000030518f;
184  for (unsigned int i = 0; i < I_depth.getHeight(); i++) {
185  for (unsigned int j = 0; j < I_depth.getWidth(); j++) {
186  vpIoTools::readBinaryValueLE(file_depth, I_depth[i][j]);
187  double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
188  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
189  vpColVector pt3d(4, 1.0);
190  pt3d[0] = x*Z;
191  pt3d[1] = y*Z;
192  pt3d[2] = Z;
193  pointcloud[i*I_depth.getWidth()+j] = pt3d;
194  }
195  }
196 
197  std::ifstream file_pose(pose_filename.c_str());
198  if (!file_pose.is_open()) {
199  return false;
200  }
201 
202  for (unsigned int i = 0; i < 4; i++) {
203  for (unsigned int j = 0; j < 4; j++) {
204  file_pose >> cMo[i][j];
205  }
206  }
207 
208  return true;
209  }
210 }
211 
212 int main(int argc, const char *argv[])
213 {
214  try {
215  std::string env_ipath;
216  std::string opt_ipath = "";
217  bool opt_click_allowed = true;
218  bool opt_display = true;
219  bool useScanline = false;
220 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
221  // To avoid Debian test timeout
222  int opt_lastFrame = 5;
223 #else
224  int opt_lastFrame = -1;
225 #endif
226  bool use_mask = false;
227 
228  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
229  // environment variable value
230  env_ipath = vpIoTools::getViSPImagesDataPath();
231 
232  // Read the command line options
233  if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display,
234  useScanline, opt_lastFrame, use_mask)) {
235  return EXIT_FAILURE;
236  }
237 
238  std::cout << "useScanline: " << useScanline << std::endl;
239  std::cout << "use_mask: " << use_mask << std::endl;
240 
241  // Test if an input path is set
242  if (opt_ipath.empty() && env_ipath.empty()) {
243  usage(argv[0], NULL);
244  std::cerr << std::endl << "ERROR:" << std::endl;
245  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
246  << " environment variable to specify the location of the " << std::endl
247  << " image path where test images are located." << std::endl
248  << std::endl;
249 
250  return EXIT_FAILURE;
251  }
252 
253  std::string input_directory = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/Castle-simu");
254  if (!vpIoTools::checkDirectory(input_directory)) {
255  std::cerr << "ViSP-images does not contain the folder: " << input_directory << "!" << std::endl;
256  return EXIT_SUCCESS;
257  }
258 
259  // Initialise a display
260 #if defined VISP_HAVE_X11
261  vpDisplayX display1, display2;
262 #elif defined VISP_HAVE_GDI
263  vpDisplayGDI display1, display2;
264 #elif defined VISP_HAVE_OPENCV
265  vpDisplayOpenCV display1, display2;
266 #elif defined VISP_HAVE_D3D9
267  vpDisplayD3D display1, display2;
268 #elif defined VISP_HAVE_GTK
269  vpDisplayGTK display1, display2;
270 #else
271  opt_display = false;
272 #endif
273 
274  std::vector<int> tracker_type;
275  tracker_type.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
276  vpMbGenericTracker tracker(tracker_type);
277 #if defined(VISP_HAVE_XML2)
278  tracker.loadConfigFile(input_directory + "/Config/chateau_depth.xml");
279 #else
280  {
281  vpCameraParameters cam_depth;
282  cam_depth.initPersProjWithoutDistortion(700.0, 700.0, 320.0, 240.0);
283  tracker.setCameraParameters(cam_depth);
284  }
285  // Depth
286  tracker.setDepthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION);
287  tracker.setDepthNormalPclPlaneEstimationMethod(2);
288  tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
289  tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
290  tracker.setDepthNormalSamplingStep(2, 2);
291 
292  tracker.setDepthDenseSamplingStep(4, 4);
293 
294 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
295  tracker.setKltMaskBorder(5);
296 #endif
297 
298  tracker.setAngleAppear(vpMath::rad(85.0));
299  tracker.setAngleDisappear(vpMath::rad(89.0));
300  tracker.setNearClippingDistance(0.01);
301  tracker.setFarClippingDistance(2.0);
302  tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
303 #endif
304  tracker.loadModel(input_directory + "/Models/chateau.cao");
306  T[0][0] = -1;
307  T[0][3] = -0.2;
308  T[1][1] = 0;
309  T[1][2] = 1;
310  T[1][3] = 0.12;
311  T[2][1] = 1;
312  T[2][2] = 0;
313  T[2][3] = -0.15;
314  tracker.loadModel(input_directory + "/Models/cube.cao", false, T);
315  vpCameraParameters cam_depth;
316  tracker.getCameraParameters(cam_depth);
317  tracker.setDisplayFeatures(true);
318  tracker.setScanLineVisibilityTest(useScanline);
319 
321  vpImage<uint16_t> I_depth_raw;
322  vpImage<vpRGBa> I_depth;
323  vpHomogeneousMatrix cMo_truth;
324  std::vector<vpColVector> pointcloud;
325  int cpt_frame = 1;
326  if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
327  std::cerr << "Cannot read first frame!" << std::endl;
328  return EXIT_FAILURE;
329  }
330 
331  vpImage<bool> mask(I.getHeight(), I.getWidth());
332  const double roi_step = 7.0;
333  const double roi_step2 = 6.0;
334  if (use_mask) {
335  mask = false;
336  for (unsigned int i = (unsigned int) (I.getRows()/roi_step); i < (unsigned int) (I.getRows()*roi_step2/roi_step); i++) {
337  for (unsigned int j = (unsigned int) (I.getCols()/roi_step); j < (unsigned int) (I.getCols()*roi_step2/roi_step); j++) {
338  mask[i][j] = true;
339  }
340  }
341  tracker.setMask(mask);
342  }
343 
344  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
345  if (opt_display) {
346 #ifdef VISP_HAVE_DISPLAY
347  display1.init(I, 0, 0, "Image");
348  display2.init(I_depth, I.getWidth(), 0, "Depth");
349 #endif
350  }
351 
352  vpHomogeneousMatrix depth_M_color;
353  depth_M_color[0][3] = -0.05;
354  tracker.initFromPose(I, depth_M_color*cMo_truth);
355 
356  bool click = false, quit = false;
357  std::vector<double> vec_err_t, vec_err_tu;
358  std::vector<double> time_vec;
359  while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit
360  && (opt_lastFrame > 0 ? (int)cpt_frame <= opt_lastFrame : true)) {
361  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
362 
363  if (opt_display) {
365  vpDisplay::display(I_depth);
366  }
367 
368  double t = vpTime::measureTimeMs();
369  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
370  std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
371  mapOfPointclouds["Camera"] = &pointcloud;
372  std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
373  mapOfWidths["Camera"] = I_depth.getWidth();
374  mapOfHeights["Camera"] = I_depth.getHeight();
375 
376  tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
377  vpHomogeneousMatrix cMo = tracker.getPose();
378  t = vpTime::measureTimeMs() - t;
379  time_vec.push_back(t);
380 
381  if (opt_display) {
382  tracker.display(I_depth, cMo, cam_depth, vpColor::red, 3);
383  vpDisplay::displayFrame(I_depth, cMo, cam_depth, 0.05, vpColor::none, 3);
384 
385  std::stringstream ss;
386  ss << "Frame: " << cpt_frame;
387  vpDisplay::displayText(I_depth, 20, 20, ss.str(), vpColor::red);
388  ss.str("");
389  ss << "Nb features: " << tracker.getError().getRows();
390  vpDisplay::displayText(I_depth, 40, 20, ss.str(), vpColor::red);
391  }
392 
393  vpPoseVector pose_est(cMo);
394  vpPoseVector pose_truth(depth_M_color*cMo_truth);
395  vpColVector t_est(3), t_truth(3);
396  vpColVector tu_est(3), tu_truth(3);
397  for (int i = 0; i < 3; i++) {
398  t_est[i] = pose_est[i];
399  t_truth[i] = pose_truth[i];
400  tu_est[i] = pose_est[i+3];
401  tu_truth[i] = pose_truth[i+3];
402  }
403 
404  vpColVector t_err = t_truth-t_est, tu_err = tu_truth-tu_est;
405  double t_err2 = sqrt(t_err.sumSquare()), tu_err2 = vpMath::deg(sqrt(tu_err.sumSquare()));
406  vec_err_t.push_back( t_err2 );
407  vec_err_tu.push_back( tu_err2 );
408  const double t_thresh = useScanline ? 0.003 : 0.002;
409  const double tu_thresh = useScanline ? 0.5 : 0.4;
410  if ( !use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh) ) { //no accuracy test with mask
411  std::cerr << "Pose estimated exceeds the threshold (t_thresh = 0.003, tu_thresh = 0.5)!" << std::endl;
412  std::cout << "t_err: " << sqrt(t_err.sumSquare()) << " ; tu_err: " << vpMath::deg(sqrt(tu_err.sumSquare())) << std::endl;
413  return EXIT_FAILURE;
414  }
415 
416  if (opt_display) {
417  if (use_mask) {
418  vpRect roi(vpImagePoint(I.getRows()/roi_step, I.getCols()/roi_step),
419  vpImagePoint(I.getRows()*roi_step2/roi_step, I.getCols()*roi_step2/roi_step));
421  }
422 
423  vpDisplay::flush(I);
424  vpDisplay::flush(I_depth);
425  }
426 
427  if (opt_display && opt_click_allowed) {
429  if (vpDisplay::getClick(I, button, click)) {
430  switch (button) {
432  quit = !click;
433  break;
434 
436  click = !click;
437  break;
438 
439  default:
440  break;
441  }
442  }
443  }
444 
445  cpt_frame++;
446  }
447 
448  if (!time_vec.empty())
449  std::cout << "Computation time, Mean: " << vpMath::getMean(time_vec) << " ms ; Median: " << vpMath::getMedian(time_vec)
450  << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms" << std::endl;
451 
452  if (!vec_err_t.empty())
453  std::cout << "Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
454 
455  if (!vec_err_tu.empty())
456  std::cout << "Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
457 
458 #if defined(VISP_HAVE_XML2)
459  // Cleanup memory allocated by xml library used to parse the xml config
460  // file in vpMbGenericTracker::loadConfigFile()
462 #endif
463 
464  return EXIT_SUCCESS;
465  } catch (const vpException &e) {
466  std::cout << "Catch an exception: " << e << std::endl;
467  return EXIT_FAILURE;
468  }
469 }
470 #else
471 int main() {
472  std::cout << "Enable MBT module (VISP_HAVE_MODULE_MBT) to launch this test." << std::endl;
473  return 0;
474 }
475 #endif
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:468
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1317
Implementation of an homogeneous matrix and operations on such kind of matrices.
unsigned int getRows() const
Definition: vpImage.h:211
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:1865
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:222
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
static const vpColor none
Definition: vpColor.h:192
error that can be emited by ViSP classes.
Definition: vpException.h:71
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Real-time 6D object pose tracking using its CAD model.
static void flush(const vpImage< unsigned char > &I)
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:676
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:1542
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:169
Generic class defining intrinsic camera parameters.
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
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)
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
static double deg(double rad)
Definition: vpMath.h:95
unsigned int getHeight() const
Definition: vpImage.h:178
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))
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
double sumSquare() const
Defines a rectangle in the plane.
Definition: vpRect.h:78
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static const vpColor yellow
Definition: vpColor.h:188
unsigned int getWidth() const
Definition: vpImage.h:239
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)