44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_MODULE_MBT) && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
48 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
49 #include <type_traits>
52 #include <visp3/core/vpIoTools.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/mbt/vpMbGenericTracker.h>
62 #define GETOPTARGS "i:dcle:mCh"
66 void usage(
const char *name,
const char *badparam)
69 Regression test for vpGenericTracker and depth.\n\
72 %s [-i <test image path>] [-c] [-d] [-h] [-l] \n\
73 [-e <last frame index>] [-m] [-C]\n",
78 -i <input image path> \n\
79 Set image input path.\n\
80 These images come from ViSP-images-x.y.z.tar.gz available \n\
81 on the ViSP website.\n\
82 Setting the VISP_INPUT_IMAGE_PATH environment\n\
83 variable produces the same behavior than using\n\
87 Turn off the display.\n\
90 Disable the mouse click. Useful to automate the \n\
91 execution of this program without human intervention.\n\
94 Use the scanline for visibility tests.\n\
96 -e <last frame index>\n\
97 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
100 Set a tracking mask.\n\
106 Print the help.\n\n");
109 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
112 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
bool &useScanline,
113 int &lastFrame,
bool &use_mask,
bool &use_color_image)
124 click_allowed =
false;
133 lastFrame = atoi(optarg_);
139 use_color_image =
true;
142 usage(argv[0],
nullptr);
147 usage(argv[0], optarg_);
153 if ((c == 1) || (c == -1)) {
155 usage(argv[0],
nullptr);
156 std::cerr <<
"ERROR: " << std::endl;
157 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
164 template <
typename Type>
168 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
169 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
170 "Template function supports only unsigned char and vpRGBa images!");
172 #if VISP_HAVE_DATASET_VERSION >= 0x030600
173 std::string ext(
"png");
175 std::string ext(
"pgm");
177 char buffer[FILENAME_MAX];
178 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/Images/Image_%04d." + ext).c_str(), cpt);
179 std::string image_filename = buffer;
181 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
182 std::string depth_filename = buffer;
184 snprintf(buffer, FILENAME_MAX, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
185 std::string pose_filename = buffer;
193 unsigned int depth_width = 0, depth_height = 0;
194 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
195 if (!file_depth.is_open())
200 I_depth.
resize(depth_height, depth_width);
201 pointcloud.resize(depth_height * depth_width);
203 const float depth_scale = 0.000030518f;
204 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
205 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
207 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
213 pointcloud[i * I_depth.
getWidth() + j] = pt3d;
217 std::ifstream file_pose(pose_filename.c_str());
218 if (!file_pose.is_open()) {
222 for (
unsigned int i = 0; i < 4; i++) {
223 for (
unsigned int j = 0; j < 4; j++) {
224 file_pose >> cMo[i][j];
231 template <
typename Type>
232 bool run(
vpImage<Type> &I,
const std::string &input_directory,
bool opt_click_allowed,
bool opt_display,
233 bool useScanline,
int opt_lastFrame,
bool use_mask)
235 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
236 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
237 "Template function supports only unsigned char and vpRGBa images!");
240 #if defined(VISP_HAVE_X11)
242 #elif defined(VISP_HAVE_GDI)
244 #elif defined(HAVE_OPENCV_HIGHGUI)
246 #elif defined(VISP_HAVE_D3D9)
248 #elif defined(VISP_HAVE_GTK)
254 std::vector<int> tracker_type;
258 #if defined(VISP_HAVE_PUGIXML)
259 tracker.loadConfigFile(input_directory +
"/Config/chateau_depth.xml");
265 tracker.setCameraParameters(cam_depth);
269 tracker.setDepthNormalPclPlaneEstimationMethod(2);
270 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
271 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
272 tracker.setDepthNormalSamplingStep(2, 2);
274 tracker.setDepthDenseSamplingStep(4, 4);
276 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
277 tracker.setKltMaskBorder(5);
282 tracker.setNearClippingDistance(0.01);
283 tracker.setFarClippingDistance(2.0);
286 tracker.loadModel(input_directory +
"/Models/chateau.cao");
296 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
298 tracker.getCameraParameters(cam_depth);
299 tracker.setDisplayFeatures(
true);
300 tracker.setScanLineVisibilityTest(useScanline);
305 std::vector<vpColVector> pointcloud;
307 if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
308 std::cerr <<
"Cannot read first frame!" << std::endl;
313 const double roi_step = 7.0;
314 const double roi_step2 = 6.0;
317 for (
unsigned int i = (
unsigned int)(I.
getRows() / roi_step);
318 i < (
unsigned int)(I.
getRows() * roi_step2 / roi_step); i++) {
319 for (
unsigned int j = (
unsigned int)(I.
getCols() / roi_step);
320 j < (
unsigned int)(I.
getCols() * roi_step2 / roi_step); j++) {
324 tracker.setMask(mask);
329 #ifdef VISP_HAVE_DISPLAY
330 display1.
init(I, 0, 0,
"Image");
336 depth_M_color[0][3] = -0.05;
337 tracker.initFromPose(I, depth_M_color * cMo_truth);
339 bool click =
false, quit =
false, correct_accuracy =
true;
340 std::vector<double> vec_err_t, vec_err_tu;
341 std::vector<double> time_vec;
342 while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit &&
343 (opt_lastFrame > 0 ? (
int)cpt_frame <= opt_lastFrame :
true)) {
352 std::map<std::string, const vpImage<Type> *> mapOfImages;
353 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
354 mapOfPointclouds[
"Camera"] = &pointcloud;
355 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
356 mapOfWidths[
"Camera"] = I_depth.
getWidth();
357 mapOfHeights[
"Camera"] = I_depth.
getHeight();
359 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
362 time_vec.push_back(t);
365 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
368 std::stringstream ss;
369 ss <<
"Frame: " << cpt_frame;
372 ss <<
"Nb features: " << tracker.getError().getRows();
380 for (
unsigned int i = 0; i < 3; i++) {
381 t_est[i] = pose_est[i];
382 t_truth[i] = pose_truth[i];
383 tu_est[i] = pose_est[i + 3];
384 tu_truth[i] = pose_truth[i + 3];
387 vpColVector t_err = t_truth - t_est, tu_err = tu_truth - tu_est;
389 vec_err_t.push_back(t_err2);
390 vec_err_tu.push_back(tu_err2);
391 const double t_thresh = useScanline ? 0.003 : 0.002;
392 const double tu_thresh = useScanline ? 0.5 : 0.4;
393 if (!use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh)) {
394 std::cerr <<
"Pose estimated exceeds the threshold (t_thresh = " << t_thresh <<
", tu_thresh = " << tu_thresh
395 <<
")!" << std::endl;
396 std::cout <<
"t_err: " << t_err2 <<
" ; tu_err: " << tu_err2 << std::endl;
397 correct_accuracy =
false;
411 if (opt_display && opt_click_allowed) {
432 if (!time_vec.empty())
437 if (!vec_err_t.empty())
438 std::cout <<
"Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
440 if (!vec_err_tu.empty())
441 std::cout <<
"Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
443 return correct_accuracy ? EXIT_SUCCESS : EXIT_FAILURE;
447 int main(
int argc,
const char *argv[])
450 std::string env_ipath;
451 std::string opt_ipath =
"";
452 bool opt_click_allowed =
true;
453 bool opt_display =
true;
454 bool useScanline =
false;
455 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
457 int opt_lastFrame = 5;
459 int opt_lastFrame = -1;
461 bool use_mask =
false;
462 bool use_color_image =
false;
469 if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display, useScanline, opt_lastFrame, use_mask,
474 std::cout <<
"useScanline: " << useScanline << std::endl;
475 std::cout <<
"use_mask: " << use_mask << std::endl;
476 std::cout <<
"use_color_image: " << use_color_image << std::endl;
479 if (opt_ipath.empty() && env_ipath.empty()) {
480 usage(argv[0],
nullptr);
481 std::cerr << std::endl <<
"ERROR:" << std::endl;
482 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
483 <<
" environment variable to specify the location of the " << std::endl
484 <<
" image path where test images are located." << std::endl
490 std::string input_directory =
493 std::cerr <<
"ViSP-images does not contain the folder: " << input_directory <<
"!" << std::endl;
497 if (use_color_image) {
499 return run(I_color, input_directory, opt_click_allowed, opt_display, useScanline, opt_lastFrame, use_mask);
503 return run(I_gray, input_directory, opt_click_allowed, opt_display, useScanline, opt_lastFrame, use_mask);
507 std::cout <<
"Catch an exception: " << e << std::endl;
511 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
514 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
520 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.
static const vpColor none
static const vpColor yellow
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.
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 read(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
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
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()