44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/gui/vpDisplayD3D.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/gui/vpDisplayGTK.h>
55 #include <visp3/gui/vpDisplayOpenCV.h>
56 #include <visp3/gui/vpDisplayX.h>
57 #include <visp3/io/vpImageIo.h>
58 #include <visp3/io/vpParseArgv.h>
59 #include <visp3/io/vpVideoReader.h>
60 #include <visp3/mbt/vpMbGenericTracker.h>
62 #define GETOPTARGS "X:M:i:n:dchfolwvpT:e:u:"
65 #define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images
69 void usage(
const char *name,
const char *badparam)
72 Example of tracking with vpGenericTracker.\n\
75 %s [-i <test image path>] [-X <config file depth>]\n\
76 [-M <model name depth>] [-n <initialisation file base name>]\n\
77 [-f] [-c] [-d] [-h] [-o] [-w] [-l] [-v] [-p]\n\
78 [-T <tracker type>] [-e <last frame index>]\n\
79 [-u <disable face>]\n", name);
83 -i <input image path> \n\
84 Set image input path.\n\
85 These images come from ViSP-images-x.y.z.tar.gz available \n\
86 on the ViSP website.\n\
87 Setting the VISP_INPUT_IMAGE_PATH environment\n\
88 variable produces the same behavior than using\n\
92 Set the config file (the xml file) to use for the depth sensor.\n\
93 The config file is used to specify the parameters of the tracker.\n\
96 Specify the name of the file of the model for the depth sensor.\n\
97 The model can either be a vrml model (.wrl) or a .cao file.\n\
99 -n <initialisation file base name> \n\
100 Base name of the initialisation file. The file will be 'base_name'.init .\n\
101 This base name is also used for the optional picture specifying where to \n\
102 click (a .ppm picture).\n\
105 Turn off the display of the visual features. \n\
108 Turn off the display.\n\
111 Disable the mouse click. Useful to automate the \n\
112 execution of this program without human intervention.\n\
115 Use Ogre3D for visibility tests\n\
118 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
121 Use the scanline for visibility tests.\n\
124 Compute covariance matrix.\n\
127 Compute gradient projection error.\n\
130 Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
132 -e <last frame index>\n\
133 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
136 Disable castle element (1=floor, 2=front_door, 4=slope, 8=tower_front, 16=tower_left, 32=tower_right, 64=tower_back).\n\
139 Print the help.\n\n");
142 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
145 bool getOptions(
int argc,
const char **argv, std::string &ipath, std::string &configFile_depth,
146 std::string &modelFile_depth, std::string &initFile,
bool &displayFeatures,
bool &click_allowed,
147 bool &display,
bool &useOgre,
bool &showOgreConfigDialog,
bool &useScanline,
bool &computeCovariance,
148 bool &projectionError,
int &tracker_type_depth,
int &lastFrame,
int &disable_castle_faces)
159 configFile_depth = optarg_;
162 modelFile_depth = optarg_;
168 displayFeatures =
false;
171 click_allowed =
false;
183 showOgreConfigDialog =
true;
186 computeCovariance =
true;
189 projectionError =
true;
192 tracker_type_depth = atoi(optarg_);
195 lastFrame = atoi(optarg_);
198 disable_castle_faces = atoi(optarg_);
202 usage(argv[0], NULL);
206 usage(argv[0], optarg_);
212 if ((c == 1) || (c == -1)) {
214 usage(argv[0], NULL);
215 std::cerr <<
"ERROR: " << std::endl;
216 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
223 struct rs_intrinsics {
235 void rs_deproject_pixel_to_point(
float point[3],
const rs_intrinsics &intrin,
const float pixel[2],
float depth)
237 float x = (pixel[0] - intrin.ppx) / intrin.fx;
238 float y = (pixel[1] - intrin.ppy) / intrin.fy;
240 float r2 = x * x + y * y;
241 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
242 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
243 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
248 point[0] = depth * x;
249 point[1] = depth * y;
254 std::vector<vpColVector> &pointcloud,
unsigned int &pointcloud_width,
unsigned int &pointcloud_height)
259 std::stringstream ss;
260 ss << input_directory <<
"/image_%04d.pgm";
261 sprintf(buffer, ss.str().c_str(), cpt);
262 std::string filename_image = buffer;
265 std::cerr <<
"Cannot read: " << filename_image << std::endl;
272 ss << input_directory <<
"/depth_image_%04d.bin";
273 sprintf(buffer, ss.str().c_str(), cpt);
274 std::string filename_depth = buffer;
276 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
277 if (!file_depth.is_open()) {
281 unsigned int height = 0, width = 0;
285 I_depth_raw.
resize(height, width);
287 uint16_t depth_value = 0;
288 for (
unsigned int i = 0; i < height; i++) {
289 for (
unsigned int j = 0; j < width; j++) {
291 I_depth_raw[i][j] = depth_value;
296 pointcloud_width = width;
297 pointcloud_height = height;
298 pointcloud.
resize((
size_t)width * height);
301 const float depth_scale = 0.000124986647f;
302 rs_intrinsics depth_intrinsic;
303 depth_intrinsic.ppx = 311.484558f;
304 depth_intrinsic.ppy = 246.283234f;
305 depth_intrinsic.fx = 476.053619f;
306 depth_intrinsic.fy = 476.053497f;
307 depth_intrinsic.coeffs[0] = 0.165056542f;
308 depth_intrinsic.coeffs[1] = -0.0508309528f;
309 depth_intrinsic.coeffs[2] = 0.00435937941f;
310 depth_intrinsic.coeffs[3] = 0.00541406544f;
311 depth_intrinsic.coeffs[4] = 0.250085592f;
313 for (
unsigned int i = 0; i < height; i++) {
314 for (
unsigned int j = 0; j < width; j++) {
315 float scaled_depth = I_depth_raw[i][j] * depth_scale;
317 float pixel[2] = {(float)j, (
float)i};
318 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
321 data_3D[0] = point[0];
322 data_3D[1] = point[1];
323 data_3D[2] = point[2];
325 pointcloud[(size_t)(i * width + j)] = data_3D;
332 void loadConfiguration(
vpMbTracker *
const tracker,
const std::string &
333 #
if defined(VISP_HAVE_PUGIXML) && USE_XML
338 #if defined(VISP_HAVE_PUGIXML) && USE_XML
345 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
346 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
347 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
370 std::vector<std::string> getCastleElementNames(
const int element)
372 std::vector<std::string> element_names;
375 element_names.push_back(
"floor");
377 element_names.push_back(
"front_door");
379 element_names.push_back(
"slope");
381 element_names.push_back(
"tower_front");
383 element_names.push_back(
"tower_left");
385 element_names.push_back(
"tower_right");
387 element_names.push_back(
"tower_back");
389 return element_names;
393 int main(
int argc,
const char **argv)
396 std::string env_ipath;
397 std::string opt_ipath;
399 std::string opt_configFile_depth;
400 std::string opt_modelFile_depth;
401 std::string opt_initFile;
402 std::string initFile;
403 bool displayFeatures =
true;
404 bool opt_click_allowed =
true;
405 bool opt_display =
true;
406 bool useOgre =
false;
407 bool showOgreConfigDialog =
false;
408 bool useScanline =
false;
409 bool computeCovariance =
false;
410 bool projectionError =
false;
412 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
414 int opt_lastFrame = 5;
416 int opt_lastFrame = -1;
418 int disable_castle_faces = 0;
425 if (!env_ipath.empty())
429 if (!getOptions(argc, argv, opt_ipath, opt_configFile_depth, opt_modelFile_depth, opt_initFile, displayFeatures,
430 opt_click_allowed, opt_display, useOgre, showOgreConfigDialog, useScanline, computeCovariance,
431 projectionError, trackerType_depth, opt_lastFrame, disable_castle_faces)) {
436 if (opt_ipath.empty() && env_ipath.empty()) {
437 usage(argv[0], NULL);
438 std::cerr << std::endl <<
"ERROR:" << std::endl;
439 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
440 <<
" environment variable to specify the location of the " << std::endl
441 <<
" image path where test images are located." << std::endl
452 std::cerr <<
"ViSP-images does not contain the folder: " << dir_path <<
"!" << std::endl;
456 std::string configFile_depth;
457 if (!opt_configFile_depth.empty())
458 configFile_depth = opt_configFile_depth;
463 std::string modelFile_depth;
464 if (!opt_modelFile_depth.empty())
465 modelFile_depth = opt_modelFile_depth;
470 std::string vrml_ext =
".wrl";
472 (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
475 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
476 std::cout <<
"use_vrml: " << use_vrml << std::endl;
478 std::cerr <<
"Error: vrml model file is only supported if ViSP is "
479 "build with Coin3D 3rd party"
485 if (!opt_initFile.empty())
486 initFile = opt_initFile;
492 std::vector<vpColVector> pointcloud;
493 unsigned int pointcloud_width, pointcloud_height;
494 if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) {
495 std::cerr <<
"Cannot open sequence: " << ipath << std::endl;
502 #if defined VISP_HAVE_X11
504 #elif defined VISP_HAVE_GDI
506 #elif defined VISP_HAVE_OPENCV
508 #elif defined VISP_HAVE_D3D9
510 #elif defined VISP_HAVE_GTK
516 #if defined(VISP_HAVE_DISPLAY)
518 display.init(I_depth, 100, 100,
"Depth");
533 loadConfiguration(tracker, configFile_depth);
563 std::string depth_M_color_filename =
566 std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
567 depth_M_color.
load(depth_M_color_file);
571 if (opt_display && opt_click_allowed) {
582 if (opt_display && opt_click_allowed) {
589 vpHomogeneousMatrix cMoi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
595 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
596 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
597 mapOfPointclouds[
"Camera"] = &pointcloud;
598 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
599 mapOfWidths[
"Camera"] = pointcloud_width;
600 mapOfHeights[
"Camera"] = pointcloud_height;
602 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
610 bool quit =
false, click =
false;
611 unsigned int frame_index = 0;
612 std::vector<double> time_vec;
613 while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
614 (opt_lastFrame > 0 ? (
int)frame_index <= opt_lastFrame :
true)) {
621 std::stringstream ss;
622 ss <<
"Num frame: " << frame_index;
627 if (frame_index == 10) {
628 std::cout <<
"----------Test reset tracker----------" << std::endl;
635 loadConfiguration(tracker, configFile_depth);
646 #if USE_SMALL_DATASET
647 if (frame_index == 20) {
648 cMo.
buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
650 if (frame_index == 50) {
651 cMo.
buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
653 std::cout <<
"Test set pose" << std::endl;
657 #if USE_SMALL_DATASET
659 if (frame_index < 15 || frame_index >= 20) {
662 if (frame_index < 30 || frame_index >= 50) {
664 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
665 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
666 mapOfPointclouds[
"Camera"] = &pointcloud;
667 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
668 mapOfWidths[
"Camera"] = pointcloud_width;
669 mapOfHeights[
"Camera"] = pointcloud_height;
671 if (disable_castle_faces) {
672 std::vector<std::string> element_names = getCastleElementNames(disable_castle_faces);
673 std::cout <<
"Disable: ";
674 for (
size_t idx = 0; idx < element_names.size(); idx++) {
675 std::cout << element_names[idx];
676 if (idx + 1 < element_names.size())
680 dynamic_cast<vpMbGenericTracker *
>(tracker)->setUseDepthDenseTracking(element_names[idx],
false);
682 dynamic_cast<vpMbGenericTracker *
>(tracker)->setUseDepthNormalTracking(element_names[idx],
false);
684 std::cout << std::endl;
688 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
690 time_vec.push_back(t);
700 std::stringstream ss;
701 ss <<
"Computation time: " << t <<
" ms";
711 ss <<
"Projection error: " << projection_error;
716 if (opt_click_allowed && opt_display) {
735 if (computeCovariance) {
736 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
739 if (projectionError) {
740 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
751 std::cout <<
"\nFinal poses, cMo:\n" << cMo << std::endl;
756 if (opt_click_allowed && !quit) {
763 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION >= 2)
772 std::cout <<
"Catch an exception: " << e << std::endl;
780 std::cerr <<
"visp_mbt, visp_gui modules and OpenCV are required to run "