Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
tutorial-mb-generic-tracker-rgbd-blender.cpp
1 #include <iostream>
3 
4 #include <visp3/core/vpDisplay.h>
5 #include <visp3/core/vpIoTools.h>
6 #include <visp3/core/vpXmlParserCamera.h>
7 #include <visp3/gui/vpDisplayGDI.h>
8 #include <visp3/gui/vpDisplayOpenCV.h>
9 #include <visp3/gui/vpDisplayX.h>
10 #include <visp3/io/vpImageIo.h>
11 #include <visp3/mbt/vpMbGenericTracker.h>
12 
13 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) && defined(VISP_HAVE_PUGIXML)
14 namespace
15 {
16 bool read_data(unsigned int cpt, const std::string &video_color_images, const std::string &video_depth_images,
17  bool disable_depth, const std::string &video_ground_truth,
19  unsigned int &depth_width, unsigned int &depth_height,
20  std::vector<vpColVector> &pointcloud, const vpCameraParameters &cam_depth,
21  vpHomogeneousMatrix &cMo_ground_truth)
22 {
23  char buffer[FILENAME_MAX];
24  // Read color
25  snprintf(buffer, FILENAME_MAX, video_color_images.c_str(), cpt);
26  std::string filename_color = buffer;
27 
28  if (!vpIoTools::checkFilename(filename_color)) {
29  std::cerr << "Cannot read: " << filename_color << std::endl;
30  return false;
31  }
32  vpImageIo::read(I, filename_color);
33 
34  if (!disable_depth) {
35  // Read depth
36  snprintf(buffer, FILENAME_MAX, video_depth_images.c_str(), cpt);
37  std::string filename_depth = buffer;
38 
39  if (!vpIoTools::checkFilename(filename_depth)) {
40  std::cerr << "Cannot read: " << filename_depth << std::endl;
41  return false;
42  }
43  cv::Mat depth_raw = cv::imread(filename_depth, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
44  if (depth_raw.empty()) {
45  std::cerr << "Cannot read: " << filename_depth << std::endl;
46  return false;
47  }
48 
49  depth_width = static_cast<unsigned int>(depth_raw.cols);
50  depth_height = static_cast<unsigned int>(depth_raw.rows);
51  I_depth_raw.resize(depth_height, depth_width);
52  pointcloud.resize(depth_width * depth_height);
53 
54  for (int i = 0; i < depth_raw.rows; i++) {
55  for (int j = 0; j < depth_raw.cols; j++) {
56  I_depth_raw[i][j] = static_cast<uint16_t>(32767.5f * depth_raw.at<cv::Vec3f>(i, j)[0]);
57  double x = 0.0, y = 0.0;
58  // Manually limit the field of view of the depth camera
59  double Z = depth_raw.at<cv::Vec3f>(i, j)[0] > 2.0f ? 0.0 : static_cast<double>(depth_raw.at<cv::Vec3f>(i, j)[0]);
60  vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
61  size_t idx = static_cast<size_t>(i * depth_raw.cols + j);
62  pointcloud[idx].resize(3);
63  pointcloud[idx][0] = x * Z;
64  pointcloud[idx][1] = y * Z;
65  pointcloud[idx][2] = Z;
66  }
67  }
68  }
69 
70  // Read ground truth
71  snprintf(buffer, FILENAME_MAX, video_ground_truth.c_str(), cpt);
72  std::string filename_pose = buffer;
73 
74  cMo_ground_truth.load(filename_pose);
75 
76  return true;
77 }
78 } // namespace
79 
80 void usage(const char **argv, int error, const std::string &data_path, const std::string &model_path, int first_frame)
81 {
82  std::cout << "Synopsis" << std::endl
83  << " " << argv[0]
84  << " [--data-path <path>] [--model-path <path>] [--first-frame <index>] [--disable-depth] "
85  << " [--disable-klt] [--step-by-step] [--display-ground-truth] [--help, -h]" << std::endl
86  << std::endl;
87  std::cout << "Description" << std::endl
88  << " --data-path <path> Path to the data generated by Blender get_camera_pose_teabox.py" << std::endl
89  << " Python script."
90  << " Default: " << data_path << std::endl
91  << std::endl
92  << " --model-path <path> Path to the cad model and tracker settings." << std::endl
93  << " Default: " << model_path << std::endl
94  << std::endl
95  << " --first-frame <index> First frame number to process." << std::endl
96  << " Default: " << first_frame << std::endl
97  << std::endl
98  << " --disable-depth Flag to turn off tracker depth features." << std::endl
99  << std::endl
100  << " --disable-klt Flag to turn off tracker keypoints features." << std::endl
101  << std::endl
102  << " --step-by-step Flag to enable step by step mode." << std::endl
103  << std::endl
104  << " --display-ground-truth Flag to enable displaying ground truth." << std::endl
105  << " When this flag is enabled, there is no tracking. This flag is useful" << std::endl
106  << " to validate the ground truth over the rendered images." << std::endl
107  << std::endl
108  << " --help, -h Print this helper message." << std::endl
109  << std::endl;
110  if (error) {
111  std::cout << "Error" << std::endl
112  << " "
113  << "Unsupported parameter " << argv[error] << std::endl;
114  }
115 }
116 
117 int main(int argc, const char **argv)
118 {
119  std::string opt_data_path = "data/teabox";
120  std::string opt_model_path = "model/teabox";
121  unsigned int opt_first_frame = 1;
122  bool opt_disable_depth = false;
123  bool opt_disable_klt = false;
124  bool opt_display_ground_truth = false;
125  bool opt_step_by_step = false;
126 
127  for (int i = 1; i < argc; i++) {
128  if (std::string(argv[i]) == "--data-path" && i + 1 < argc) {
129  opt_data_path = std::string(argv[i + 1]);
130  i++;
131  }
132  else if (std::string(argv[i]) == "--model-path" && i + 1 < argc) {
133  opt_model_path = std::string(argv[i + 1]);
134  i++;
135  }
136  else if (std::string(argv[i]) == "--disable-depth") {
137  opt_disable_depth = true;
138  }
139  else if (std::string(argv[i]) == "--disable-klt") {
140  opt_disable_klt = true;
141  }
142  else if (std::string(argv[i]) == "--display-ground-truth") {
143  opt_display_ground_truth = true;
144  }
145  else if (std::string(argv[i]) == "--step-by-step") {
146  opt_step_by_step = true;
147  }
148  else if (std::string(argv[i]) == "--first-frame" && i + 1 < argc) {
149  opt_first_frame = static_cast<unsigned int>(atoi(argv[i + 1]));
150  i++;
151  }
152  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
153  usage(argv, 0, opt_data_path, opt_model_path, opt_first_frame);
154  return EXIT_SUCCESS;
155  }
156  else {
157  usage(argv, i, opt_data_path, opt_model_path, opt_first_frame);
158  return EXIT_FAILURE;
159  }
160  }
161 
162  std::string video_color_images = vpIoTools::createFilePath(opt_data_path, "color/%04d_L.jpg");
163  std::string video_depth_images = vpIoTools::createFilePath(opt_data_path, "depth/Image%04d_R.exr");
164  std::string ground_truth = vpIoTools::createFilePath(opt_data_path, "ground-truth/Camera_L_%04d.txt");
165  std::string extrinsic_file = vpIoTools::createFilePath(opt_data_path, "depth_M_color.txt");
166  std::string color_camera_name = "Camera_L";
167  std::string depth_camera_name = "Camera_R";
168  std::string color_intrinsic_file = vpIoTools::createFilePath(opt_data_path, color_camera_name + ".xml");
169  std::string depth_intrinsic_file = vpIoTools::createFilePath(opt_data_path, depth_camera_name + ".xml");
170  std::string mbt_config_color = vpIoTools::createFilePath(opt_model_path, "teabox_color.xml");
171  std::string mbt_config_depth = vpIoTools::createFilePath(opt_model_path, "teabox_depth.xml");
172  std::string mbt_cad_model = vpIoTools::createFilePath(opt_model_path, "teabox.cao");
173  std::string mbt_init_file = vpIoTools::createFilePath(opt_model_path, "teabox.init");
174 
175  std::cout << "Input data" << std::endl;
176  std::cout << " Color images : " << video_color_images << std::endl;
177  std::cout << " Depth images : " << (opt_disable_depth ? "Disabled" : video_depth_images) << std::endl;
178  std::cout << " Extrinsics : " << (opt_disable_depth ? "Disabled" : extrinsic_file) << std::endl;
179  std::cout << " Color intrinsics: " << color_intrinsic_file << std::endl;
180  std::cout << " Depth intrinsics: " << (opt_disable_depth ? "Disabled" : depth_intrinsic_file) << std::endl;
181  std::cout << " Ground truth : " << ground_truth << std::endl;
182  std::cout << "Tracker settings" << std::endl;
183  std::cout << " Color config : " << mbt_config_color << std::endl;
184  std::cout << " Depth config : " << mbt_config_depth << std::endl;
185  std::cout << " CAD model : " << mbt_cad_model << std::endl;
186  std::cout << " First frame : " << opt_first_frame << std::endl;
187  std::cout << " Step by step : " << opt_step_by_step << std::endl;
188  if (opt_display_ground_truth) {
189  std::cout << " Ground truth is used to project the cad model (no tracking)" << std::endl;
190  }
191  else {
192  std::cout << " Init file : " << mbt_init_file << std::endl;
193  std::cout << " Features : moving-edges " << (opt_disable_klt ? "" : "+ keypoints") << (opt_disable_depth ? "" : " + depth") << std::endl;
194  }
195 
196  std::vector<int> tracker_types;
197  if (opt_disable_klt) {
198  tracker_types.push_back(vpMbGenericTracker::EDGE_TRACKER);
199  }
200  else {
201 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
203 #else
204  std::cout << "Warning: keypoints cannot be used as features since ViSP is not build with OpenCV 3rd party" << std::endl;
205 #endif
206  }
207  if (!opt_disable_depth)
208  tracker_types.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
209 
210  vpMbGenericTracker tracker(tracker_types);
211  if (!opt_disable_depth)
212  tracker.loadConfigFile(mbt_config_color, mbt_config_depth);
213  else
214  tracker.loadConfigFile(mbt_config_color);
215  tracker.loadModel(mbt_cad_model);
216  vpCameraParameters cam_color, cam_depth;
217 
218  // Update intrinsics camera parameters from Blender generated data
220  if (p.parse(cam_color, color_intrinsic_file, color_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion)
222  std::cout << "Cannot found intrinsics for camera " << color_camera_name << std::endl;
223  }
224  if (p.parse(cam_depth, depth_intrinsic_file, depth_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion)
226  std::cout << "Cannot found intrinsics for camera " << depth_camera_name << std::endl;
227  }
228 
229  if (!opt_disable_depth)
230  tracker.setCameraParameters(cam_color, cam_depth);
231  else
232  tracker.setCameraParameters(cam_color);
233 
234  // Reload intrinsics from tracker (useless)
235  if (!opt_disable_depth)
236  tracker.getCameraParameters(cam_color, cam_depth);
237  else
238  tracker.getCameraParameters(cam_color);
239  tracker.setDisplayFeatures(true);
240  std::cout << "cam_color:\n" << cam_color << std::endl;
241 
242  if (!opt_disable_depth)
243  std::cout << "cam_depth:\n" << cam_depth << std::endl;
244 
245  vpImage<uint16_t> I_depth_raw;
246  vpImage<unsigned char> I, I_depth;
247  unsigned int depth_width = 0, depth_height = 0;
248  std::vector<vpColVector> pointcloud;
249  vpHomogeneousMatrix cMo_ground_truth;
250 
251  unsigned int frame_cpt = opt_first_frame;
252  read_data(frame_cpt, video_color_images, video_depth_images, opt_disable_depth, ground_truth,
253  I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth, cMo_ground_truth);
254  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
255 
256 #if defined(VISP_HAVE_X11)
257  vpDisplayX d1, d2;
258 #elif defined(VISP_HAVE_GDI)
259  vpDisplayGDI d1, d2;
260 #elif defined (HAVE_OPENCV_HIGHGUI)
261  vpDisplayOpenCV d1, d2;
262 #endif
263 
264  d1.init(I, 0, 0, "Color image");
265  if (!opt_disable_depth) {
266  d2.init(I_depth, static_cast<int>(I.getWidth()), 0, "Depth image");
267  }
268 
269  vpHomogeneousMatrix depth_M_color;
270  if (!opt_disable_depth) {
271  depth_M_color.load(extrinsic_file);
272  tracker.setCameraTransformationMatrix("Camera2", depth_M_color);
273  std::cout << "depth_M_color:\n" << depth_M_color << std::endl;
274  }
275 
276  if (opt_display_ground_truth) {
277  tracker.initFromPose(I, cMo_ground_truth); // I and I_depth must be the same size when using depth features!
278  }
279  else {
280  tracker.initClick(I, mbt_init_file, true); // I and I_depth must be the same size when using depth features!
281  }
282 
283  try {
284  bool quit = false;
285  while (!quit && read_data(frame_cpt, video_color_images, video_depth_images, opt_disable_depth,
286  ground_truth, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth,
287  cMo_ground_truth)) {
288  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
290  vpDisplay::display(I_depth);
291 
292  if (opt_display_ground_truth) {
293  tracker.initFromPose(I, cMo_ground_truth); // I and I_depth must be the same size when using depth features!
294  }
295  else {
296  if (!opt_disable_depth) {
297  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
298  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
299  std::map<std::string, unsigned int> mapOfPointCloudWidths;
300  std::map<std::string, unsigned int> mapOfPointCloudHeights;
301 
302  mapOfImages["Camera1"] = &I;
303  mapOfPointClouds["Camera2"] = &pointcloud;
304  mapOfPointCloudWidths["Camera2"] = depth_width;
305  mapOfPointCloudHeights["Camera2"] = depth_height;
306  tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
307  }
308  else {
309  tracker.track(I);
310  }
311  }
312 
313  vpHomogeneousMatrix cMo = tracker.getPose();
314  std::cout << "\nFrame: " << frame_cpt << std::endl;
315  if (!opt_display_ground_truth)
316  std::cout << "cMo:\n" << cMo << std::endl;
317  std::cout << "cMo ground truth:\n" << cMo_ground_truth << std::endl;
318  if (!opt_disable_depth) {
319  tracker.display(I, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth, vpColor::red, 2);
320  vpDisplay::displayFrame(I_depth, depth_M_color * cMo, cam_depth, 0.05, vpColor::none, 2);
321  }
322  else {
323  tracker.display(I, cMo, cam_color, vpColor::red, 2);
324  }
325 
326  vpDisplay::displayFrame(I, cMo, cam_color, 0.05, vpColor::none, 2);
327  std::ostringstream oss;
328  oss << "Frame: " << frame_cpt;
329  vpDisplay::setTitle(I, oss.str());
330  if (opt_step_by_step) {
331  vpDisplay::displayText(I, 20, 10, "Left click to trigger next step", vpColor::red);
332  vpDisplay::displayText(I, 40, 10, "Right click to quit step-by-step mode", vpColor::red);
333  }
334  else {
335  vpDisplay::displayText(I, 20, 10, "Left click to trigger step-by-step mode", vpColor::red);
336  vpDisplay::displayText(I, 40, 10, "Right click to exit...", vpColor::red);
337  }
338  if (!opt_display_ground_truth) {
339  {
340  std::stringstream ss;
341  ss << "Nb features: " << tracker.getError().size();
342  vpDisplay::displayText(I, I.getHeight() - 50, 20, ss.str(), vpColor::red);
343  }
344  {
345  std::stringstream ss;
346  ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt()
347  << ", depth " << tracker.getNbFeaturesDepthDense();
348  vpDisplay::displayText(I, I.getHeight() - 30, 20, ss.str(), vpColor::red);
349  }
350  }
351 
352  vpDisplay::flush(I);
353  vpDisplay::flush(I_depth);
354 
355  // Button 1: start step by step if not enabled from command line option
356  // Button 2: enables step by step mode
357  // Button 3: ends step by step mode if enabled
358  // quit otherwise
360  if (vpDisplay::getClick(I, button, opt_step_by_step)) {
361  if (button == vpMouseButton::button1 && opt_step_by_step == false) {
362  opt_step_by_step = true;
363  }
364  else if (button == vpMouseButton::button3 && opt_step_by_step == true) {
365  opt_step_by_step = false;
366  }
367  else if (button == vpMouseButton::button3 && opt_step_by_step == false) {
368  quit = true;
369  }
370  else if (button == vpMouseButton::button2) {
371  opt_step_by_step = true;
372  }
373  }
374 
375  frame_cpt++;
376  }
377 
378  vpDisplay::flush(I);
380  }
381  catch (std::exception &e) {
382  std::cerr << "Catch exception: " << e.what() << std::endl;
383  }
384 
385  return EXIT_SUCCESS;
386 }
387 #else
388 int main()
389 {
390  std::cout << "To run this tutorial, ViSP should be built with OpenCV and pugixml libraries." << std::endl;
391  return EXIT_SUCCESS;
392 }
393 #endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
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...
Definition: vpDisplayX.h:128
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 setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void load(std::ifstream &f)
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)
Definition: vpImageIo.cpp:143
unsigned int getWidth() const
Definition: vpImage.h:245
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:783
unsigned int getHeight() const
Definition: vpImage.h:184
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:1199
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:2142
Real-time 6D object pose tracking using its CAD model.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
XML parser to load and save intrinsic camera parameters.
vpCameraParameters getCameraParameters() const
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)