Visual Servoing Platform version 3.5.0
mbtGenericTrackingDepthOnly.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 * Example of tracking with vpGenericTracker on Castel.
33 *
34 *****************************************************************************/
35
42#include <cstdlib>
43#include <iostream>
44#include <visp3/core/vpConfig.h>
45
46#if (defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)) \
47 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
48
49#include <visp3/core/vpDebug.h>
50#include <visp3/core/vpHomogeneousMatrix.h>
51#include <visp3/core/vpIoTools.h>
52#include <visp3/core/vpMath.h>
53#include <visp3/gui/vpDisplayD3D.h>
54#include <visp3/gui/vpDisplayGDI.h>
55#include <visp3/gui/vpDisplayGTK.h>
56#include <visp3/gui/vpDisplayOpenCV.h>
57#include <visp3/gui/vpDisplayX.h>
58#include <visp3/io/vpImageIo.h>
59#include <visp3/io/vpParseArgv.h>
60#include <visp3/io/vpVideoReader.h>
61#include <visp3/mbt/vpMbGenericTracker.h>
62
63#define GETOPTARGS "X:M:i:n:dchfolwvpT:e:u:"
64
65#define USE_XML 1
66#define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images
67
68namespace
69{
70void usage(const char *name, const char *badparam)
71{
72 fprintf(stdout, "\n\
73 Example of tracking with vpGenericTracker.\n\
74 \n\
75 SYNOPSIS\n\
76 %s [-i <test image path>] [-X <config file depth>]\n\
77 [-M <model name depth>] [-n <initialisation file base name>]\n\
78 [-f] [-c] [-d] [-h] [-o] [-w] [-l] [-v] [-p]\n\
79 [-T <tracker type>] [-e <last frame index>]\n\
80 [-u <disable face>]\n", name);
81
82 fprintf(stdout, "\n\
83 OPTIONS: \n\
84 -i <input image path> \n\
85 Set image input path.\n\
86 These images come from ViSP-images-x.y.z.tar.gz available \n\
87 on the ViSP website.\n\
88 Setting the VISP_INPUT_IMAGE_PATH environment\n\
89 variable produces the same behavior than using\n\
90 this option.\n\
91 \n\
92 -X <config file> \n\
93 Set the config file (the xml file) to use for the depth sensor.\n\
94 The config file is used to specify the parameters of the tracker.\n\
95 \n\
96 -M <model name> \n\
97 Specify the name of the file of the model for the depth sensor.\n\
98 The model can either be a vrml model (.wrl) or a .cao file.\n\
99 \n\
100 -n <initialisation file base name> \n\
101 Base name of the initialisation file. The file will be 'base_name'.init .\n\
102 This base name is also used for the optional picture specifying where to \n\
103 click (a .ppm picture).\n\
104 \n\
105 -f \n\
106 Turn off the display of the visual features. \n\
107 \n\
108 -d \n\
109 Turn off the display.\n\
110 \n\
111 -c\n\
112 Disable the mouse click. Useful to automate the \n\
113 execution of this program without human intervention.\n\
114 \n\
115 -o\n\
116 Use Ogre3D for visibility tests\n\
117 \n\
118 -w\n\
119 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
120 \n\
121 -l\n\
122 Use the scanline for visibility tests.\n\
123 \n\
124 -v\n\
125 Compute covariance matrix.\n\
126 \n\
127 -p\n\
128 Compute gradient projection error.\n\
129 \n\
130 -T <tracker type>\n\
131 Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
132 \n\
133 -e <last frame index>\n\
134 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
135 \n\
136 -u <disable>\n\
137 Disable castle element (1=floor, 2=front_door, 4=slope, 8=tower_front, 16=tower_left, 32=tower_right, 64=tower_back).\n\
138 \n\
139 -h \n\
140 Print the help.\n\n");
141
142 if (badparam)
143 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
144}
145
146bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile_depth,
147 std::string &modelFile_depth, std::string &initFile, bool &displayFeatures, bool &click_allowed,
148 bool &display, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance,
149 bool &projectionError, int &tracker_type_depth, int &lastFrame, int &disable_castle_faces)
150{
151 const char *optarg_;
152 int c;
153 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
154
155 switch (c) {
156 case 'i':
157 ipath = optarg_;
158 break;
159 case 'X':
160 configFile_depth = optarg_;
161 break;
162 case 'M':
163 modelFile_depth = optarg_;
164 break;
165 case 'n':
166 initFile = optarg_;
167 break;
168 case 'f':
169 displayFeatures = false;
170 break;
171 case 'c':
172 click_allowed = false;
173 break;
174 case 'd':
175 display = false;
176 break;
177 case 'o':
178 useOgre = true;
179 break;
180 case 'l':
181 useScanline = true;
182 break;
183 case 'w':
184 showOgreConfigDialog = true;
185 break;
186 case 'v':
187 computeCovariance = true;
188 break;
189 case 'p':
190 projectionError = true;
191 break;
192 case 'T':
193 tracker_type_depth = atoi(optarg_);
194 break;
195 case 'e':
196 lastFrame = atoi(optarg_);
197 break;
198 case 'u':
199 disable_castle_faces = atoi(optarg_);
200 break;
201
202 case 'h':
203 usage(argv[0], NULL);
204 return false;
205 break;
206 default:
207 usage(argv[0], optarg_);
208 return false;
209 break;
210 }
211 }
212
213 if ((c == 1) || (c == -1)) {
214 // standalone param or error
215 usage(argv[0], NULL);
216 std::cerr << "ERROR: " << std::endl;
217 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
218 return false;
219 }
220
221 return true;
222}
223
224struct rs_intrinsics {
225 float ppx;
227 float ppy;
229 float fx;
231 float fy;
233 float coeffs[5];
234};
235
236void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth)
237{
238 float x = (pixel[0] - intrin.ppx) / intrin.fx;
239 float y = (pixel[1] - intrin.ppy) / intrin.fy;
240
241 float r2 = x * x + y * y;
242 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
243 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
244 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
245
246 x = ux;
247 y = uy;
248
249 point[0] = depth * x;
250 point[1] = depth * y;
251 point[2] = depth;
252}
253
254bool read_data(unsigned int cpt, const std::string &input_directory, vpImage<unsigned char> &I, vpImage<uint16_t> &I_depth_raw,
255 std::vector<vpColVector> &pointcloud, unsigned int &pointcloud_width, unsigned int &pointcloud_height)
256{
257 char buffer[256];
258
259 // Read image
260 std::stringstream ss;
261 ss << input_directory << "/image_%04d.pgm";
262 sprintf(buffer, ss.str().c_str(), cpt);
263 std::string filename_image = buffer;
264
265 if (!vpIoTools::checkFilename(filename_image)) {
266 std::cerr << "Cannot read: " << filename_image << std::endl;
267 return false;
268 }
269 vpImageIo::read(I, filename_image);
270
271 // Read raw depth
272 ss.str("");
273 ss << input_directory << "/depth_image_%04d.bin";
274 sprintf(buffer, ss.str().c_str(), cpt);
275 std::string filename_depth = buffer;
276
277 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
278 if (!file_depth.is_open()) {
279 return false;
280 }
281
282 unsigned int height = 0, width = 0;
283 vpIoTools::readBinaryValueLE(file_depth, height);
284 vpIoTools::readBinaryValueLE(file_depth, width);
285
286 I_depth_raw.resize(height, width);
287
288 uint16_t depth_value = 0;
289 for (unsigned int i = 0; i < height; i++) {
290 for (unsigned int j = 0; j < width; j++) {
291 vpIoTools::readBinaryValueLE(file_depth, depth_value);
292 I_depth_raw[i][j] = depth_value;
293 }
294 }
295
296 // Transform pointcloud
297 pointcloud_width = width;
298 pointcloud_height = height;
299 pointcloud.resize((size_t)width * height);
300
301 // Only for Creative SR300
302 const float depth_scale = 0.000124986647f;
303 rs_intrinsics depth_intrinsic;
304 depth_intrinsic.ppx = 311.484558f;
305 depth_intrinsic.ppy = 246.283234f;
306 depth_intrinsic.fx = 476.053619f;
307 depth_intrinsic.fy = 476.053497f;
308 depth_intrinsic.coeffs[0] = 0.165056542f;
309 depth_intrinsic.coeffs[1] = -0.0508309528f;
310 depth_intrinsic.coeffs[2] = 0.00435937941f;
311 depth_intrinsic.coeffs[3] = 0.00541406544f;
312 depth_intrinsic.coeffs[4] = 0.250085592f;
313
314 for (unsigned int i = 0; i < height; i++) {
315 for (unsigned int j = 0; j < width; j++) {
316 float scaled_depth = I_depth_raw[i][j] * depth_scale;
317 float point[3];
318 float pixel[2] = {(float)j, (float)i};
319 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
320
321 vpColVector data_3D(3);
322 data_3D[0] = point[0];
323 data_3D[1] = point[1];
324 data_3D[2] = point[2];
325
326 pointcloud[(size_t)(i * width + j)] = data_3D;
327 }
328 }
329
330 return true;
331}
332
333void loadConfiguration(vpMbTracker *const tracker, const std::string &
334#if USE_XML
335 configFile_depth
336#endif
337)
338{
339#if USE_XML
340 // From the xml file
341 dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(configFile_depth);
342#else
343 // Depth
344 dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalFeatureEstimationMethod(
346 dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
347 dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
348 dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
349 dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalSamplingStep(2, 2);
350
351 dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthDenseSamplingStep(4, 4);
352
354 cam.initPersProjWithoutDistortion(476.0536193848, 476.0534973145, 311.4845581055, 246.2832336426);
355
356 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam);
357
358 tracker->setAngleAppear(vpMath::rad(70));
359 tracker->setAngleDisappear(vpMath::rad(80));
360
361 // Specify the clipping to
362 tracker->setNearClippingDistance(0.01);
363 tracker->setFarClippingDistance(2.0);
365// tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING
366// | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
367// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
368#endif
369}
370
371std::vector<std::string> getCastleElementNames(const int element)
372{
373 std::vector<std::string> element_names;
374
375 if (element & 0x1)
376 element_names.push_back("floor");
377 if (element & 0x2)
378 element_names.push_back("front_door");
379 if (element & 0x4)
380 element_names.push_back("slope");
381 if (element & 0x8)
382 element_names.push_back("tower_front");
383 if (element & 0x10)
384 element_names.push_back("tower_left");
385 if (element & 0x20)
386 element_names.push_back("tower_right");
387 if (element & 0x40)
388 element_names.push_back("tower_back");
389
390 return element_names;
391}
392}
393
394int main(int argc, const char **argv)
395{
396 try {
397 std::string env_ipath;
398 std::string opt_ipath;
399 std::string ipath;
400 std::string opt_configFile_depth;
401 std::string opt_modelFile_depth;
402 std::string opt_initFile;
403 std::string initFile;
404 bool displayFeatures = true;
405 bool opt_click_allowed = true;
406 bool opt_display = true;
407 bool useOgre = false;
408 bool showOgreConfigDialog = false;
409 bool useScanline = false;
410 bool computeCovariance = false;
411 bool projectionError = false;
412 int trackerType_depth = vpMbGenericTracker::DEPTH_DENSE_TRACKER;
413#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
414 // To avoid Debian test timeout
415 int opt_lastFrame = 5;
416#else
417 int opt_lastFrame = -1;
418#endif
419 int disable_castle_faces = 0;
420
421 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
422 // environment variable value
424
425 // Set the default input path
426 if (!env_ipath.empty())
427 ipath = env_ipath;
428
429 // Read the command line options
430 if (!getOptions(argc, argv, opt_ipath, opt_configFile_depth, opt_modelFile_depth, opt_initFile, displayFeatures,
431 opt_click_allowed, opt_display, useOgre, showOgreConfigDialog, useScanline, computeCovariance,
432 projectionError, trackerType_depth, opt_lastFrame, disable_castle_faces)) {
433 return EXIT_FAILURE;
434 }
435
436 // Test if an input path is set
437 if (opt_ipath.empty() && env_ipath.empty()) {
438 usage(argv[0], NULL);
439 std::cerr << std::endl << "ERROR:" << std::endl;
440 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
441 << " environment variable to specify the location of the " << std::endl
442 << " image path where test images are located." << std::endl
443 << std::endl;
444
445 return EXIT_FAILURE;
446 }
447
448 // Get the option values
449 ipath = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/castel");
450
451 std::string dir_path = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth");
452 if (!vpIoTools::checkDirectory(dir_path)) {
453 std::cerr << "ViSP-images does not contain the folder: " << dir_path << "!" << std::endl;
454 return EXIT_SUCCESS;
455 }
456
457 std::string configFile_depth;
458 if (!opt_configFile_depth.empty())
459 configFile_depth = opt_configFile_depth;
460 else
461 configFile_depth =
462 vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau_depth.xml");
463
464 std::string modelFile_depth;
465 if (!opt_modelFile_depth.empty())
466 modelFile_depth = opt_modelFile_depth;
467 else
468 modelFile_depth =
469 vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.cao");
470
471 std::string vrml_ext = ".wrl";
472 bool use_vrml =
473 (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
474
475 if (use_vrml) {
476#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
477 std::cout << "use_vrml: " << use_vrml << std::endl;
478#else
479 std::cerr << "Error: vrml model file is only supported if ViSP is "
480 "build with Coin3D 3rd party"
481 << std::endl;
482 return EXIT_FAILURE;
483#endif
484 }
485
486 if (!opt_initFile.empty())
487 initFile = opt_initFile;
488 else
489 initFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.init");
490
491 vpImage<unsigned char> I, I_depth;
492 vpImage<uint16_t> I_depth_raw;
493 std::vector<vpColVector> pointcloud;
494 unsigned int pointcloud_width, pointcloud_height;
495 if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) {
496 std::cerr << "Cannot open sequence: " << ipath << std::endl;
497 return EXIT_FAILURE;
498 }
499
500 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
501
502// initialise a display
503#if defined VISP_HAVE_X11
504 vpDisplayX display, display2;
505#elif defined VISP_HAVE_GDI
506 vpDisplayGDI display, display2;
507#elif defined VISP_HAVE_OPENCV
508 vpDisplayOpenCV display, display2;
509#elif defined VISP_HAVE_D3D9
510 vpDisplayD3D display, display2;
511#elif defined VISP_HAVE_GTK
512 vpDisplayGTK display, display2;
513#else
514 opt_display = false;
515#endif
516 if (opt_display) {
517#if defined(VISP_HAVE_DISPLAY)
518 display.setDownScalingFactor(vpDisplay::SCALE_AUTO);
519 display.init(I_depth, 100, 100, "Depth");
521 display2.init(I, I_depth.getWidth()+100, 100, "Image");
522#endif
523 vpDisplay::display(I_depth);
525 vpDisplay::flush(I_depth);
527 }
528
529 // Object pointer to check that inheritance is ok
530 vpMbTracker *tracker = new vpMbGenericTracker(1, trackerType_depth);
533
534 loadConfiguration(tracker, configFile_depth);
535
536 // Display the moving edges, and the Klt points
537 tracker->setDisplayFeatures(displayFeatures);
538
539 // Tells if the tracker has to use Ogre3D for visibility tests
540 tracker->setOgreVisibilityTest(useOgre);
541 if (useOgre)
542 tracker->setOgreShowConfigDialog(showOgreConfigDialog);
543
544 // Tells if the tracker has to use the scanline visibility tests
545 tracker->setScanLineVisibilityTest(useScanline);
546
547 // Tells if the tracker has to compute the covariance matrix
548 tracker->setCovarianceComputation(computeCovariance);
549
550 // Tells if the tracker has to compute the projection error
551 tracker->setProjectionErrorComputation(projectionError);
552
553 // For generic projection error computation
554 tracker->setProjectionErrorDisplay(true);
557
558 // Retrieve the camera parameters from the tracker
559 dynamic_cast<vpMbGenericTracker *>(tracker)->getCameraParameters(cam);
560
561 vpCameraParameters cam_color;
562 cam_color.initPersProjWithoutDistortion(615.1674804688, 615.1675415039, 312.1889953613, 243.4373779297);
563 vpHomogeneousMatrix depth_M_color;
564 std::string depth_M_color_filename =
565 vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/depth_M_color.txt");
566 {
567 std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
568 depth_M_color.load(depth_M_color_file);
569 }
570
571 // Loop to position the object
572 if (opt_display && opt_click_allowed) {
573 while (!vpDisplay::getClick(I_depth, false)) {
574 vpDisplay::display(I_depth);
575 vpDisplay::displayText(I_depth, 15, 10, "click after positioning the object", vpColor::red);
576 vpDisplay::flush(I_depth);
577 }
578 }
579
580 // Load the 3D model (either a vrml file or a .cao file)
581 dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile_depth);
582
583 if (opt_display && opt_click_allowed) {
584 // Initialise the tracker by clicking on the image
585 dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(I_depth, initFile, true);
586 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(cMo);
587 // display the 3D model at the given pose
588 dynamic_cast<vpMbGenericTracker *>(tracker)->display(I_depth, cMo, cam, vpColor::red);
589 } else {
590 vpHomogeneousMatrix cMoi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
591 dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(I_depth, cMoi);
592 }
593
594 // track the model
595 {
596 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
597 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
598 mapOfPointclouds["Camera"] = &pointcloud;
599 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
600 mapOfWidths["Camera"] = pointcloud_width;
601 mapOfHeights["Camera"] = pointcloud_height;
602
603 dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
604 }
605 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(cMo);
606
607 if (opt_display) {
608 vpDisplay::flush(I_depth);
609 }
610
611 bool quit = false, click = false;
612 unsigned int frame_index = 0;
613 std::vector<double> time_vec;
614 while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
615 (opt_lastFrame > 0 ? (int)frame_index <= opt_lastFrame : true)) {
616 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
617
618 if (opt_display) {
619 vpDisplay::display(I_depth);
621
622 std::stringstream ss;
623 ss << "Num frame: " << frame_index;
624 vpDisplay::displayText(I_depth, 40, 20, ss.str(), vpColor::red);
625 }
626
627 // Test reset the tracker
628 if (frame_index == 10) {
629 std::cout << "----------Test reset tracker----------" << std::endl;
630 if (opt_display) {
631 vpDisplay::display(I_depth);
632 }
633
634 tracker->resetTracker();
635
636 loadConfiguration(tracker, configFile_depth);
637 dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile_depth);
638 dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam);
639 tracker->setOgreVisibilityTest(useOgre);
640 tracker->setScanLineVisibilityTest(useScanline);
641 tracker->setCovarianceComputation(computeCovariance);
642 tracker->setProjectionErrorComputation(projectionError);
643 dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(I_depth, cMo);
644 }
645
646 // Test set an initial pose
647#if USE_SMALL_DATASET
648 if (frame_index == 20) {
649 cMo.buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
650#else
651 if (frame_index == 50) {
652 cMo.buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
653#endif
654 std::cout << "Test set pose" << std::endl;
655 dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(I_depth, cMo);
656 }
657
658#if USE_SMALL_DATASET
659 // track the object: stop tracking from frame 15 to 20
660 if (frame_index < 15 || frame_index >= 20) {
661#else
662 // track the object: stop tracking from frame 30 to 50
663 if (frame_index < 30 || frame_index >= 50) {
664#endif
665 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
666 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
667 mapOfPointclouds["Camera"] = &pointcloud;
668 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
669 mapOfWidths["Camera"] = pointcloud_width;
670 mapOfHeights["Camera"] = pointcloud_height;
671
672 if (disable_castle_faces) {
673 std::vector<std::string> element_names = getCastleElementNames(disable_castle_faces);
674 std::cout << "Disable: ";
675 for (size_t idx = 0; idx < element_names.size(); idx++) {
676 std::cout << element_names[idx];
677 if (idx + 1 < element_names.size())
678 std::cout << ", ";
679
680 if (trackerType_depth & vpMbGenericTracker::DEPTH_DENSE_TRACKER)
681 dynamic_cast<vpMbGenericTracker *>(tracker)->setUseDepthDenseTracking(element_names[idx], false);
682 if (trackerType_depth & vpMbGenericTracker::DEPTH_NORMAL_TRACKER)
683 dynamic_cast<vpMbGenericTracker *>(tracker)->setUseDepthNormalTracking(element_names[idx], false);
684 }
685 std::cout << std::endl;
686 }
687
688 double t = vpTime::measureTimeMs();
689 dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
690 t = vpTime::measureTimeMs() - t;
691 time_vec.push_back(t);
692
693 dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(cMo);
694
695 if (opt_display) {
696 // display the 3D model
697 dynamic_cast<vpMbGenericTracker *>(tracker)->display(I_depth, cMo, cam, vpColor::darkRed);
698 // display the frame
699 vpDisplay::displayFrame(I_depth, cMo, cam, 0.05);
700 // computation time
701 std::stringstream ss;
702 ss << "Computation time: " << t << " ms";
703 vpDisplay::displayText(I_depth, 60, 20, ss.str(), vpColor::red);
704 // nb features
705 ss.str("");
706 ss << "nb features: " << tracker->getError().getRows();
707 vpDisplay::displayText(I_depth, 80, 20, ss.str(), vpColor::red);
708
709 // generic projection error computed on image from the RGB camera
710 double projection_error = tracker->computeCurrentProjectionError(I, depth_M_color.inverse()*cMo, cam_color);
711 ss.str("");
712 ss << "Projection error: " << projection_error;
713 vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
714 }
715 }
716
717 if (opt_click_allowed && opt_display) {
718 vpDisplay::displayText(I_depth, 10, 10, "Click to quit", vpColor::red);
720 if (vpDisplay::getClick(I_depth, button, click)) {
721 switch (button) {
723 quit = !click;
724 break;
725
727 click = !click;
728 break;
729
730 default:
731 break;
732 }
733 }
734 }
735
736 if (computeCovariance) {
737 std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
738 }
739
740 if (projectionError) {
741 std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl;
742 }
743
744 if (opt_display) {
745 vpDisplay::flush(I_depth);
747 }
748
749 frame_index++;
750 }
751
752 std::cout << "\nFinal poses, cMo:\n" << cMo << std::endl;
753 std::cout << "\nComputation time, Mean: " << vpMath::getMean(time_vec)
754 << " ms ; Median: " << vpMath::getMedian(time_vec) << " ms ; Std: " << vpMath::getStdev(time_vec) << " ms"
755 << std::endl;
756
757 if (opt_click_allowed && !quit) {
758 vpDisplay::getClick(I_depth);
759 }
760
761 delete tracker;
762 tracker = NULL;
763
764 return EXIT_SUCCESS;
765 } catch (const vpException &e) {
766 std::cout << "Catch an exception: " << e << std::endl;
767 return EXIT_FAILURE;
768 }
769}
770
771#elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
772int main()
773{
774 std::cout << "Cannot run this example: visp_mbt, visp_gui modules are required."
775 << std::endl;
776 return EXIT_SUCCESS;
777}
778#else
779int main()
780{
781 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
782 return EXIT_SUCCESS;
783}
784#endif
unsigned int getRows() const
Definition: vpArray2D.h:289
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
static const vpColor red
Definition: vpColor.h:217
static const vpColor darkRed
Definition: vpColor.h:218
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)
virtual void setDownScalingFactor(unsigned int scale)
Definition: vpDisplay.cpp:231
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))
@ SCALE_AUTO
Definition: vpDisplay.h:183
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.
void load(std::ifstream &f)
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
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
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
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
Real-time 6D object pose tracking using its CAD model.
Main methods for a model-based tracker.
Definition: vpMbTracker.h:105
virtual void setProjectionErrorDisplayArrowLength(unsigned int length)
Definition: vpMbTracker.h:594
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam)
virtual void resetTracker()=0
virtual void setOgreShowConfigDialog(bool showConfigDialog)
Definition: vpMbTracker.h:643
virtual void setDisplayFeatures(bool displayF)
Definition: vpMbTracker.h:517
virtual vpColVector getError() const =0
virtual void setAngleDisappear(const double &a)
Definition: vpMbTracker.h:480
virtual void setCovarianceComputation(const bool &flag)
Definition: vpMbTracker.h:499
virtual void setScanLineVisibilityTest(const bool &v)
Definition: vpMbTracker.h:601
virtual void setOgreVisibilityTest(const bool &v)
virtual vpMatrix getCovarianceMatrix() const
Definition: vpMbTracker.h:265
virtual void setProjectionErrorDisplay(bool display)
Definition: vpMbTracker.h:589
virtual void setNearClippingDistance(const double &dist)
virtual void setFarClippingDistance(const double &dist)
virtual double getProjectionError() const
Definition: vpMbTracker.h:310
virtual void setProjectionErrorComputation(const bool &flag)
Definition: vpMbTracker.h:584
virtual void setClipping(const unsigned int &flags)
virtual void setProjectionErrorDisplayArrowThickness(unsigned int thickness)
Definition: vpMbTracker.h:599
virtual void setAngleAppear(const double &a)
Definition: vpMbTracker.h:469
virtual unsigned int getClipping() const
Definition: vpMbTracker.h:256
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
VISP_EXPORT double measureTimeMs()