44#include <visp3/core/vpConfig.h>
46#if (defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)) && \
47 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
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>
63#define GETOPTARGS "X:M:i:n:dchfolwvpT:e:u:"
66#define USE_SMALL_DATASET 1
70void usage(
const char *name,
const char *badparam)
73 Example of tracking with vpGenericTracker.\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",
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\
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\
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\
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\
107 Turn off the display of the visual features. \n\
110 Turn off the display.\n\
113 Disable the mouse click. Useful to automate the \n\
114 execution of this program without human intervention.\n\
117 Use Ogre3D for visibility tests\n\
120 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
123 Use the scanline for visibility tests.\n\
126 Compute covariance matrix.\n\
129 Compute gradient projection error.\n\
132 Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
134 -e <last frame index>\n\
135 Specify the index of the last frame. Once reached, the tracking is stopped.\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\
141 Print the help.\n\n");
144 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
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)
161 configFile_depth = optarg_;
164 modelFile_depth = optarg_;
170 displayFeatures =
false;
173 click_allowed =
false;
185 showOgreConfigDialog =
true;
188 computeCovariance =
true;
191 projectionError =
true;
194 tracker_type_depth = atoi(optarg_);
197 lastFrame = atoi(optarg_);
200 disable_castle_faces = atoi(optarg_);
204 usage(argv[0], NULL);
208 usage(argv[0], optarg_);
214 if ((c == 1) || (c == -1)) {
216 usage(argv[0], NULL);
217 std::cerr <<
"ERROR: " << std::endl;
218 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
225struct rs_intrinsics {
237void rs_deproject_pixel_to_point(
float point[3],
const rs_intrinsics &intrin,
const float pixel[2],
float depth)
239 float x = (pixel[0] - intrin.ppx) / intrin.fx;
240 float y = (pixel[1] - intrin.ppy) / intrin.fy;
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);
250 point[0] = depth * x;
251 point[1] = depth * y;
256 vpImage<uint16_t> &I_depth_raw, std::vector<vpColVector> &pointcloud,
unsigned int &pointcloud_width,
257 unsigned int &pointcloud_height)
259#if VISP_HAVE_DATASET_VERSION >= 0x030600
260 std::string ext(
"png");
262 std::string ext(
"pgm");
265 std::stringstream ss;
266 ss << input_directory <<
"/image_";
267 ss << std::setfill(
'0') << std::setw(4);
271 std::string filename_image = ss.str();
274 std::cerr <<
"Cannot read: " << filename_image << std::endl;
281 ss << input_directory <<
"/depth_image_";
282 ss << std::setfill(
'0') << std::setw(4);
285 std::string filename_depth = ss.str();
287 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
288 if (!file_depth.is_open()) {
292 unsigned int height = 0, width = 0;
296 I_depth_raw.
resize(height, width);
298 uint16_t depth_value = 0;
299 for (
unsigned int i = 0; i < height; i++) {
300 for (
unsigned int j = 0; j < width; j++) {
302 I_depth_raw[i][j] = depth_value;
307 pointcloud_width = width;
308 pointcloud_height = height;
309 pointcloud.resize((
size_t)width * height);
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;
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;
328 float pixel[2] = {(float)j, (
float)i};
329 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
332 data_3D[0] = point[0];
333 data_3D[1] = point[1];
334 data_3D[2] = point[2];
336 pointcloud[(size_t)(i * width + j)] = data_3D;
343void loadConfiguration(
vpMbTracker *
const tracker,
const std::string &
356 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
357 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
358 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
381std::vector<std::string> getCastleElementNames(
const int element)
383 std::vector<std::string> element_names;
386 element_names.push_back(
"floor");
388 element_names.push_back(
"front_door");
390 element_names.push_back(
"slope");
392 element_names.push_back(
"tower_front");
394 element_names.push_back(
"tower_left");
396 element_names.push_back(
"tower_right");
398 element_names.push_back(
"tower_back");
400 return element_names;
404int main(
int argc,
const char **argv)
407 std::string env_ipath;
408 std::string opt_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;
423#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
425 int opt_lastFrame = 5;
427 int opt_lastFrame = -1;
429 int disable_castle_faces = 0;
436 if (!env_ipath.empty())
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)) {
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
463 std::cerr <<
"ViSP-images does not contain the folder: " << dir_path <<
"!" << std::endl;
467 std::string configFile_depth;
468 if (!opt_configFile_depth.empty())
469 configFile_depth = opt_configFile_depth;
474 std::string modelFile_depth;
475 if (!opt_modelFile_depth.empty())
476 modelFile_depth = opt_modelFile_depth;
481 std::string vrml_ext =
".wrl";
483 (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
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;
489 std::cerr <<
"Error: vrml model file is only supported if ViSP is "
490 "build with Coin3D 3rd party"
496 if (!opt_initFile.empty())
497 initFile = opt_initFile;
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;
513#if defined(VISP_HAVE_X11)
515#elif defined(VISP_HAVE_GDI)
517#elif defined(HAVE_OPENCV_HIGHGUI)
519#elif defined(VISP_HAVE_D3D9)
521#elif defined(VISP_HAVE_GTK)
527#if defined(VISP_HAVE_DISPLAY)
529 display.init(I_depth, 100, 100,
"Depth");
531 display2.
init(I, I_depth.
getWidth() + 100, 100,
"Image");
544 loadConfiguration(tracker, configFile_depth);
574 std::string depth_M_color_filename =
577 std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
578 depth_M_color.
load(depth_M_color_file);
582 if (opt_display && opt_click_allowed) {
593 if (opt_display && opt_click_allowed) {
600 vpHomogeneousMatrix cMoi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
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;
613 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
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)) {
632 std::stringstream ss;
633 ss <<
"Num frame: " << frame_index;
638 if (frame_index == 10) {
639 std::cout <<
"----------Test reset tracker----------" << std::endl;
646 loadConfiguration(tracker, configFile_depth);
658 if (frame_index == 20) {
659 cMo.
buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
661 if (frame_index == 50) {
662 cMo.
buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
664 std::cout <<
"Test set pose" << std::endl;
670 if (frame_index < 15 || frame_index >= 20) {
673 if (frame_index < 30 || frame_index >= 50) {
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;
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())
691 dynamic_cast<vpMbGenericTracker *
>(tracker)->setUseDepthDenseTracking(element_names[idx],
false);
693 dynamic_cast<vpMbGenericTracker *
>(tracker)->setUseDepthNormalTracking(element_names[idx],
false);
695 std::cout << std::endl;
699 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
701 time_vec.push_back(t);
711 std::stringstream ss;
712 ss <<
"Computation time: " << t <<
" ms";
722 ss <<
"Projection error: " << projection_error;
727 if (opt_click_allowed && opt_display) {
746 if (computeCovariance) {
747 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
750 if (projectionError) {
751 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
762 std::cout <<
"\nFinal poses, cMo:\n" << cMo << std::endl;
767 if (opt_click_allowed && !quit) {
776 std::cout <<
"Catch an exception: " << e << std::endl;
781#elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
784 std::cout <<
"Cannot run this example: visp_mbt, visp_gui modules are required." << std::endl;
790 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
unsigned int getRows() const
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 darkRed
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...
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.
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.
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
static double rad(double deg)
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static double getMean(const std::vector< double > &v)
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
@ ROBUST_FEATURE_ESTIMATION
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
VISP_EXPORT double measureTimeMs()