44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_MODULE_MBT)
48 #include <visp3/core/vpIoTools.h>
49 #include <visp3/io/vpParseArgv.h>
50 #include <visp3/io/vpImageIo.h>
51 #include <visp3/gui/vpDisplayX.h>
52 #include <visp3/gui/vpDisplayGDI.h>
53 #include <visp3/gui/vpDisplayOpenCV.h>
54 #include <visp3/gui/vpDisplayD3D.h>
55 #include <visp3/gui/vpDisplayGTK.h>
56 #include <visp3/mbt/vpMbGenericTracker.h>
58 #define GETOPTARGS "i:dcle:mh"
62 void usage(
const char *name,
const char *badparam)
65 Regression test for vpGenericTracker and depth.\n\
68 %s [-i <test image path>] [-c] [-d] [-h] [-l] \n\
69 [-e <last frame index>] [-m]\n", name);
73 -i <input image path> \n\
74 Set image input path.\n\
75 These images come from ViSP-images-x.y.z.tar.gz available \n\
76 on the ViSP website.\n\
77 Setting the VISP_INPUT_IMAGE_PATH environment\n\
78 variable produces the same behavior than using\n\
82 Turn off the display.\n\
85 Disable the mouse click. Useful to automate the \n\
86 execution of this program without human intervention.\n\
89 Use the scanline for visibility tests.\n\
91 -e <last frame index>\n\
92 Specify the index of the last frame. Once reached, the tracking is stopped.\n\
95 Set a tracking mask.\n\
98 Print the help.\n\n");
101 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
104 bool getOptions(
int argc,
const char **argv, std::string &ipath,
bool &click_allowed,
bool &display,
105 bool &useScanline,
int &lastFrame,
bool &use_mask)
116 click_allowed =
false;
125 lastFrame = atoi(optarg_);
131 usage(argv[0], NULL);
136 usage(argv[0], optarg_);
142 if ((c == 1) || (c == -1)) {
144 usage(argv[0], NULL);
145 std::cerr <<
"ERROR: " << std::endl;
146 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
153 bool read_data(
const std::string &input_directory,
const int cpt,
const vpCameraParameters &cam_depth,
158 sprintf(buffer, std::string(input_directory +
"/Images/Image_%04d.pgm").c_str(), cpt);
159 std::string image_filename = buffer;
161 sprintf(buffer, std::string(input_directory +
"/Depth/Depth_%04d.bin").c_str(), cpt);
162 std::string depth_filename = buffer;
164 sprintf(buffer, std::string(input_directory +
"/CameraPose/Camera_%03d.txt").c_str(), cpt);
165 std::string pose_filename = buffer;
173 unsigned int depth_width = 0, depth_height = 0;
174 std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
175 if (!file_depth.is_open())
180 I_depth.
resize(depth_height, depth_width);
181 pointcloud.resize(depth_height*depth_width);
183 const float depth_scale = 0.000030518f;
184 for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
185 for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
187 double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
193 pointcloud[i*I_depth.
getWidth()+j] = pt3d;
197 std::ifstream file_pose(pose_filename.c_str());
198 if (!file_pose.is_open()) {
202 for (
unsigned int i = 0; i < 4; i++) {
203 for (
unsigned int j = 0; j < 4; j++) {
204 file_pose >> cMo[i][j];
212 int main(
int argc,
const char *argv[])
215 std::string env_ipath;
216 std::string opt_ipath =
"";
217 bool opt_click_allowed =
true;
218 bool opt_display =
true;
219 bool useScanline =
false;
220 #if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
222 int opt_lastFrame = 5;
224 int opt_lastFrame = -1;
226 bool use_mask =
false;
233 if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display,
234 useScanline, opt_lastFrame, use_mask)) {
238 std::cout <<
"useScanline: " << useScanline << std::endl;
239 std::cout <<
"use_mask: " << use_mask << std::endl;
242 if (opt_ipath.empty() && env_ipath.empty()) {
243 usage(argv[0], NULL);
244 std::cerr << std::endl <<
"ERROR:" << std::endl;
245 std::cerr <<
" Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
246 <<
" environment variable to specify the location of the " << std::endl
247 <<
" image path where test images are located." << std::endl
253 std::string input_directory =
vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath,
"mbt-depth/Castle-simu");
255 std::cerr <<
"ViSP-images does not contain the folder: " << input_directory <<
"!" << std::endl;
260 #if defined VISP_HAVE_X11
262 #elif defined VISP_HAVE_GDI
264 #elif defined VISP_HAVE_OPENCV
266 #elif defined VISP_HAVE_D3D9
268 #elif defined VISP_HAVE_GTK
274 std::vector<int> tracker_type;
277 #if defined(VISP_HAVE_XML2)
278 tracker.loadConfigFile(input_directory +
"/Config/chateau_depth.xml");
283 tracker.setCameraParameters(cam_depth);
287 tracker.setDepthNormalPclPlaneEstimationMethod(2);
288 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
289 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
290 tracker.setDepthNormalSamplingStep(2, 2);
292 tracker.setDepthDenseSamplingStep(4, 4);
294 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
295 tracker.setKltMaskBorder(5);
300 tracker.setNearClippingDistance(0.01);
301 tracker.setFarClippingDistance(2.0);
304 tracker.loadModel(input_directory +
"/Models/chateau.cao");
314 tracker.loadModel(input_directory +
"/Models/cube.cao",
false, T);
316 tracker.getCameraParameters(cam_depth);
317 tracker.setDisplayFeatures(
true);
318 tracker.setScanLineVisibilityTest(useScanline);
324 std::vector<vpColVector> pointcloud;
326 if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
327 std::cerr <<
"Cannot read first frame!" << std::endl;
332 const double roi_step = 7.0;
333 const double roi_step2 = 6.0;
336 for (
unsigned int i = (
unsigned int) (I.
getRows()/roi_step); i < (
unsigned int) (I.
getRows()*roi_step2/roi_step); i++) {
337 for (
unsigned int j = (
unsigned int) (I.
getCols()/roi_step); j < (
unsigned int) (I.
getCols()*roi_step2/roi_step); j++) {
341 tracker.setMask(mask);
346 #ifdef VISP_HAVE_DISPLAY
347 display1.
init(I, 0, 0,
"Image");
353 depth_M_color[0][3] = -0.05;
354 tracker.initFromPose(I, depth_M_color*cMo_truth);
356 bool click =
false, quit =
false;
357 std::vector<double> vec_err_t, vec_err_tu;
358 std::vector<double> time_vec;
359 while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit
360 && (opt_lastFrame > 0 ? (
int)cpt_frame <= opt_lastFrame :
true)) {
369 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
370 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
371 mapOfPointclouds[
"Camera"] = &pointcloud;
372 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
373 mapOfWidths[
"Camera"] = I_depth.
getWidth();
374 mapOfHeights[
"Camera"] = I_depth.
getHeight();
376 tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
379 time_vec.push_back(t);
382 tracker.display(I_depth, cMo, cam_depth,
vpColor::red, 3);
385 std::stringstream ss;
386 ss <<
"Frame: " << cpt_frame;
389 ss <<
"Nb features: " << tracker.getError().getRows();
397 for (
int i = 0; i < 3; i++) {
398 t_est[i] = pose_est[i];
399 t_truth[i] = pose_truth[i];
400 tu_est[i] = pose_est[i+3];
401 tu_truth[i] = pose_truth[i+3];
404 vpColVector t_err = t_truth-t_est, tu_err = tu_truth-tu_est;
406 vec_err_t.push_back( t_err2 );
407 vec_err_tu.push_back( tu_err2 );
408 const double t_thresh = useScanline ? 0.003 : 0.002;
409 const double tu_thresh = useScanline ? 0.5 : 0.4;
410 if ( !use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh) ) {
411 std::cerr <<
"Pose estimated exceeds the threshold (t_thresh = 0.003, tu_thresh = 0.5)!" << std::endl;
412 std::cout <<
"t_err: " << sqrt(t_err.
sumSquare()) <<
" ; tu_err: " <<
vpMath::deg(sqrt(tu_err.sumSquare())) << std::endl;
427 if (opt_display && opt_click_allowed) {
448 if (!time_vec.empty())
452 if (!vec_err_t.empty())
453 std::cout <<
"Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
455 if (!vec_err_tu.empty())
456 std::cout <<
"Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
458 #if defined(VISP_HAVE_XML2)
466 std::cout <<
"Catch an exception: " << e << std::endl;
472 std::cout <<
"Enable MBT module (VISP_HAVE_MODULE_MBT) to launch this test." << std::endl;