44#include <visp3/core/vpConfig.h>
46#if defined(VISP_HAVE_MODULE_MBT) && \
47 (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
49#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
53#include <visp3/core/vpFont.h>
54#include <visp3/core/vpImageDraw.h>
55#include <visp3/core/vpIoTools.h>
56#include <visp3/gui/vpDisplayD3D.h>
57#include <visp3/gui/vpDisplayGDI.h>
58#include <visp3/gui/vpDisplayGTK.h>
59#include <visp3/gui/vpDisplayOpenCV.h>
60#include <visp3/gui/vpDisplayX.h>
61#include <visp3/io/vpImageIo.h>
62#include <visp3/io/vpParseArgv.h>
63#include <visp3/mbt/vpMbGenericTracker.h>
65#define GETOPTARGS "i:dsclt:e:DmCh"
69void usage(
const char *name,
const char *badparam)
72 Regression test for vpGenericTracker.\n\
75 %s [-i <test image path>] [-c] [-d] [-s] [-h] [-l] \n\
76 [-t <tracker type>] [-e <last frame index>] [-D] [-m] [-C]\n",
81 -i <input image path> \n\
82 Set image input path.\n\
83 These images come from ViSP-images-x.y.z.tar.gz available \n\
84 on the ViSP website.\n\
85 Setting the VISP_INPUT_IMAGE_PATH environment\n\
86 variable produces the same behavior than using\n\
90 Turn off the display.\n\
93 If display is turn off, tracking results are saved in a video folder.\n\
96 Disable the mouse click. Useful to automate the \n\
97 execution of this program without human intervention.\n\
100 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
103 Use the scanline for visibility tests.\n\
105 -e <last frame index>\n\
106 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
112 Set a tracking mask.\n\
118 Print the help.\n\n");
121 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
124bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
bool &save,
125 bool &useScanline,
int &trackerType,
int &lastFrame,
bool &use_depth,
bool &use_mask,
126 bool &use_color_image)
137 click_allowed =
false;
149 trackerType = atoi(optarg_);
152 lastFrame = atoi(optarg_);
161 use_color_image =
true;
164 usage(argv[0], NULL);
169 usage(argv[0], optarg_);
175 if ((c == 1) || (c == -1)) {
177 usage(argv[0], NULL);
178 std::cerr <<
"ERROR: " << std::endl;
179 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
186template <
typename Type>
190#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
191 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
192 "Template function supports only unsigned char and vpRGBa images!");
194#if VISP_HAVE_DATASET_VERSION >= 0x030600
195 std::string ext(
"png");
197 std::string ext(
"pgm");
199 char buffer[FILENAME_MAX];
200 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/Images/Image_%04d." + ext).c_str(), cpt);
201 std::string image_filename = buffer;
203 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
204 std::string depth_filename = buffer;
206 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
207 std::string pose_filename = buffer;
215 unsigned int depth_width = 0, depth_height = 0;
216 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
217 if (!file_depth.is_open())
222 I_depth.
resize(depth_height, depth_width);
223 pointcloud.resize(depth_height * depth_width);
225 const float depth_scale = 0.000030518f;
226 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
227 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
229 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
235 pointcloud[i * I_depth.
getWidth() + j] = pt3d;
239 std::ifstream file_pose(pose_filename.c_str());
240 if (!file_pose.is_open()) {
244 for (
unsigned int i = 0; i < 4; i++) {
245 for (
unsigned int j = 0; j < 4; j++) {
246 file_pose >> cMo[i][j];
257template <
typename Type>
258bool run(
const std::string &input_directory,
bool opt_click_allowed,
bool opt_display,
bool useScanline,
259 int trackerType_image,
int opt_lastFrame,
bool use_depth,
bool use_mask,
bool save)
261#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
262 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
263 "Template function supports only unsigned char and vpRGBa images!");
266#if defined(VISP_HAVE_X11)
268#elif defined(VISP_HAVE_GDI)
270#elif defined(HAVE_OPENCV_HIGHGUI)
272#elif defined(VISP_HAVE_D3D9)
274#elif defined(VISP_HAVE_GTK)
280 std::vector<int> tracker_type(2);
281 tracker_type[0] = trackerType_image;
284 std::string configFileCam1 = input_directory + std::string(
"/Config/chateau.xml");
285 std::string configFileCam2 = input_directory + std::string(
"/Config/chateau_depth.xml");
286 std::cout <<
"Load config file for camera 1: " << configFileCam1 << std::endl;
287 std::cout <<
"Load config file for camera 2: " << configFileCam2 << std::endl;
288 tracker.loadConfigFile(configFileCam1, configFileCam2);
295 tracker.setCameraParameters(cam_color, cam_depth);
308 tracker.setMovingEdge(me);
311#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
313 tracker.setKltMaskBorder(5);
322 tracker.setKltOpencv(klt);
327 tracker.setDepthNormalPclPlaneEstimationMethod(2);
328 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
329 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
330 tracker.setDepthNormalSamplingStep(2, 2);
332 tracker.setDepthDenseSamplingStep(4, 4);
336 tracker.setNearClippingDistance(0.01);
337 tracker.setFarClippingDistance(2.0);
341#ifdef VISP_HAVE_COIN3D
342 tracker.loadModel(input_directory +
"/Models/chateau.wrl", input_directory +
"/Models/chateau.cao");
344 tracker.loadModel(input_directory +
"/Models/chateau.cao", input_directory +
"/Models/chateau.cao");
355 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
357 tracker.getCameraParameters(cam_color, cam_depth);
358 tracker.setDisplayFeatures(
true);
359 tracker.setScanLineVisibilityTest(useScanline);
361 std::map<int, std::pair<double, double> > map_thresh;
363#ifdef VISP_HAVE_COIN3D
365 useScanline ? std::pair<double, double>(0.005, 3.9) : std::pair<double, double>(0.007, 3.9);
366#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
368 useScanline ? std::pair<double, double>(0.007, 1.9) : std::pair<double, double>(0.007, 1.8);
370 useScanline ? std::pair<double, double>(0.005, 3.7) : std::pair<double, double>(0.006, 3.4);
373 useScanline ? std::pair<double, double>(0.003, 1.7) : std::pair<double, double>(0.002, 0.8);
374#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
376 std::pair<double, double>(0.002, 0.3);
379 useScanline ? std::pair<double, double>(0.002, 1.8) : std::pair<double, double>(0.002, 0.7);
383 useScanline ? std::pair<double, double>(0.008, 2.3) : std::pair<double, double>(0.007, 2.1);
384#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
386 useScanline ? std::pair<double, double>(0.006, 1.7) : std::pair<double, double>(0.005, 1.4);
388 useScanline ? std::pair<double, double>(0.004, 1.2) : std::pair<double, double>(0.004, 1.2);
391 useScanline ? std::pair<double, double>(0.002, 0.7) : std::pair<double, double>(0.001, 0.4);
392#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
394 std::pair<double, double>(0.002, 0.3);
397 useScanline ? std::pair<double, double>(0.001, 0.5) : std::pair<double, double>(0.001, 0.4);
404 std::vector<vpColVector> pointcloud;
406 if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
407 std::cerr <<
"Cannot read first frame!" << std::endl;
412 const double roi_step = 7.0;
413 const double roi_step2 = 6.0;
416 for (
unsigned int i = (
unsigned int)(I.
getRows() / roi_step);
417 i < (
unsigned int)(I.
getRows() * roi_step2 / roi_step); i++) {
418 for (
unsigned int j = (
unsigned int)(I.
getCols() / roi_step);
419 j < (
unsigned int)(I.
getCols() * roi_step2 / roi_step); j++) {
423 tracker.setMask(mask);
435#ifdef VISP_HAVE_DISPLAY
436 display1.
init(I, 0, 0,
"Image");
442 depth_M_color[0][3] = -0.05;
443 tracker.setCameraTransformationMatrix(
"Camera2", depth_M_color);
444 tracker.initFromPose(I, cMo_truth);
447 bool click =
false, quit =
false, correct_accuracy =
true;
448 std::vector<double> vec_err_t, vec_err_tu;
449 std::vector<double> time_vec;
450 while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit &&
451 (opt_lastFrame > 0 ? (
int)cpt_frame <= opt_lastFrame : true)) {
459 convert(I, resultsColor);
460 convert(I_depth, resultsDepth);
464 std::map<std::string, const vpImage<Type> *> mapOfImages;
465 mapOfImages[
"Camera1"] = &I;
466 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
467 mapOfPointclouds[
"Camera2"] = &pointcloud;
468 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
470 mapOfWidths[
"Camera2"] = 0;
471 mapOfHeights[
"Camera2"] = 0;
474 mapOfWidths[
"Camera2"] = I_depth.
getWidth();
475 mapOfHeights[
"Camera2"] = I_depth.
getHeight();
478 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
481 time_vec.push_back(t);
484 tracker.display(I, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
488 std::stringstream ss;
489 ss <<
"Frame: " << cpt_frame;
492 ss <<
"Nb features: " << tracker.getError().getRows();
497 std::map<std::string, std::vector<std::vector<double> > > mapOfModels;
498 std::map<std::string, unsigned int> mapOfW;
501 std::map<std::string, unsigned int> mapOfH;
502 mapOfH[
"Camera1"] = I_depth.
getWidth();
504 std::map<std::string, vpHomogeneousMatrix> mapOfcMos;
505 mapOfcMos[
"Camera1"] = cMo;
506 mapOfcMos[
"Camera2"] = depth_M_color * cMo;
507 std::map<std::string, vpCameraParameters> mapOfCams;
508 mapOfCams[
"Camera1"] = cam_color;
509 mapOfCams[
"Camera2"] = cam_depth;
510 tracker.getModelForDisplay(mapOfModels, mapOfW, mapOfH, mapOfcMos, mapOfCams);
511 for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfModels.begin();
512 it != mapOfModels.end(); ++it) {
513 for (
size_t i = 0; i < it->second.size(); i++) {
515 if (std::fabs(it->second[i][0]) <= std::numeric_limits<double>::epsilon()) {
525 std::map<std::string, std::vector<std::vector<double> > > mapOfFeatures;
526 tracker.getFeaturesForDisplay(mapOfFeatures);
527 for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfFeatures.begin();
528 it != mapOfFeatures.end(); ++it) {
529 for (
size_t i = 0; i < it->second.size(); i++) {
530 if (std::fabs(it->second[i][0]) <=
531 std::numeric_limits<double>::epsilon()) {
533 if (std::fabs(it->second[i][3]) <= std::numeric_limits<double>::epsilon()) {
536 else if (std::fabs(it->second[i][3] - 1) <=
537 std::numeric_limits<double>::epsilon()) {
540 else if (std::fabs(it->second[i][3] - 2) <=
541 std::numeric_limits<double>::epsilon()) {
544 else if (std::fabs(it->second[i][3] - 3) <=
545 std::numeric_limits<double>::epsilon()) {
548 else if (std::fabs(it->second[i][3] - 4) <=
549 std::numeric_limits<double>::epsilon()) {
553 vpImagePoint(it->second[i][1], it->second[i][2]), 3, color, 1);
555 else if (std::fabs(it->second[i][0] - 1) <=
556 std::numeric_limits<double>::epsilon()) {
565 std::ostringstream oss;
566 oss <<
"Tracking time: " << t <<
" ms";
574 for (
unsigned int i = 0; i < 3; i++) {
575 t_est[i] = pose_est[i];
576 t_truth[i] = pose_truth[i];
577 tu_est[i] = pose_est[i + 3];
578 tu_truth[i] = pose_truth[i + 3];
581 vpColVector t_err = t_truth - t_est, tu_err = tu_truth - tu_est;
582 const double t_thresh =
584 const double tu_thresh =
587 vec_err_t.push_back(t_err2);
588 vec_err_tu.push_back(tu_err2);
589 if (!use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh)) {
590 std::cerr <<
"Pose estimated exceeds the threshold (t_thresh = " << t_thresh <<
" ; tu_thresh = " << tu_thresh
591 <<
")!" << std::endl;
592 std::cout <<
"t_err: " << t_err2 <<
" ; tu_err: " << tu_err2 << std::endl;
593 correct_accuracy =
false;
609 char buffer[FILENAME_MAX];
610 std::ostringstream oss;
611 oss <<
"results/image_%04d.png";
612 snprintf(buffer, FILENAME_MAX, oss.str().c_str(), cpt_frame);
615 results.insert(resultsDepth,
vpImagePoint(0, resultsColor.getWidth()));
621 if (opt_display && opt_click_allowed) {
642 if (!time_vec.empty())
647 if (!vec_err_t.empty())
648 std::cout <<
"Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
650 if (!vec_err_tu.empty())
651 std::cout <<
"Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
653 std::cout <<
"Test result: " << (correct_accuracy ?
"success" :
"failure") << std::endl;
654 return correct_accuracy ? EXIT_SUCCESS : EXIT_FAILURE;
658int main(
int argc,
const char *argv [])
661 std::string env_ipath;
662 std::string opt_ipath =
"";
663 bool opt_click_allowed =
true;
664 bool opt_display =
true;
665 bool opt_save =
false;
666 bool useScanline =
false;
668#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
670 int opt_lastFrame = 5;
672 int opt_lastFrame = -1;
674 bool use_depth =
false;
675 bool use_mask =
false;
676 bool use_color_image =
false;
683 if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_save, useScanline, trackerType_image,
684 opt_lastFrame, use_depth, use_mask, use_color_image)) {
688#if ! (defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO))
689 if (trackerType_image == 2 || trackerType_image == 3) {
690 std::cout <<
"Using klt tracker is not possible without OpenCV imgproc and video modules." << std::endl;
691 std::cout <<
"Use rather command line option -t 1 to use edges." << std::endl;
695 std::cout <<
"trackerType_image: " << trackerType_image << std::endl;
696 std::cout <<
"useScanline: " << useScanline << std::endl;
697 std::cout <<
"use_depth: " << use_depth << std::endl;
698 std::cout <<
"use_mask: " << use_mask << std::endl;
699 std::cout <<
"use_color_image: " << use_color_image << std::endl;
700#ifdef VISP_HAVE_COIN3D
701 std::cout <<
"COIN3D available." << std::endl;
704#if !defined(VISP_HAVE_MODULE_KLT) || (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100))
705 if (trackerType_image & 2) {
706 std::cout <<
"KLT features cannot be used: ViSP is not built with "
707 "KLT module or OpenCV is not available.\nTest is not run."
714 if (opt_ipath.empty() && env_ipath.empty()) {
715 usage(argv[0], NULL);
716 std::cerr << std::endl <<
"ERROR:" << std::endl;
717 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
718 <<
" environment variable to specify the location of the " << std::endl
719 <<
" image path where test images are located." << std::endl
725 std::string input_directory =
728 std::cerr <<
"ViSP-images does not contain the folder: " << input_directory <<
"!" << std::endl;
732 if (use_color_image) {
733 return run<vpRGBa>(input_directory, opt_click_allowed, opt_display, useScanline, trackerType_image, opt_lastFrame,
734 use_depth, use_mask, opt_save);
737 return run<unsigned char>(input_directory, opt_click_allowed, opt_display, useScanline, trackerType_image,
738 opt_lastFrame, use_depth, use_mask, opt_save);
741 std::cout <<
"Test succeed" << std::endl;
745 std::cout <<
"Catch an exception: " << e << std::endl;
749#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
752 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
758 std::cout <<
"Enable MBT module (VISP_HAVE_MODULE_MBT) to launch this test." << std::endl;
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
static const vpColor cyan
static const vpColor none
static const vpColor blue
static const vpColor purple
static const vpColor yellow
static const vpColor green
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)
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 displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
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.
Font drawing functions for image.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void drawLine(vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, unsigned char color, unsigned int thickness=1)
static void drawCross(vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, unsigned char color, unsigned int thickness=1)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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 getCols() const
unsigned int getHeight() const
unsigned int getRows() 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)
static double deg(double rad)
Real-time 6D object pose tracking using its CAD model.
@ 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)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Implementation of a pose vector and operations on poses.
Defines a rectangle in the plane.
VISP_EXPORT double measureTimeMs()