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