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)
50 #include <type_traits>
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"
69 void 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);
124 bool 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],
nullptr);
169 usage(argv[0], optarg_);
175 if ((c == 1) || (c == -1)) {
177 usage(argv[0],
nullptr);
178 std::cerr <<
"ERROR: " << std::endl;
179 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
186 template <
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];
257 template <
typename Type>
258 bool 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;
285 #if defined(VISP_HAVE_PUGIXML)
286 std::string configFileCam1 = input_directory + std::string(
"/Config/chateau.xml");
287 std::string configFileCam2 = input_directory + std::string(
"/Config/chateau_depth.xml");
288 std::cout <<
"Load config file for camera 1: " << configFileCam1 << std::endl;
289 std::cout <<
"Load config file for camera 2: " << configFileCam2 << std::endl;
290 tracker.loadConfigFile(configFileCam1, configFileCam2);
297 tracker.setCameraParameters(cam_color, cam_depth);
310 tracker.setMovingEdge(me);
313 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
315 tracker.setKltMaskBorder(5);
324 tracker.setKltOpencv(klt);
329 tracker.setDepthNormalPclPlaneEstimationMethod(2);
330 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
331 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
332 tracker.setDepthNormalSamplingStep(2, 2);
334 tracker.setDepthDenseSamplingStep(4, 4);
338 tracker.setNearClippingDistance(0.01);
339 tracker.setFarClippingDistance(2.0);
343 #ifdef VISP_HAVE_COIN3D
344 tracker.loadModel(input_directory +
"/Models/chateau.wrl", input_directory +
"/Models/chateau.cao");
346 tracker.loadModel(input_directory +
"/Models/chateau.cao", input_directory +
"/Models/chateau.cao");
357 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
359 tracker.getCameraParameters(cam_color, cam_depth);
360 tracker.setDisplayFeatures(
true);
361 tracker.setScanLineVisibilityTest(useScanline);
363 std::map<int, std::pair<double, double> > map_thresh;
365 #ifdef VISP_HAVE_COIN3D
367 useScanline ? std::pair<double, double>(0.005, 6.) : std::pair<double, double>(0.007, 3.9);
368 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
370 useScanline ? std::pair<double, double>(0.007, 1.9) : std::pair<double, double>(0.007, 1.8);
372 useScanline ? std::pair<double, double>(0.005, 3.7) : std::pair<double, double>(0.006, 3.4);
375 useScanline ? std::pair<double, double>(0.003, 1.7) : std::pair<double, double>(0.002, 0.8);
376 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
378 std::pair<double, double>(0.002, 0.3);
381 useScanline ? std::pair<double, double>(0.002, 1.8) : std::pair<double, double>(0.002, 0.7);
385 useScanline ? std::pair<double, double>(0.015, 3.0) : std::pair<double, double>(0.009, 4.0);
386 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
388 useScanline ? std::pair<double, double>(0.006, 1.7) : std::pair<double, double>(0.005, 1.4);
390 useScanline ? std::pair<double, double>(0.004, 1.2) : std::pair<double, double>(0.004, 1.2);
393 useScanline ? std::pair<double, double>(0.002, 0.7) : std::pair<double, double>(0.001, 0.4);
394 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
396 std::pair<double, double>(0.002, 0.3);
399 useScanline ? std::pair<double, double>(0.001, 0.5) : std::pair<double, double>(0.001, 0.4);
406 std::vector<vpColVector> pointcloud;
408 if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
409 std::cerr <<
"Cannot read first frame!" << std::endl;
414 const double roi_step = 7.0;
415 const double roi_step2 = 6.0;
418 for (
unsigned int i = (
unsigned int)(I.
getRows() / roi_step);
419 i < (
unsigned int)(I.
getRows() * roi_step2 / roi_step); i++) {
420 for (
unsigned int j = (
unsigned int)(I.
getCols() / roi_step);
421 j < (
unsigned int)(I.
getCols() * roi_step2 / roi_step); j++) {
425 tracker.setMask(mask);
437 #ifdef VISP_HAVE_DISPLAY
438 display1.
init(I, 0, 0,
"Image");
444 depth_M_color[0][3] = -0.05;
445 tracker.setCameraTransformationMatrix(
"Camera2", depth_M_color);
446 tracker.initFromPose(I, cMo_truth);
449 bool click =
false, quit =
false, correct_accuracy =
true;
450 std::vector<double> vec_err_t, vec_err_tu;
451 std::vector<double> time_vec;
452 while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit &&
453 (opt_lastFrame > 0 ? (
int)cpt_frame <= opt_lastFrame :
true)) {
461 convert(I, resultsColor);
462 convert(I_depth, resultsDepth);
466 std::map<std::string, const vpImage<Type> *> mapOfImages;
467 mapOfImages[
"Camera1"] = &I;
468 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
469 mapOfPointclouds[
"Camera2"] = &pointcloud;
470 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
472 mapOfWidths[
"Camera2"] = 0;
473 mapOfHeights[
"Camera2"] = 0;
476 mapOfWidths[
"Camera2"] = I_depth.
getWidth();
477 mapOfHeights[
"Camera2"] = I_depth.
getHeight();
480 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
483 time_vec.push_back(t);
486 tracker.display(I, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
490 std::stringstream ss;
491 ss <<
"Frame: " << cpt_frame;
494 ss <<
"Nb features: " << tracker.getError().getRows();
499 std::map<std::string, std::vector<std::vector<double> > > mapOfModels;
500 std::map<std::string, unsigned int> mapOfW;
502 mapOfW[
"Camera2"] = I_depth.
getWidth();
503 std::map<std::string, unsigned int> mapOfH;
506 std::map<std::string, vpHomogeneousMatrix> mapOfcMos;
507 mapOfcMos[
"Camera1"] = cMo;
508 mapOfcMos[
"Camera2"] = depth_M_color * cMo;
509 std::map<std::string, vpCameraParameters> mapOfCams;
510 mapOfCams[
"Camera1"] = cam_color;
511 mapOfCams[
"Camera2"] = cam_depth;
512 tracker.getModelForDisplay(mapOfModels, mapOfW, mapOfH, mapOfcMos, mapOfCams);
513 for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfModels.begin();
514 it != mapOfModels.end(); ++it) {
515 for (
size_t i = 0; i < it->second.size(); i++) {
517 if (std::fabs(it->second[i][0]) <= std::numeric_limits<double>::epsilon()) {
527 std::map<std::string, std::vector<std::vector<double> > > mapOfFeatures;
528 tracker.getFeaturesForDisplay(mapOfFeatures);
529 for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfFeatures.begin();
530 it != mapOfFeatures.end(); ++it) {
531 for (
size_t i = 0; i < it->second.size(); i++) {
532 if (std::fabs(it->second[i][0]) <=
533 std::numeric_limits<double>::epsilon()) {
535 if (std::fabs(it->second[i][3]) <= std::numeric_limits<double>::epsilon()) {
538 else if (std::fabs(it->second[i][3] - 1) <=
539 std::numeric_limits<double>::epsilon()) {
542 else if (std::fabs(it->second[i][3] - 2) <=
543 std::numeric_limits<double>::epsilon()) {
546 else if (std::fabs(it->second[i][3] - 3) <=
547 std::numeric_limits<double>::epsilon()) {
550 else if (std::fabs(it->second[i][3] - 4) <=
551 std::numeric_limits<double>::epsilon()) {
555 vpImagePoint(it->second[i][1], it->second[i][2]), 3, color, 1);
557 else if (std::fabs(it->second[i][0] - 1) <=
558 std::numeric_limits<double>::epsilon()) {
567 std::ostringstream oss;
568 oss <<
"Tracking time: " << t <<
" ms";
576 for (
unsigned int i = 0; i < 3; i++) {
577 t_est[i] = pose_est[i];
578 t_truth[i] = pose_truth[i];
579 tu_est[i] = pose_est[i + 3];
580 tu_truth[i] = pose_truth[i + 3];
583 vpColVector t_err = t_truth - t_est, tu_err = tu_truth - tu_est;
584 const double t_thresh =
586 const double tu_thresh =
589 vec_err_t.push_back(t_err2);
590 vec_err_tu.push_back(tu_err2);
591 if (!use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh)) {
592 std::cerr <<
"Pose estimated exceeds the threshold (t_thresh = " << t_thresh <<
" ; tu_thresh = " << tu_thresh
593 <<
")!" << std::endl;
594 std::cout <<
"t_err: " << t_err2 <<
" ; tu_err: " << tu_err2 << std::endl;
595 correct_accuracy =
false;
611 char buffer[FILENAME_MAX];
612 std::ostringstream oss;
613 oss <<
"results/image_%04d.png";
614 snprintf(buffer, FILENAME_MAX, oss.str().c_str(), cpt_frame);
617 results.insert(resultsDepth,
vpImagePoint(0, resultsColor.getWidth()));
623 if (opt_display && opt_click_allowed) {
644 if (!time_vec.empty())
649 if (!vec_err_t.empty())
650 std::cout <<
"Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
652 if (!vec_err_tu.empty())
653 std::cout <<
"Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
655 std::cout <<
"Test result: " << (correct_accuracy ?
"success" :
"failure") << std::endl;
656 return correct_accuracy ? EXIT_SUCCESS : EXIT_FAILURE;
660 int main(
int argc,
const char *argv[])
663 std::string env_ipath;
664 std::string opt_ipath =
"";
665 bool opt_click_allowed =
true;
666 bool opt_display =
true;
667 bool opt_save =
false;
668 bool useScanline =
false;
670 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
672 int opt_lastFrame = 5;
674 int opt_lastFrame = -1;
676 bool use_depth =
false;
677 bool use_mask =
false;
678 bool use_color_image =
false;
685 if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_save, useScanline, trackerType_image,
686 opt_lastFrame, use_depth, use_mask, use_color_image)) {
690 #if ! (defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO))
691 if (trackerType_image == 2 || trackerType_image == 3) {
692 std::cout <<
"Using klt tracker is not possible without OpenCV imgproc and video modules." << std::endl;
693 std::cout <<
"Use rather command line option -t 1 to use edges." << std::endl;
697 std::cout <<
"trackerType_image: " << trackerType_image << std::endl;
698 std::cout <<
"useScanline: " << useScanline << std::endl;
699 std::cout <<
"use_depth: " << use_depth << std::endl;
700 std::cout <<
"use_mask: " << use_mask << std::endl;
701 std::cout <<
"use_color_image: " << use_color_image << std::endl;
702 #ifdef VISP_HAVE_COIN3D
703 std::cout <<
"COIN3D available." << std::endl;
706 #if !defined(VISP_HAVE_MODULE_KLT) || (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100))
707 if (trackerType_image & 2) {
708 std::cout <<
"KLT features cannot be used: ViSP is not built with "
709 "KLT module or OpenCV is not available.\nTest is not run."
716 if (opt_ipath.empty() && env_ipath.empty()) {
717 usage(argv[0],
nullptr);
718 std::cerr << std::endl <<
"ERROR:" << std::endl;
719 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
720 <<
" environment variable to specify the location of the " << std::endl
721 <<
" image path where test images are located." << std::endl
727 std::string input_directory =
730 std::cerr <<
"ViSP-images does not contain the folder: " << input_directory <<
"!" << std::endl;
734 if (use_color_image) {
735 return run<vpRGBa>(input_directory, opt_click_allowed, opt_display, useScanline, trackerType_image, opt_lastFrame,
736 use_depth, use_mask, opt_save);
739 return run<unsigned char>(input_directory, opt_click_allowed, opt_display, useScanline, trackerType_image,
740 opt_lastFrame, use_depth, use_mask, opt_save);
743 std::cout <<
"Test succeed" << std::endl;
747 std::cout <<
"Catch an exception: " << e << std::endl;
751 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
754 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
760 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="") vp_override
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 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)
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.
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
VISP_EXPORT double measureTimeMs()