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:"
65 #define USE_SMALL_DATASET 1
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",
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\
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\
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\
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\
106 Turn off the display of the visual features. \n\
109 Turn off the display.\n\
112 Disable the mouse click. Useful to automate the \n\
113 execution of this program without human intervention.\n\
116 Use Ogre3D for visibility tests\n\
119 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
122 Use the scanline for visibility tests.\n\
125 Compute covariance matrix.\n\
128 Compute gradient projection error.\n\
131 Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
133 -e <last frame index>\n\
134 Specify the index of the last frame. Once reached, the tracking is stopped.\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\
140 Print the help.\n\n");
143 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
146 bool 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)
160 configFile_depth = optarg_;
163 modelFile_depth = optarg_;
169 displayFeatures =
false;
172 click_allowed =
false;
184 showOgreConfigDialog =
true;
187 computeCovariance =
true;
190 projectionError =
true;
193 tracker_type_depth = atoi(optarg_);
196 lastFrame = atoi(optarg_);
199 disable_castle_faces = atoi(optarg_);
203 usage(argv[0],
nullptr);
207 usage(argv[0], optarg_);
213 if ((c == 1) || (c == -1)) {
215 usage(argv[0],
nullptr);
216 std::cerr <<
"ERROR: " << std::endl;
217 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
224 struct vpRealsenseIntrinsics_t
237 void rs_deproject_pixel_to_point(
float point[3],
const vpRealsenseIntrinsics_t &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 vpRealsenseIntrinsics_t 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;
343 void loadConfiguration(
vpMbTracker *
const tracker,
const std::string &
344 #
if defined(VISP_HAVE_PUGIXML)
349 #if defined(VISP_HAVE_PUGIXML)
356 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
357 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
358 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
381 std::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;
404 int 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],
nullptr);
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) {
601 vpHomogeneousMatrix cMoi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
607 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
608 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
609 mapOfPointclouds[
"Camera"] = &pointcloud;
610 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
611 mapOfWidths[
"Camera"] = pointcloud_width;
612 mapOfHeights[
"Camera"] = pointcloud_height;
614 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
622 bool quit =
false, click =
false;
623 unsigned int frame_index = 0;
624 std::vector<double> time_vec;
625 while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
626 (opt_lastFrame > 0 ? (
int)frame_index <= opt_lastFrame :
true)) {
633 std::stringstream ss;
634 ss <<
"Num frame: " << frame_index;
639 if (frame_index == 10) {
640 std::cout <<
"----------Test reset tracker----------" << std::endl;
647 loadConfiguration(tracker, configFile_depth);
658 #if USE_SMALL_DATASET
659 if (frame_index == 20) {
660 cMo.
buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
662 if (frame_index == 50) {
663 cMo.
buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
665 std::cout <<
"Test set pose" << std::endl;
669 #if USE_SMALL_DATASET
671 if (frame_index < 15 || frame_index >= 20) {
674 if (frame_index < 30 || frame_index >= 50) {
676 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
677 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
678 mapOfPointclouds[
"Camera"] = &pointcloud;
679 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
680 mapOfWidths[
"Camera"] = pointcloud_width;
681 mapOfHeights[
"Camera"] = pointcloud_height;
683 if (disable_castle_faces) {
684 std::vector<std::string> element_names = getCastleElementNames(disable_castle_faces);
685 std::cout <<
"Disable: ";
686 for (
size_t idx = 0; idx < element_names.size(); idx++) {
687 std::cout << element_names[idx];
688 if (idx + 1 < element_names.size())
692 dynamic_cast<vpMbGenericTracker *
>(tracker)->setUseDepthDenseTracking(element_names[idx],
false);
694 dynamic_cast<vpMbGenericTracker *
>(tracker)->setUseDepthNormalTracking(element_names[idx],
false);
696 std::cout << std::endl;
700 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
702 time_vec.push_back(t);
712 std::stringstream ss;
713 ss <<
"Computation time: " << t <<
" ms";
723 ss <<
"Projection error: " << projection_error;
728 if (opt_click_allowed && opt_display) {
747 if (computeCovariance) {
748 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
751 if (projectionError) {
752 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
763 std::cout <<
"\nFinal poses, cMo:\n" << cMo << std::endl;
768 if (opt_click_allowed && !quit) {
778 std::cout <<
"Catch an exception: " << e << std::endl;
783 #elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
786 std::cout <<
"Cannot run this example: visp_mbt, visp_gui modules are required." << std::endl;
792 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="") vp_override
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)
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)
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT double measureTimeMs()