Regression test for depth MBT.
#include <cstdlib>
#include <iostream>
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_MODULE_MBT)
#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/mbt/vpMbGenericTracker.h>
#define GETOPTARGS "i:dcle:mh"
namespace
{
void usage(const char *name, const char *badparam)
{
fprintf(stdout, "\n\
Regression test for vpGenericTracker and depth.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-c] [-d] [-h] [-l] \n\
[-e <last frame index>] [-m]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
These images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behavior than using\n\
this option.\n\
\n\
-d \n\
Turn off the display.\n\
\n\
-c\n\
Disable the mouse click. Useful to automate the \n\
execution of this program without human intervention.\n\
\n\
-l\n\
Use the scanline for visibility tests.\n\
\n\
-e <last frame index>\n\
Specify the index of the last frame. Once reached, the tracking is stopped.\n\
\n\
-m \n\
Set a tracking mask.\n\
\n\
-h \n\
Print the help.\n\n");
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display,
bool &useScanline, int &lastFrame, bool &use_mask)
{
const char *optarg_;
int c;
switch (c) {
case 'i':
ipath = optarg_;
break;
case 'c':
click_allowed = false;
break;
case 'd':
display = false;
break;
case 'l':
useScanline = true;
break;
case 'e':
lastFrame = atoi(optarg_);
break;
case 'm':
use_mask = true;
break;
case 'h':
usage(argv[0], NULL);
return false;
break;
default:
usage(argv[0], optarg_);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
usage(argv[0], NULL);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
}
return true;
}
bool read_data(
const std::string &input_directory,
const int cpt,
const vpCameraParameters &cam_depth,
{
char buffer[256];
sprintf(buffer, std::string(input_directory + "/Images/Image_%04d.pgm").c_str(), cpt);
std::string image_filename = buffer;
sprintf(buffer, std::string(input_directory + "/Depth/Depth_%04d.bin").c_str(), cpt);
std::string depth_filename = buffer;
sprintf(buffer, std::string(input_directory + "/CameraPose/Camera_%03d.txt").c_str(), cpt);
std::string pose_filename = buffer;
return false;
unsigned int depth_width = 0, depth_height = 0;
std::ifstream file_depth(depth_filename.c_str(), std::ios::in | std::ios::binary);
if (!file_depth.is_open())
return false;
I_depth.
resize(depth_height, depth_width);
pointcloud.resize(depth_height*depth_width);
const float depth_scale = 0.000030518f;
for (
unsigned int i = 0; i < I_depth.
getHeight(); i++) {
for (
unsigned int j = 0; j < I_depth.
getWidth(); j++) {
double x = 0.0, y = 0.0, Z = I_depth[i][j] * depth_scale;
pt3d[0] = x*Z;
pt3d[1] = y*Z;
pt3d[2] = Z;
pointcloud[i*I_depth.
getWidth()+j] = pt3d;
}
}
std::ifstream file_pose(pose_filename.c_str());
if (!file_pose.is_open()) {
return false;
}
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int j = 0; j < 4; j++) {
file_pose >> cMo[i][j];
}
}
return true;
}
}
int main(int argc, const char *argv[])
{
try {
std::string env_ipath;
std::string opt_ipath = "";
bool opt_click_allowed = true;
bool opt_display = true;
bool useScanline = false;
#if defined(__mips__) || defined(__mips) || defined(mips) || defined(__MIPS__)
int opt_lastFrame = 5;
#else
int opt_lastFrame = -1;
#endif
bool use_mask = false;
if (!getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display,
useScanline, opt_lastFrame, use_mask)) {
return EXIT_FAILURE;
}
std::cout << "useScanline: " << useScanline << std::endl;
std::cout << "use_mask: " << use_mask << std::endl;
if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
return EXIT_FAILURE;
}
std::string input_directory =
vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath,
"mbt-depth/Castle-simu");
std::cerr << "ViSP-images does not contain the folder: " << input_directory << "!" << std::endl;
return EXIT_SUCCESS;
}
#if defined VISP_HAVE_X11
#elif defined VISP_HAVE_GDI
#elif defined VISP_HAVE_OPENCV
#elif defined VISP_HAVE_D3D9
#elif defined VISP_HAVE_GTK
#else
opt_display = false;
#endif
std::vector<int> tracker_type;
#if defined(VISP_HAVE_XML2)
tracker.
loadConfigFile(input_directory +
"/Config/chateau_depth.xml");
#else
{
}
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
#endif
#endif
tracker.
loadModel(input_directory +
"/Models/chateau.cao");
T[0][0] = -1;
T[0][3] = -0.2;
T[1][1] = 0;
T[1][2] = 1;
T[1][3] = 0.12;
T[2][1] = 1;
T[2][2] = 0;
T[2][3] = -0.15;
tracker.
loadModel(input_directory +
"/Models/cube.cao",
false, T);
std::vector<vpColVector> pointcloud;
int cpt_frame = 1;
if (!read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth)) {
std::cerr << "Cannot read first frame!" << std::endl;
return EXIT_FAILURE;
}
const double roi_step = 7.0;
const double roi_step2 = 6.0;
if (use_mask) {
mask = false;
for (
unsigned int i = (
unsigned int) (I.
getRows()/roi_step); i < (
unsigned int) (I.
getRows()*roi_step2/roi_step); i++) {
for (
unsigned int j = (
unsigned int) (I.
getCols()/roi_step); j < (
unsigned int) (I.
getCols()*roi_step2/roi_step); j++) {
mask[i][j] = true;
}
}
}
if (opt_display) {
#ifdef VISP_HAVE_DISPLAY
display1.
init(I, 0, 0,
"Image");
#endif
}
depth_M_color[0][3] = -0.05;
bool click = false, quit = false;
std::vector<double> vec_err_t, vec_err_tu;
std::vector<double> time_vec;
while (read_data(input_directory, cpt_frame, cam_depth, I, I_depth_raw, pointcloud, cMo_truth) && !quit
&& (opt_lastFrame > 0 ? (int)cpt_frame <= opt_lastFrame : true)) {
if (opt_display) {
}
std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
mapOfPointclouds["Camera"] = &pointcloud;
std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
mapOfWidths[
"Camera"] = I_depth.
getWidth();
mapOfHeights[
"Camera"] = I_depth.
getHeight();
tracker.
track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
time_vec.push_back(t);
if (opt_display) {
std::stringstream ss;
ss << "Frame: " << cpt_frame;
ss.str("");
}
for (int i = 0; i < 3; i++) {
t_est[i] = pose_est[i];
t_truth[i] = pose_truth[i];
tu_est[i] = pose_est[i+3];
tu_truth[i] = pose_truth[i+3];
}
vpColVector t_err = t_truth-t_est, tu_err = tu_truth-tu_est;
vec_err_t.push_back( t_err2 );
vec_err_tu.push_back( tu_err2 );
const double t_thresh = useScanline ? 0.003 : 0.002;
const double tu_thresh = useScanline ? 0.5 : 0.4;
if ( !use_mask && (t_err2 > t_thresh || tu_err2 > tu_thresh) ) {
std::cerr << "Pose estimated exceeds the threshold (t_thresh = 0.003, tu_thresh = 0.5)!" << std::endl;
std::cout <<
"t_err: " << sqrt(t_err.
sumSquare()) <<
" ; tu_err: " <<
vpMath::deg(sqrt(tu_err.sumSquare())) << std::endl;
return EXIT_FAILURE;
}
if (opt_display) {
if (use_mask) {
}
}
if (opt_display && opt_click_allowed) {
switch (button) {
quit = !click;
break;
click = !click;
break;
default:
break;
}
}
}
cpt_frame++;
}
if (!time_vec.empty())
if (!vec_err_t.empty())
std::cout << "Max translation error: " << *std::max_element(vec_err_t.begin(), vec_err_t.end()) << std::endl;
if (!vec_err_tu.empty())
std::cout << "Max thetau error: " << *std::max_element(vec_err_tu.begin(), vec_err_tu.end()) << std::endl;
#if defined(VISP_HAVE_XML2)
#endif
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}
#else
int main() {
std::cout << "Enable MBT module (VISP_HAVE_MODULE_MBT) to launch this test." << std::endl;
return 0;
}
#endif