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