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/vpIoTools.h>
54#include <visp3/core/vpImageDraw.h>
55#include <visp3/core/vpFont.h>
56#include <visp3/io/vpParseArgv.h>
57#include <visp3/io/vpImageIo.h>
58#include <visp3/gui/vpDisplayX.h>
59#include <visp3/gui/vpDisplayGDI.h>
60#include <visp3/gui/vpDisplayOpenCV.h>
61#include <visp3/gui/vpDisplayD3D.h>
62#include <visp3/gui/vpDisplayGTK.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", name);
80 -i <input image path> \n\
81 Set image input path.\n\
82 These images come from ViSP-images-x.y.z.tar.gz available \n\
83 on the ViSP website.\n\
84 Setting the VISP_INPUT_IMAGE_PATH environment\n\
85 variable produces the same behavior than using\n\
89 Turn off the display.\n\
92 If display is turn off, tracking results are saved in a video folder.\n\
95 Disable the mouse click. Useful to automate the \n\
96 execution of this program without human intervention.\n\
99 Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
102 Use the scanline for visibility tests.\n\
104 -e <last frame index>\n\
105 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
111 Set a tracking mask.\n\
117 Print the help.\n\n");
120 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
123 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
bool &save,
124 bool &useScanline,
int &trackerType,
int &lastFrame,
bool &use_depth,
bool &use_mask,
125 bool &use_color_image)
136 click_allowed =
false;
148 trackerType = atoi(optarg_);
151 lastFrame = atoi(optarg_);
160 use_color_image =
true;
163 usage(argv[0], NULL);
168 usage(argv[0], optarg_);
174 if ((c == 1) || (c == -1)) {
176 usage(argv[0], NULL);
177 std::cerr <<
"ERROR: " << std::endl;
178 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
185 template <
typename Type>
186 bool read_data(
const std::string &input_directory,
int cpt,
const vpCameraParameters &cam_depth,
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!");
195 sprintf(buffer, std::string(input_directory +
"/Images/Image_%04d.pgm").c_str(), cpt);
196 std::string image_filename = buffer;
198 sprintf(buffer, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
199 std::string depth_filename = buffer;
201 sprintf(buffer, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
202 std::string pose_filename = buffer;
210 unsigned int depth_width = 0, depth_height = 0;
211 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
212 if (!file_depth.is_open())
217 I_depth.
resize(depth_height, depth_width);
218 pointcloud.resize(depth_height*depth_width);
220 const float depth_scale = 0.000030518f;
221 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
222 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
224 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
230 pointcloud[i*I_depth.
getWidth()+j] = pt3d;
234 std::ifstream file_pose(pose_filename.c_str());
235 if (!file_pose.is_open()) {
239 for (
unsigned int i = 0; i < 4; i++) {
240 for (
unsigned int j = 0; j < 4; j++) {
241 file_pose >> cMo[i][j];
258 template <
typename Type>
259 bool run(
const std::string &input_directory,
260 bool opt_click_allowed,
bool opt_display,
bool useScanline,
int trackerType_image,
261 int opt_lastFrame,
bool use_depth,
bool use_mask,
bool save) {
262#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
263 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
264 "Template function supports only unsigned char and vpRGBa images!");
267#if defined VISP_HAVE_X11
269#elif defined VISP_HAVE_GDI
271#elif defined VISP_HAVE_OPENCV
273#elif defined VISP_HAVE_D3D9
275#elif defined VISP_HAVE_GTK
281 std::vector<int> tracker_type(2);
282 tracker_type[0] = trackerType_image;
285 std::string configFileCam1 = input_directory + std::string(
"/Config/chateau.xml");
286 std::string configFileCam2 = input_directory + std::string(
"/Config/chateau_depth.xml");
287 std::cout <<
"Load config file for camera 1: " << configFileCam1 << std::endl;
288 std::cout <<
"Load config file for camera 2: " << configFileCam2 << std::endl;
289 tracker.loadConfigFile(configFileCam1, configFileCam2);
296 tracker.setCameraParameters(cam_color, cam_depth);
308 tracker.setMovingEdge(me);
311#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
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.7);
366#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
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.5) :
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) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
376 = std::pair<double, double>(0.002, 0.3);
378 = useScanline ? std::pair<double, double>(0.002, 1.8) :
std::pair<double, double>(0.002, 0.7);
382 = useScanline ? std::pair<double, double>(0.007, 2.3) :
std::pair<double, double>(0.007, 2.1);
383#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
385 = useScanline ? std::pair<double, double>(0.006, 1.7) :
std::pair<double, double>(0.005, 1.4);
387 = useScanline ? std::pair<double, double>(0.004, 1.2) :
std::pair<double, double>(0.004, 1.2);
390 = useScanline ? std::pair<double, double>(0.002, 0.7) :
std::pair<double, double>(0.001, 0.4);
391#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
393 = std::pair<double, double>(0.002, 0.3);
395 = useScanline ? std::pair<double, double>(0.001, 0.5) :
std::pair<double, double>(0.001, 0.4);
402 std::vector<vpColVector> pointcloud;
404 if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
405 std::cerr <<
"Cannot read first frame!" << std::endl;
410 const double roi_step = 7.0;
411 const double roi_step2 = 6.0;
414 for (
unsigned int i = (
unsigned int) (I.
getRows()/roi_step); i < (
unsigned int) (I.
getRows()*roi_step2/roi_step); i++) {
415 for (
unsigned int j = (
unsigned int) (I.
getCols()/roi_step); j < (
unsigned int) (I.
getCols()*roi_step2/roi_step); j++) {
419 tracker.setMask(mask);
431#ifdef VISP_HAVE_DISPLAY
432 display1.
init(I, 0, 0,
"Image");
438 depth_M_color[0][3] = -0.05;
439 tracker.setCameraTransformationMatrix(
"Camera2", depth_M_color);
440 tracker.initFromPose(I, cMo_truth);
443 bool click =
false, quit =
false, correct_accuracy =
true;
444 std::vector<double> vec_err_t, vec_err_tu;
445 std::vector<double> time_vec;
446 while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit
447 && (opt_lastFrame > 0 ? (
int)cpt_frame <= opt_lastFrame :
true)) {
454 convert(I, resultsColor);
455 convert(I_depth, resultsDepth);
459 std::map<std::string, const vpImage<Type> *> mapOfImages;
460 mapOfImages[
"Camera1"] = &I;
461 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
462 mapOfPointclouds[
"Camera2"] = &pointcloud;
463 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
465 mapOfWidths[
"Camera2"] = 0;
466 mapOfHeights[
"Camera2"] = 0;
468 mapOfWidths[
"Camera2"] = I_depth.
getWidth();
469 mapOfHeights[
"Camera2"] = I_depth.
getHeight();
472 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
475 time_vec.push_back(t);
478 tracker.display(I, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth,
vpColor::red, 3);
482 std::stringstream ss;
483 ss <<
"Frame: " << cpt_frame;
486 ss <<
"Nb features: " << tracker.getError().getRows();
490 std::map<std::string, std::vector<std::vector<double> > > mapOfModels;
491 std::map<std::string, unsigned int> mapOfW;
494 std::map<std::string, unsigned int> mapOfH;
495 mapOfH[
"Camera1"] = I_depth.
getWidth();
497 std::map<std::string, vpHomogeneousMatrix> mapOfcMos;
498 mapOfcMos[
"Camera1"] = cMo;
499 mapOfcMos[
"Camera2"] = depth_M_color*cMo;
500 std::map<std::string, vpCameraParameters> mapOfCams;
501 mapOfCams[
"Camera1"] = cam_color;
502 mapOfCams[
"Camera2"] = cam_depth;
503 tracker.getModelForDisplay(mapOfModels, mapOfW, mapOfH, mapOfcMos, mapOfCams);
504 for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfModels.begin();
505 it != mapOfModels.end(); ++it) {
506 for (
size_t i = 0; i < it->second.size(); i++) {
508 if (std::fabs(it->second[i][0]) <= std::numeric_limits<double>::epsilon()) {
517 std::map<std::string, std::vector<std::vector<double> > > mapOfFeatures;
518 tracker.getFeaturesForDisplay(mapOfFeatures);
519 for (std::map<std::string, std::vector<std::vector<double> > >::const_iterator it = mapOfFeatures.begin();
520 it != mapOfFeatures.end(); ++it) {
521 for (
size_t i = 0; i < it->second.size(); i++) {
522 if (std::fabs(it->second[i][0]) <= std::numeric_limits<double>::epsilon()) {
524 if (std::fabs(it->second[i][3]) <= std::numeric_limits<double>::epsilon()) {
526 }
else if (std::fabs(it->second[i][3] - 1) <= std::numeric_limits<double>::epsilon()) {
528 }
else if (std::fabs(it->second[i][3] - 2) <= std::numeric_limits<double>::epsilon()) {
530 }
else if (std::fabs(it->second[i][3] - 3) <= std::numeric_limits<double>::epsilon()) {
532 }
else if (std::fabs(it->second[i][3] - 4) <= std::numeric_limits<double>::epsilon()) {
537 }
else if (std::fabs(it->second[i][0] - 1) <= std::numeric_limits<double>::epsilon()) {
546 std::ostringstream oss;
547 oss <<
"Tracking time: " << t <<
" ms";
555 for (
unsigned int i = 0; i < 3; i++) {
556 t_est[i] = pose_est[i];
557 t_truth[i] = pose_truth[i];
558 tu_est[i] = pose_est[i+3];
559 tu_truth[i] = pose_truth[i+3];
562 vpColVector t_err = t_truth-t_est, tu_err = tu_truth-tu_est;
566 vec_err_t.push_back( t_err2 );
567 vec_err_tu.push_back( tu_err2 );
568 if ( !use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh) ) {
569 std::cerr <<
"Pose estimated exceeds the threshold (t_thresh = " << t_thresh <<
" ; tu_thresh = " << tu_thresh <<
")!" << std::endl;
570 std::cout <<
"t_err: " << t_err2 <<
" ; tu_err: " << tu_err2 << std::endl;
571 correct_accuracy =
false;
587 std::ostringstream oss;
588 oss <<
"results/image_%04d.png";
589 sprintf(buffer, oss.str().c_str(), cpt_frame);
592 results.insert(resultsDepth,
vpImagePoint(0, resultsColor.getWidth()));
598 if (opt_display && opt_click_allowed) {
619 if (!time_vec.empty())
623 if (!vec_err_t.empty())
624 std::cout <<
"Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
626 if (!vec_err_tu.empty())
627 std::cout <<
"Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
629 return correct_accuracy ? EXIT_SUCCESS : EXIT_FAILURE;
633int main(
int argc,
const char *argv[])
636 std::string env_ipath;
637 std::string opt_ipath =
"";
638 bool opt_click_allowed =
true;
639 bool opt_display =
true;
640 bool opt_save =
false;
641 bool useScanline =
false;
643#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
645 int opt_lastFrame = 5;
647 int opt_lastFrame = -1;
649 bool use_depth =
false;
650 bool use_mask =
false;
651 bool use_color_image =
false;
658 if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, opt_save,
659 useScanline, trackerType_image, opt_lastFrame, use_depth,
660 use_mask, use_color_image)) {
664 std::cout <<
"trackerType_image: " << trackerType_image << std::endl;
665 std::cout <<
"useScanline: " << useScanline << std::endl;
666 std::cout <<
"use_depth: " << use_depth << std::endl;
667 std::cout <<
"use_mask: " << use_mask << std::endl;
668 std::cout <<
"use_color_image: " << use_color_image << std::endl;
669#ifdef VISP_HAVE_COIN3D
670 std::cout <<
"COIN3D available." << std::endl;
673#if !defined(VISP_HAVE_MODULE_KLT) || (!defined(VISP_HAVE_OPENCV) || (VISP_HAVE_OPENCV_VERSION < 0x020100))
674 if (trackerType_image & 2) {
675 std::cout <<
"KLT features cannot be used: ViSP is not built with "
676 "KLT module or OpenCV is not available.\nTest is not run."
683 if (opt_ipath.empty() && env_ipath.empty()) {
684 usage(argv[0], NULL);
685 std::cerr << std::endl <<
"ERROR:" << std::endl;
686 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
687 <<
" environment variable to specify the location of the " << std::endl
688 <<
" image path where test images are located." << std::endl
694 std::string input_directory =
vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath,
"mbt-depth/Castle-simu");
696 std::cerr <<
"ViSP-images does not contain the folder: " << input_directory <<
"!" << std::endl;
700 if (use_color_image) {
701 return run<vpRGBa>(input_directory, opt_click_allowed, opt_display, useScanline,
702 trackerType_image, opt_lastFrame, use_depth, use_mask, opt_save);
704 return run<unsigned char>(input_directory, opt_click_allowed, opt_display, useScanline,
705 trackerType_image, opt_lastFrame, use_depth, use_mask, opt_save);
708 std::cout <<
"Catch an exception: " << e << std::endl;
712#elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
715 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
720 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 functionnalities.
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 flush(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))
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 emited 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 setMaskSize(const unsigned int &a)
void setMu2(const double &mu_2)
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()