44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_MODULE_MBT)
48 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
49 #include <type_traits>
52 #include <visp3/core/vpIoTools.h>
53 #include <visp3/io/vpParseArgv.h>
54 #include <visp3/io/vpImageIo.h>
55 #include <visp3/gui/vpDisplayX.h>
56 #include <visp3/gui/vpDisplayGDI.h>
57 #include <visp3/gui/vpDisplayOpenCV.h>
58 #include <visp3/gui/vpDisplayD3D.h>
59 #include <visp3/gui/vpDisplayGTK.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", name);
77 -i <input image path> \n\
78 Set image input path.\n\
79 These images come from ViSP-images-x.y.z.tar.gz available \n\
80 on the ViSP website.\n\
81 Setting the VISP_INPUT_IMAGE_PATH environment\n\
82 variable produces the same behavior than using\n\
86 Turn off the display.\n\
89 Disable the mouse click. Useful to automate the \n\
90 execution of this program without human intervention.\n\
93 Use the scanline for visibility tests.\n\
95 -e <last frame index>\n\
96 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
99 Set a tracking mask.\n\
105 Print the help.\n\n");
108 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
111 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
112 bool &useScanline,
int &lastFrame,
bool &use_mask,
bool &use_color_image)
123 click_allowed =
false;
132 lastFrame = atoi(optarg_);
138 use_color_image =
true;
141 usage(argv[0], NULL);
146 usage(argv[0], optarg_);
152 if ((c == 1) || (c == -1)) {
154 usage(argv[0], NULL);
155 std::cerr <<
"ERROR: " << std::endl;
156 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
163 template <
typename Type>
164 bool read_data(
const std::string &input_directory,
int cpt,
const vpCameraParameters &cam_depth,
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!");
173 sprintf(buffer, std::string(input_directory +
"/Images/Image_%04d.pgm").c_str(), cpt);
174 std::string image_filename = buffer;
176 sprintf(buffer, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
177 std::string depth_filename = buffer;
179 sprintf(buffer, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
180 std::string pose_filename = buffer;
188 unsigned int depth_width = 0, depth_height = 0;
189 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
190 if (!file_depth.is_open())
195 I_depth.
resize(depth_height, depth_width);
196 pointcloud.resize(depth_height*depth_width);
198 const float depth_scale = 0.000030518f;
199 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
200 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
202 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
208 pointcloud[i*I_depth.
getWidth()+j] = pt3d;
212 std::ifstream file_pose(pose_filename.c_str());
213 if (!file_pose.is_open()) {
217 for (
unsigned int i = 0; i < 4; i++) {
218 for (
unsigned int j = 0; j < 4; j++) {
219 file_pose >> cMo[i][j];
226 template <
typename Type>
227 bool run(
vpImage<Type> &I,
const std::string &input_directory,
bool opt_click_allowed,
228 bool opt_display,
bool useScanline,
int opt_lastFrame,
bool use_mask) {
229 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
230 static_assert(std::is_same<Type, unsigned char>::value || std::is_same<Type, vpRGBa>::value,
231 "Template function supports only unsigned char and vpRGBa images!");
234 #if defined VISP_HAVE_X11
236 #elif defined VISP_HAVE_GDI
238 #elif defined VISP_HAVE_OPENCV
240 #elif defined VISP_HAVE_D3D9
242 #elif defined VISP_HAVE_GTK
248 std::vector<int> tracker_type;
251 #if defined(VISP_HAVE_PUGIXML)
252 tracker.loadConfigFile(input_directory +
"/Config/chateau_depth.xml");
257 tracker.setCameraParameters(cam_depth);
261 tracker.setDepthNormalPclPlaneEstimationMethod(2);
262 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
263 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
264 tracker.setDepthNormalSamplingStep(2, 2);
266 tracker.setDepthDenseSamplingStep(4, 4);
268 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
269 tracker.setKltMaskBorder(5);
274 tracker.setNearClippingDistance(0.01);
275 tracker.setFarClippingDistance(2.0);
278 tracker.loadModel(input_directory +
"/Models/chateau.cao");
288 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
290 tracker.getCameraParameters(cam_depth);
291 tracker.setDisplayFeatures(
true);
292 tracker.setScanLineVisibilityTest(useScanline);
297 std::vector<vpColVector> pointcloud;
299 if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
300 std::cerr <<
"Cannot read first frame!" << std::endl;
305 const double roi_step = 7.0;
306 const double roi_step2 = 6.0;
309 for (
unsigned int i = (
unsigned int) (I.
getRows()/roi_step); i < (
unsigned int) (I.
getRows()*roi_step2/roi_step); i++) {
310 for (
unsigned int j = (
unsigned int) (I.
getCols()/roi_step); j < (
unsigned int) (I.
getCols()*roi_step2/roi_step); j++) {
314 tracker.setMask(mask);
319 #ifdef VISP_HAVE_DISPLAY
320 display1.
init(I, 0, 0,
"Image");
326 depth_M_color[0][3] = -0.05;
327 tracker.initFromPose(I, depth_M_color*cMo_truth);
329 bool click =
false, quit =
false;
330 std::vector<double> vec_err_t, vec_err_tu;
331 std::vector<double> time_vec;
332 while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit
333 && (opt_lastFrame > 0 ? (
int)cpt_frame <= opt_lastFrame :
true)) {
342 std::map<std::string, const vpImage<Type> *> mapOfImages;
343 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
344 mapOfPointclouds[
"Camera"] = &pointcloud;
345 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
346 mapOfWidths[
"Camera"] = I_depth.
getWidth();
347 mapOfHeights[
"Camera"] = I_depth.
getHeight();
349 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
352 time_vec.push_back(t);
355 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
358 std::stringstream ss;
359 ss <<
"Frame: " << cpt_frame;
362 ss <<
"Nb features: " << tracker.getError().getRows();
370 for (
unsigned int i = 0; i < 3; i++) {
371 t_est[i] = pose_est[i];
372 t_truth[i] = pose_truth[i];
373 tu_est[i] = pose_est[i+3];
374 tu_truth[i] = pose_truth[i+3];
377 vpColVector t_err = t_truth-t_est, tu_err = tu_truth-tu_est;
379 vec_err_t.push_back( t_err2 );
380 vec_err_tu.push_back( tu_err2 );
381 const double t_thresh = useScanline ? 0.003 : 0.002;
382 const double tu_thresh = useScanline ? 0.5 : 0.4;
383 if ( !use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh) ) {
384 std::cerr <<
"Pose estimated exceeds the threshold (t_thresh = 0.003, tu_thresh = 0.5)!" << std::endl;
385 std::cout <<
"t_err: " << sqrt(t_err.
sumSquare()) <<
" ; tu_err: " <<
vpMath::deg(sqrt(tu_err.sumSquare())) << std::endl;
401 if (opt_display && opt_click_allowed) {
422 if (!time_vec.empty())
426 if (!vec_err_t.empty())
427 std::cout <<
"Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
429 if (!vec_err_tu.empty())
430 std::cout <<
"Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
436 int main(
int argc,
const char *argv[])
439 std::string env_ipath;
440 std::string opt_ipath =
"";
441 bool opt_click_allowed =
true;
442 bool opt_display =
true;
443 bool useScanline =
false;
444 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
446 int opt_lastFrame = 5;
448 int opt_lastFrame = -1;
450 bool use_mask =
false;
451 bool use_color_image =
false;
458 if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display,
459 useScanline, opt_lastFrame, use_mask, use_color_image)) {
463 std::cout <<
"useScanline: " << useScanline << std::endl;
464 std::cout <<
"use_mask: " << use_mask << std::endl;
465 std::cout <<
"use_color_image: " << use_color_image << std::endl;
468 if (opt_ipath.empty() && env_ipath.empty()) {
469 usage(argv[0], NULL);
470 std::cerr << std::endl <<
"ERROR:" << std::endl;
471 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
472 <<
" environment variable to specify the location of the " << std::endl
473 <<
" image path where test images are located." << std::endl
479 std::string input_directory =
vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath,
"mbt-depth/Castle-simu");
481 std::cerr <<
"ViSP-images does not contain the folder: " << input_directory <<
"!" << std::endl;
485 if (use_color_image) {
487 return run(I_color, input_directory, opt_click_allowed, opt_display, useScanline, opt_lastFrame, use_mask);
490 return run(I_gray, input_directory, opt_click_allowed, opt_display, useScanline, opt_lastFrame, use_mask);
493 std::cout <<
"Catch an exception: " << e << std::endl;
499 std::cout <<
"Enable MBT module (VISP_HAVE_MODULE_MBT) to launch this test." << std::endl;