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