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:X:m:M:i:n:dchfolwvpt:T:e:"
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>] [-X <config file depth>]\n\
77 [-m <model name>] [-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>] [-T <tracker type>] [-e <last frame index>]\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.\n\
94 The config file is used to specify the parameters of the tracker.\n\
97 Set the config file (the xml file) to use for the depth sensor.\n\
98 The config file is used to specify the parameters of the tracker.\n\
101 Specify the name of the file of the model.\n\
102 The model can either be a vrml model (.wrl) or a .cao file.\n\
105 Specify the name of the file of the model for the depth sensor.\n\
106 The model can either be a vrml model (.wrl) or a .cao file.\n\
108 -n <initialisation file base name> \n\
109 Base name of the initialisation file. The file will be 'base_name'.init .\n\
110 This base name is also used for the optional picture specifying where to \n\
111 click (a .ppm picture).\n\
114 Turn off the display of the the moving edges and Klt points. \n\
117 Turn off the display.\n\
120 Disable the mouse click. Useful to automate the \n\
121 execution of this program without human intervention.\n\
124 Use Ogre3D for visibility tests\n\
127 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
130 Use the scanline for visibility tests.\n\
133 Compute covariance matrix.\n\
136 Compute gradient projection error.\n\
139 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
142 Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
144 -e <last frame index>\n\
145 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
148 Print the help.\n\n");
151 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
154bool getOptions(
int argc,
const char **argv, std::string &ipath, std::string &configFile, std::string &configFile_depth,
155 std::string &modelFile, std::string &modelFile_depth, std::string &initFile,
bool &displayFeatures,
156 bool &click_allowed,
bool &display,
bool &useOgre,
bool &showOgreConfigDialog,
bool &useScanline,
157 bool &computeCovariance,
bool &projectionError,
int &trackerType,
int &tracker_type_depth,
169 configFile = optarg_;
172 configFile_depth = optarg_;
178 modelFile_depth = optarg_;
184 displayFeatures =
false;
187 click_allowed =
false;
199 showOgreConfigDialog =
true;
202 computeCovariance =
true;
205 projectionError =
true;
208 trackerType = atoi(optarg_);
211 tracker_type_depth = atoi(optarg_);
214 lastFrame = atoi(optarg_);
217 usage(argv[0], NULL);
222 usage(argv[0], optarg_);
228 if ((c == 1) || (c == -1)) {
230 usage(argv[0], NULL);
231 std::cerr <<
"ERROR: " << std::endl;
232 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
252void rs_deproject_pixel_to_point(
float point[3],
const rs_intrinsics &intrin,
const float pixel[2],
float depth)
254 float x = (pixel[0] - intrin.ppx) / intrin.fx;
255 float y = (pixel[1] - intrin.ppy) / intrin.fy;
257 float r2 = x * x + y * y;
258 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
259 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
260 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
265 point[0] = depth * x;
266 point[1] = depth * y;
271 vpImage<uint16_t> &I_depth_raw, std::vector<vpColVector> &pointcloud,
unsigned int &pointcloud_width,
272 unsigned int &pointcloud_height)
274#if VISP_HAVE_DATASET_VERSION >= 0x030600
275 std::string ext(
"png");
277 std::string ext(
"pgm");
280 std::stringstream ss;
281 ss << input_directory <<
"/image_";
282 ss << std::setfill(
'0') << std::setw(4);
286 std::string filename_image = ss.str();
288 std::cerr <<
"Cannot read: " << filename_image << std::endl;
295 ss << input_directory <<
"/depth_image_";
296 ss << std::setfill(
'0') << std::setw(4);
299 std::string filename_depth = ss.str();
301 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
302 if (!file_depth.is_open()) {
306 unsigned int height = 0, width = 0;
310 I_depth_raw.
resize(height, width);
312 uint16_t depth_value = 0;
313 for (
unsigned int i = 0; i < height; i++) {
314 for (
unsigned int j = 0; j < width; j++) {
316 I_depth_raw[i][j] = depth_value;
321 pointcloud_width = width;
322 pointcloud_height = height;
323 pointcloud.
resize((
size_t)width * height);
326 const float depth_scale = 0.000124986647f;
327 rs_intrinsics depth_intrinsic;
328 depth_intrinsic.ppx = 311.484558f;
329 depth_intrinsic.ppy = 246.283234f;
330 depth_intrinsic.fx = 476.053619f;
331 depth_intrinsic.fy = 476.053497f;
332 depth_intrinsic.coeffs[0] = 0.165056542f;
333 depth_intrinsic.coeffs[1] = -0.0508309528f;
334 depth_intrinsic.coeffs[2] = 0.00435937941f;
335 depth_intrinsic.coeffs[3] = 0.00541406544f;
336 depth_intrinsic.coeffs[4] = 0.250085592f;
338 for (
unsigned int i = 0; i < height; i++) {
339 for (
unsigned int j = 0; j < width; j++) {
340 float scaled_depth = I_depth_raw[i][j] * depth_scale;
342 float pixel[2] = { (float)j, (
float)i };
343 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
346 data_3D[0] = point[0];
347 data_3D[1] = point[1];
348 data_3D[2] = point[2];
350 pointcloud[(size_t)(i * width + j)] = data_3D;
371 dynamic_cast<vpMbGenericTracker *
>(tracker)->loadConfigFile(configFile, configFile_depth);
386#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
403 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
404 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
405 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
430int main(
int argc,
const char **argv)
433 std::string env_ipath;
434 std::string opt_ipath;
436 std::string opt_configFile;
437 std::string opt_configFile_depth;
438 std::string opt_modelFile;
439 std::string opt_modelFile_depth;
440 std::string opt_initFile;
441 std::string initFile;
442 bool displayFeatures =
true;
443 bool opt_click_allowed =
true;
444 bool opt_display =
true;
445 bool useOgre =
false;
446 bool showOgreConfigDialog =
false;
447 bool useScanline =
false;
448 bool computeCovariance =
false;
449 bool projectionError =
false;
452#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
454 int opt_lastFrame = 5;
456 int opt_lastFrame = -1;
464 if (!env_ipath.empty())
468 if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_configFile_depth, opt_modelFile, opt_modelFile_depth,
469 opt_initFile, displayFeatures, opt_click_allowed, opt_display, useOgre, showOgreConfigDialog,
470 useScanline, computeCovariance, projectionError, trackerType_image, trackerType_depth,
475#if !(defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO))
476 if (trackerType_image == 2 || trackerType_image == 3) {
477 std::cout <<
"KLT features cannot be used: ViSP is not built with "
478 "KLT module or OpenCV imgproc and video modules are not available."
485 if (opt_ipath.empty() && env_ipath.empty()) {
486 usage(argv[0], NULL);
487 std::cerr << std::endl <<
"ERROR:" << std::endl;
488 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
489 <<
" environment variable to specify the location of the " << std::endl
490 <<
" image path where test images are located." << std::endl
501 std::cerr <<
"ViSP-images does not contain the folder: " << dir_path <<
"!" << std::endl;
505 std::string configFile, configFile_depth;
506 if (!opt_configFile.empty())
507 configFile = opt_configFile;
512 if (!opt_configFile_depth.empty())
513 configFile_depth = opt_configFile_depth;
518 std::string modelFile, modelFile_depth;
519 if (!opt_modelFile.empty())
520 modelFile = opt_modelFile;
522#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
530 if (!opt_modelFile_depth.empty())
531 modelFile_depth = opt_modelFile_depth;
536 std::string vrml_ext =
".wrl";
538 (modelFile.compare(modelFile.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0) ||
539 (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
542#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
543 std::cout <<
"use_vrml: " << use_vrml << std::endl;
545 std::cerr <<
"Error: vrml model file is only supported if ViSP is "
546 "build with Coin3D 3rd party"
552 if (!opt_initFile.empty())
553 initFile = opt_initFile;
559 std::vector<vpColVector> pointcloud;
560 unsigned int pointcloud_width, pointcloud_height;
561 if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) {
562 std::cerr <<
"Cannot open sequence: " << ipath << std::endl;
569#if defined(VISP_HAVE_X11)
571#elif defined(VISP_HAVE_GDI)
573#elif defined(HAVE_OPENCV_HIGHGUI)
575#elif defined(VISP_HAVE_D3D9)
577#elif defined(VISP_HAVE_GTK)
583#if defined(VISP_HAVE_DISPLAY)
586 display1.
init(I, 100, 100,
"Test tracking (Left)");
588 "Test tracking (Right)");
596 std::vector<int> trackerTypes(2);
597 trackerTypes[0] = trackerType_image;
598 trackerTypes[1] = trackerType_depth;
604 loadConfiguration(tracker, configFile, configFile_depth);
607 std::string depth_M_color_filename =
610 std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
611 depth_M_color.
load(depth_M_color_file);
612 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformationMatrices;
613 mapOfCameraTransformationMatrices[
"Camera2"] = depth_M_color;
614 dynamic_cast<vpMbGenericTracker *
>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrices);
638 if (opt_display && opt_click_allowed) {
649 if (opt_display && opt_click_allowed) {
650 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
651 mapOfImages[
"Camera1"] = &I;
652 mapOfImages[
"Camera2"] = &I_depth;
653 std::map<std::string, std::string> mapOfInitFiles;
654 mapOfInitFiles[
"Camera1"] = initFile;
657 dynamic_cast<vpMbGenericTracker *
>(tracker)->initClick(mapOfImages, mapOfInitFiles,
true);
663 vpHomogeneousMatrix c1Moi(0.06846423368, 0.09062570884, 0.3401096693, -2.671882598, 0.1174275908, -0.6011935263);
664 vpHomogeneousMatrix c2Moi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
670 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
671 mapOfImages[
"Camera1"] = &I;
672 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
673 mapOfPointclouds[
"Camera2"] = &pointcloud;
674 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
675 mapOfWidths[
"Camera2"] = pointcloud_width;
676 mapOfHeights[
"Camera2"] = pointcloud_height;
678 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
687 bool quit =
false, click =
false;
688 unsigned int frame_index = 0;
689 std::vector<double> time_vec;
690 while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
691 (opt_lastFrame > 0 ? (
int)frame_index <= opt_lastFrame : true)) {
698 std::stringstream ss;
699 ss <<
"Num frame: " << frame_index;
704 if (frame_index == 10) {
705 std::cout <<
"----------Test reset tracker----------" << std::endl;
713 loadConfiguration(tracker, configFile, configFile_depth);
725 if (frame_index == 20) {
726 c1Mo.
buildFrom(0.07734634051, 0.08993639906, 0.342344402, -2.708409543, 0.0669276477, -0.3798958303);
727 c2Mo.
buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
729 if (frame_index == 50) {
730 c1Mo.
buildFrom(0.09280663035, 0.09277655672, 0.330415149, -2.724431817, 0.0293932671, 0.02027966377);
731 c2Mo.
buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
733 std::cout <<
"Test set pose" << std::endl;
739 if (frame_index < 15 || frame_index >= 20) {
742 if (frame_index < 30 || frame_index >= 50) {
744 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
745 mapOfImages[
"Camera1"] = &I;
746 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
747 mapOfPointclouds[
"Camera2"] = &pointcloud;
748 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
749 mapOfWidths[
"Camera2"] = pointcloud_width;
750 mapOfHeights[
"Camera2"] = pointcloud_height;
753 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
755 time_vec.push_back(t);
766 std::stringstream ss;
767 ss <<
"Computation time: " << t <<
" ms";
774 std::stringstream ss;
775 ss <<
"Features: edges " <<
dynamic_cast<vpMbGenericTracker *
>(tracker)->getNbFeaturesEdge() <<
", klt "
783 if (opt_click_allowed && opt_display) {
802 if (computeCovariance) {
803 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
806 if (projectionError) {
807 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
818 std::cout <<
"\nFinal poses, c1Mo:\n" << c1Mo <<
"\nc2Mo:\n" << c2Mo << std::endl;
823 if (opt_click_allowed && !quit) {
833 std::cout <<
"Catch an exception: " << e << std::endl;
838#elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
841 std::cout <<
"Cannot run this example: visp_mbt, visp_gui modules are required." << std::endl;
847 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)
unsigned int getDownScalingFactor()
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)
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
unsigned int getHeight() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
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 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 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 setAngleAppear(const double &a)
virtual unsigned int getClipping() const
@ ROBUST_FEATURE_ESTIMATION
void setMu1(const double &mu_1)
void setSampleStep(const double &s)
void setRange(const unsigned int &r)
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
void setMaskSize(const unsigned int &a)
void setMu2(const double &mu_2)
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
void setMaskNumber(const unsigned int &a)
void setThreshold(const double &t)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
VISP_EXPORT double measureTimeMs()