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:"
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>] [-X <config file depth>]\n\
76 [-m <model name>] [-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>] [-T <tracker type>] [-e <last frame index>]\n",
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.\n\
93 The config file is used to specify the parameters of the tracker.\n\
96 Set the config file (the xml file) to use for the depth sensor.\n\
97 The config file is used to specify the parameters of the tracker.\n\
100 Specify the name of the file of the model.\n\
101 The model can either be a vrml model (.wrl) or a .cao file.\n\
104 Specify the name of the file of the model for the depth sensor.\n\
105 The model can either be a vrml model (.wrl) or a .cao file.\n\
107 -n <initialisation file base name> \n\
108 Base name of the initialisation file. The file will be 'base_name'.init .\n\
109 This base name is also used for the optional picture specifying where to \n\
110 click (a .ppm picture).\n\
113 Turn off the display of the the moving edges and Klt points. \n\
116 Turn off the display.\n\
119 Disable the mouse click. Useful to automate the \n\
120 execution of this program without human intervention.\n\
123 Use Ogre3D for visibility tests\n\
126 When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
129 Use the scanline for visibility tests.\n\
132 Compute covariance matrix.\n\
135 Compute gradient projection error.\n\
138 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
141 Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
143 -e <last frame index>\n\
144 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
147 Print the help.\n\n");
150 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
153 bool getOptions(
int argc,
const char **argv, std::string &ipath, std::string &configFile, std::string &configFile_depth,
154 std::string &modelFile, std::string &modelFile_depth, std::string &initFile,
bool &displayFeatures,
155 bool &click_allowed,
bool &display,
bool &useOgre,
bool &showOgreConfigDialog,
bool &useScanline,
156 bool &computeCovariance,
bool &projectionError,
int &trackerType,
int &tracker_type_depth,
168 configFile = optarg_;
171 configFile_depth = optarg_;
177 modelFile_depth = optarg_;
183 displayFeatures =
false;
186 click_allowed =
false;
198 showOgreConfigDialog =
true;
201 computeCovariance =
true;
204 projectionError =
true;
207 trackerType = atoi(optarg_);
210 tracker_type_depth = atoi(optarg_);
213 lastFrame = atoi(optarg_);
216 usage(argv[0],
nullptr);
221 usage(argv[0], optarg_);
227 if ((c == 1) || (c == -1)) {
229 usage(argv[0],
nullptr);
230 std::cerr <<
"ERROR: " << std::endl;
231 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
238 struct vpRealsenseIntrinsics_t
251 void rs_deproject_pixel_to_point(
float point[3],
const vpRealsenseIntrinsics_t &intrin,
const float pixel[2],
float depth)
253 float x = (pixel[0] - intrin.ppx) / intrin.fx;
254 float y = (pixel[1] - intrin.ppy) / intrin.fy;
256 float r2 = x * x + y * y;
257 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
258 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
259 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
264 point[0] = depth * x;
265 point[1] = depth * y;
270 vpImage<uint16_t> &I_depth_raw, std::vector<vpColVector> &pointcloud,
unsigned int &pointcloud_width,
271 unsigned int &pointcloud_height)
273 #if VISP_HAVE_DATASET_VERSION >= 0x030600
274 std::string ext(
"png");
276 std::string ext(
"pgm");
279 std::stringstream ss;
280 ss << input_directory <<
"/image_";
281 ss << std::setfill(
'0') << std::setw(4);
285 std::string filename_image = ss.str();
287 std::cerr <<
"Cannot read: " << filename_image << std::endl;
294 ss << input_directory <<
"/depth_image_";
295 ss << std::setfill(
'0') << std::setw(4);
298 std::string filename_depth = ss.str();
300 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
301 if (!file_depth.is_open()) {
305 unsigned int height = 0, width = 0;
309 I_depth_raw.
resize(height, width);
311 uint16_t depth_value = 0;
312 for (
unsigned int i = 0; i < height; i++) {
313 for (
unsigned int j = 0; j < width; j++) {
315 I_depth_raw[i][j] = depth_value;
320 pointcloud_width = width;
321 pointcloud_height = height;
322 pointcloud.
resize((
size_t)width * height);
325 const float depth_scale = 0.000124986647f;
326 vpRealsenseIntrinsics_t depth_intrinsic;
327 depth_intrinsic.ppx = 311.484558f;
328 depth_intrinsic.ppy = 246.283234f;
329 depth_intrinsic.fx = 476.053619f;
330 depth_intrinsic.fy = 476.053497f;
331 depth_intrinsic.coeffs[0] = 0.165056542f;
332 depth_intrinsic.coeffs[1] = -0.0508309528f;
333 depth_intrinsic.coeffs[2] = 0.00435937941f;
334 depth_intrinsic.coeffs[3] = 0.00541406544f;
335 depth_intrinsic.coeffs[4] = 0.250085592f;
337 for (
unsigned int i = 0; i < height; i++) {
338 for (
unsigned int j = 0; j < width; j++) {
339 float scaled_depth = I_depth_raw[i][j] * depth_scale;
341 float pixel[2] = { (float)j, (
float)i };
342 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
345 data_3D[0] = point[0];
346 data_3D[1] = point[1];
347 data_3D[2] = point[2];
349 pointcloud[(size_t)(i * width + j)] = data_3D;
358 #
if defined(VISP_HAVE_PUGIXML)
363 #
if defined(VISP_HAVE_PUGIXML)
368 #if defined(VISP_HAVE_PUGIXML)
370 dynamic_cast<vpMbGenericTracker *
>(tracker)->loadConfigFile(configFile, configFile_depth);
385 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
402 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
403 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
404 dynamic_cast<vpMbGenericTracker *
>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
429 int main(
int argc,
const char **argv)
432 std::string env_ipath;
433 std::string opt_ipath;
435 std::string opt_configFile;
436 std::string opt_configFile_depth;
437 std::string opt_modelFile;
438 std::string opt_modelFile_depth;
439 std::string opt_initFile;
440 std::string initFile;
441 bool displayFeatures =
true;
442 bool opt_click_allowed =
true;
443 bool opt_display =
true;
444 bool useOgre =
false;
445 bool showOgreConfigDialog =
false;
446 bool useScanline =
false;
447 bool computeCovariance =
false;
448 bool projectionError =
false;
451 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
453 int opt_lastFrame = 5;
455 int opt_lastFrame = -1;
463 if (!env_ipath.empty())
467 if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_configFile_depth, opt_modelFile, opt_modelFile_depth,
468 opt_initFile, displayFeatures, opt_click_allowed, opt_display, useOgre, showOgreConfigDialog,
469 useScanline, computeCovariance, projectionError, trackerType_image, trackerType_depth,
474 #if !(defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO))
475 if (trackerType_image == 2 || trackerType_image == 3) {
476 std::cout <<
"KLT features cannot be used: ViSP is not built with "
477 "KLT module or OpenCV imgproc and video modules are not available."
484 if (opt_ipath.empty() && env_ipath.empty()) {
485 usage(argv[0],
nullptr);
486 std::cerr << std::endl <<
"ERROR:" << std::endl;
487 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
488 <<
" environment variable to specify the location of the " << std::endl
489 <<
" image path where test images are located." << std::endl
500 std::cerr <<
"ViSP-images does not contain the folder: " << dir_path <<
"!" << std::endl;
504 std::string configFile, configFile_depth;
505 if (!opt_configFile.empty())
506 configFile = opt_configFile;
511 if (!opt_configFile_depth.empty())
512 configFile_depth = opt_configFile_depth;
517 std::string modelFile, modelFile_depth;
518 if (!opt_modelFile.empty())
519 modelFile = opt_modelFile;
521 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
529 if (!opt_modelFile_depth.empty())
530 modelFile_depth = opt_modelFile_depth;
535 std::string vrml_ext =
".wrl";
537 (modelFile.compare(modelFile.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0) ||
538 (modelFile_depth.compare(modelFile_depth.length() - vrml_ext.length(), vrml_ext.length(), vrml_ext) == 0);
541 #if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 2 || COIN_MAJOR_VERSION == 3 || COIN_MAJOR_VERSION == 4)
542 std::cout <<
"use_vrml: " << use_vrml << std::endl;
544 std::cerr <<
"Error: vrml model file is only supported if ViSP is "
545 "build with Coin3D 3rd party"
551 if (!opt_initFile.empty())
552 initFile = opt_initFile;
558 std::vector<vpColVector> pointcloud;
559 unsigned int pointcloud_width, pointcloud_height;
560 if (!read_data(0, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height)) {
561 std::cerr <<
"Cannot open sequence: " << ipath << std::endl;
568 #if defined(VISP_HAVE_X11)
570 #elif defined(VISP_HAVE_GDI)
572 #elif defined(HAVE_OPENCV_HIGHGUI)
574 #elif defined(VISP_HAVE_D3D9)
576 #elif defined(VISP_HAVE_GTK)
582 #if defined(VISP_HAVE_DISPLAY)
585 display1.
init(I, 100, 100,
"Test tracking (Left)");
587 "Test tracking (Right)");
595 std::vector<int> trackerTypes(2);
596 trackerTypes[0] = trackerType_image;
597 trackerTypes[1] = trackerType_depth;
603 loadConfiguration(tracker, configFile, configFile_depth);
606 std::string depth_M_color_filename =
609 std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
610 depth_M_color.
load(depth_M_color_file);
611 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformationMatrices;
612 mapOfCameraTransformationMatrices[
"Camera2"] = depth_M_color;
613 dynamic_cast<vpMbGenericTracker *
>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrices);
637 if (opt_display && opt_click_allowed) {
648 if (opt_display && opt_click_allowed) {
649 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
650 mapOfImages[
"Camera1"] = &I;
651 mapOfImages[
"Camera2"] = &I_depth;
652 std::map<std::string, std::string> mapOfInitFiles;
653 mapOfInitFiles[
"Camera1"] = initFile;
656 dynamic_cast<vpMbGenericTracker *
>(tracker)->initClick(mapOfImages, mapOfInitFiles,
true);
662 vpHomogeneousMatrix c1Moi(0.06846423368, 0.09062570884, 0.3401096693, -2.671882598, 0.1174275908, -0.6011935263);
663 vpHomogeneousMatrix c2Moi(0.04431452054, 0.09294637757, 0.3357760654, -2.677922443, 0.121297639, -0.6028463357);
669 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
670 mapOfImages[
"Camera1"] = &I;
671 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
672 mapOfPointclouds[
"Camera2"] = &pointcloud;
673 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
674 mapOfWidths[
"Camera2"] = pointcloud_width;
675 mapOfHeights[
"Camera2"] = pointcloud_height;
677 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
686 bool quit =
false, click =
false;
687 unsigned int frame_index = 0;
688 std::vector<double> time_vec;
689 while (read_data(frame_index, ipath, I, I_depth_raw, pointcloud, pointcloud_width, pointcloud_height) && !quit &&
690 (opt_lastFrame > 0 ? (
int)frame_index <= opt_lastFrame :
true)) {
697 std::stringstream ss;
698 ss <<
"Num frame: " << frame_index;
703 if (frame_index == 10) {
704 std::cout <<
"----------Test reset tracker----------" << std::endl;
712 loadConfiguration(tracker, configFile, configFile_depth);
723 #if USE_SMALL_DATASET
724 if (frame_index == 20) {
725 c1Mo.
buildFrom(0.07734634051, 0.08993639906, 0.342344402, -2.708409543, 0.0669276477, -0.3798958303);
726 c2Mo.
buildFrom(0.05319520317, 0.09223511976, 0.3380095812, -2.71438192, 0.07141055397, -0.3810081638);
728 if (frame_index == 50) {
729 c1Mo.
buildFrom(0.09280663035, 0.09277655672, 0.330415149, -2.724431817, 0.0293932671, 0.02027966377);
730 c2Mo.
buildFrom(0.06865933578, 0.09494713501, 0.3260555142, -2.730027451, 0.03498390135, 0.01989831338);
732 std::cout <<
"Test set pose" << std::endl;
736 #if USE_SMALL_DATASET
738 if (frame_index < 15 || frame_index >= 20) {
741 if (frame_index < 30 || frame_index >= 50) {
743 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
744 mapOfImages[
"Camera1"] = &I;
745 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
746 mapOfPointclouds[
"Camera2"] = &pointcloud;
747 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
748 mapOfWidths[
"Camera2"] = pointcloud_width;
749 mapOfHeights[
"Camera2"] = pointcloud_height;
752 dynamic_cast<vpMbGenericTracker *
>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
754 time_vec.push_back(t);
765 std::stringstream ss;
766 ss <<
"Computation time: " << t <<
" ms";
773 std::stringstream ss;
774 ss <<
"Features: edges " <<
dynamic_cast<vpMbGenericTracker *
>(tracker)->getNbFeaturesEdge() <<
", klt "
782 if (opt_click_allowed && opt_display) {
801 if (computeCovariance) {
802 std::cout <<
"Covariance matrix: \n" << tracker->
getCovarianceMatrix() << std::endl << std::endl;
805 if (projectionError) {
806 std::cout <<
"Projection error: " << tracker->
getProjectionError() << std::endl << std::endl;
817 std::cout <<
"\nFinal poses, c1Mo:\n" << c1Mo <<
"\nc2Mo:\n" << c2Mo << std::endl;
822 if (opt_click_allowed && !quit) {
832 std::cout <<
"Catch an exception: " << e << std::endl;
837 #elif !(defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY))
840 std::cout <<
"Cannot run this example: visp_mbt, visp_gui modules are required." << std::endl;
846 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)
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)
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 setRange(const unsigned int &range)
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
void setMaskNumber(const unsigned int &mask_number)
void setThreshold(const double &threshold)
void setSampleStep(const double &sample_step)
void setMaskSize(const unsigned int &mask_size)
void setMu2(const double &mu_2)
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()