Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
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/gui/vpDisplayGDI.h>
7 #include <visp3/gui/vpDisplayOpenCV.h>
8 #include <visp3/gui/vpDisplayX.h>
9 #include <visp3/io/vpImageIo.h>
10 #include <visp3/mbt/vpMbGenericTracker.h>
11 
12 #if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
13 namespace
14 {
15 bool read_data(unsigned int cpt, const std::string &input_directory, vpImage<unsigned char> &I,
16  vpImage<uint16_t> &I_depth_raw, unsigned int &depth_width, unsigned int &depth_height,
17  std::vector<vpColVector> &pointcloud, const vpCameraParameters &cam,
18  vpHomogeneousMatrix &cMo_ground_truth)
19 {
20  char buffer[FILENAME_MAX];
21  // Read color
22  std::stringstream ss;
23  ss << input_directory << "/images/%04d.jpg";
24  sprintf(buffer, ss.str().c_str(), cpt);
25  std::string filename_img = buffer;
26 
27  if (!vpIoTools::checkFilename(filename_img)) {
28  std::cerr << "Cannot read: " << filename_img << std::endl;
29  return false;
30  }
31  vpImageIo::read(I, filename_img);
32 
33  // Read depth
34  ss.str("");
35  ss << input_directory << "/depth/Image%04d.exr";
36  sprintf(buffer, ss.str().c_str(), cpt);
37  std::string filename_depth = buffer;
38 
39  cv::Mat depth_raw = cv::imread(filename_depth, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
40  if (depth_raw.empty()) {
41  std::cerr << "Cannot read: " << filename_depth << std::endl;
42  return false;
43  }
44 
45  depth_width = static_cast<unsigned int>(depth_raw.cols);
46  depth_height = static_cast<unsigned int>(depth_raw.rows);
47  I_depth_raw.resize(depth_height, depth_width);
48  pointcloud.resize(depth_width * depth_height);
49 
50  for (int i = 0; i < depth_raw.rows; i++) {
51  for (int j = 0; j < depth_raw.cols; j++) {
52  I_depth_raw[i][j] = static_cast<uint16_t>(32767.5f * depth_raw.at<cv::Vec3f>(i, j)[0]);
53  double x = 0.0, y = 0.0;
54  // Manually limit the field of view of the depth camera
55  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]);
56  vpPixelMeterConversion::convertPoint(cam, j, i, x, y);
57  size_t idx = static_cast<size_t>(i * depth_raw.cols + j);
58  pointcloud[idx].resize(3);
59  pointcloud[idx][0] = x * Z;
60  pointcloud[idx][1] = y * Z;
61  pointcloud[idx][2] = Z;
62  }
63  }
64 
65  // Read ground truth
66  ss.str("");
67  ss << input_directory << "/camera_poses/Camera_%03d.txt";
68  sprintf(buffer, ss.str().c_str(), cpt);
69  std::string filename_pose = buffer;
70 
71  std::ifstream f_pose;
72  f_pose.open(filename_pose.c_str()); // .c_str() to keep compat when c++11 not available
73  if (!f_pose.is_open()) {
74  std::cerr << "Cannot read: " << filename_pose << std::endl;
75  return false;
76  }
77 
78  cMo_ground_truth.load(f_pose);
79 
80  return true;
81 }
82 } // namespace
83 
84 int main(int argc, char *argv[])
85 {
86  std::string input_directory = "."; // location of the data (images, depth maps, camera poses)
87  std::string config_color = "teabox.xml", config_depth = "teabox_depth.xml";
88  std::string model_color = "teabox.cao", model_depth = "teabox.cao";
89  std::string init_file = "teabox.init";
90  std::string extrinsic_file = "depth_M_color.txt";
91  unsigned int first_frame_index = 1;
92  bool disable_depth = false;
93  bool display_ground_truth = false;
94  bool click = false;
95 
96  for (int i = 1; i < argc; i++) {
97  if (std::string(argv[i]) == "--input_directory" && i + 1 < argc) {
98  input_directory = std::string(argv[i + 1]);
99  } else if (std::string(argv[i]) == "--config_color" && i + 1 < argc) {
100  config_color = std::string(argv[i + 1]);
101  } else if (std::string(argv[i]) == "--config_depth" && i + 1 < argc) {
102  config_depth = std::string(argv[i + 1]);
103  } else if (std::string(argv[i]) == "--model_color" && i + 1 < argc) {
104  model_color = std::string(argv[i + 1]);
105  } else if (std::string(argv[i]) == "--model_depth" && i + 1 < argc) {
106  model_depth = std::string(argv[i + 1]);
107  } else if (std::string(argv[i]) == "--init_file" && i + 1 < argc) {
108  init_file = std::string(argv[i + 1]);
109  } else if (std::string(argv[i]) == "--extrinsics" && i + 1 < argc) {
110  extrinsic_file = std::string(argv[i + 1]);
111  } else if (std::string(argv[i]) == "--disable_depth") {
112  disable_depth = true;
113  } else if (std::string(argv[i]) == "--display_ground_truth") {
114  display_ground_truth = true;
115  } else if (std::string(argv[i]) == "--click") {
116  click = true;
117  } else if (std::string(argv[i]) == "--first_frame_index" && i + 1 < argc) {
118  first_frame_index = static_cast<unsigned int>(atoi(argv[i + 1]));
119  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
120  std::cout
121  << "Usage: \n"
122  << argv[0]
123  << " [--input_directory <data directory> (default: .)]"
124  " [--config_color <object.xml> (default: teabox.xml)] [--config_depth <object.xml> (default: "
125  "teabox_depth.xml)]"
126  " [--model_color <object.cao> (default: teabox.cao)] [--model_depth <object.cao> (default: teabox.cao)]"
127  " [--init_file <object.init> (default: teabox.init)]"
128  " [--extrinsics <depth to color transformation> (default: depth_M_color.txt)] [--disable_depth]"
129  " [--display_ground_truth] [--click] [--first_frame_index <index> (default: 1)]"
130  << std::endl;
131  return EXIT_SUCCESS;
132  }
133  }
134 
135  std::cout << "input_directory: " << input_directory << std::endl;
136  std::cout << "config_color: " << config_color << std::endl;
137  std::cout << "config_depth: " << config_depth << std::endl;
138  std::cout << "model_color: " << model_color << std::endl;
139  std::cout << "model_depth: " << model_depth << std::endl;
140  std::cout << "init_file: " << model_depth << std::endl;
141  std::cout << "extrinsic_file: " << extrinsic_file << std::endl;
142  std::cout << "first_frame_index: " << first_frame_index << std::endl;
143  std::cout << "disable_depth: " << disable_depth << std::endl;
144  std::cout << "display_ground_truth: " << display_ground_truth << std::endl;
145  std::cout << "click: " << click << std::endl;
146 
147  std::vector<int> tracker_types;
149  if (!disable_depth)
150  tracker_types.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
151 
152  vpMbGenericTracker tracker(tracker_types);
153  if (!disable_depth)
154  tracker.loadConfigFile(config_color, config_depth);
155  else
156  tracker.loadConfigFile(config_color);
157  tracker.loadModel(model_color);
158  vpCameraParameters cam_color, cam_depth;
159  if (!disable_depth)
160  tracker.getCameraParameters(cam_color, cam_depth);
161  else
162  tracker.getCameraParameters(cam_color);
163  tracker.setDisplayFeatures(true);
164  std::cout << "cam_color:\n" << cam_color << std::endl;
165  std::cout << "cam_depth:\n" << cam_depth << std::endl;
166 
167  vpImage<uint16_t> I_depth_raw;
168  vpImage<unsigned char> I, I_depth;
169  unsigned int depth_width = 0, depth_height = 0;
170  std::vector<vpColVector> pointcloud;
171  vpHomogeneousMatrix cMo_ground_truth;
172 
173  unsigned int frame_cpt = first_frame_index;
174  read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth,
175  cMo_ground_truth);
176  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
177 
178 #if defined(VISP_HAVE_X11)
179  vpDisplayX d1, d2;
180 #elif defined(VISP_HAVE_GDI)
181  vpDisplayGDI d1, d2;
182 #else
183  vpDisplayOpenCV d1, d2;
184 #endif
185 
186  d1.init(I, 0, 0, "Color image");
187  d2.init(I_depth, static_cast<int>(I.getWidth()), 0, "Depth image");
188 
189  vpHomogeneousMatrix depthMcolor;
190  if (!disable_depth) {
191  std::ifstream f_extrinsics;
192  f_extrinsics.open(extrinsic_file.c_str()); // .c_str() to keep compat when c++11 not available
193 
194  depthMcolor.load(f_extrinsics);
195  tracker.setCameraTransformationMatrix("Camera2", depthMcolor);
196  std::cout << "depthMcolor:\n" << depthMcolor << std::endl;
197  }
198 
199  if (display_ground_truth) {
200  tracker.initFromPose(I, cMo_ground_truth); // I and I_depth must be the same size when using depth features!
201  } else
202  tracker.initClick(I, init_file, true); // I and I_depth must be the same size when using depth features!
203 
204  try {
205  bool quit = false;
206  while (!quit && read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud,
207  cam_depth, cMo_ground_truth)) {
208  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
210  vpDisplay::display(I_depth);
211 
212  if (display_ground_truth) {
213  tracker.initFromPose(I, cMo_ground_truth); // I and I_depth must be the same size when using depth features!
214  } else {
215  if (!disable_depth) {
216  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
217  std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
218  std::map<std::string, unsigned int> mapOfPointCloudWidths;
219  std::map<std::string, unsigned int> mapOfPointCloudHeights;
220 
221  mapOfImages["Camera1"] = &I;
222  mapOfPointClouds["Camera2"] = &pointcloud;
223  mapOfPointCloudWidths["Camera2"] = depth_width;
224  mapOfPointCloudHeights["Camera2"] = depth_height;
225  tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
226  } else {
227  tracker.track(I);
228  }
229  }
230 
231  vpHomogeneousMatrix cMo = tracker.getPose();
232  std::cout << "\nFrame: " << frame_cpt << std::endl;
233  if (!display_ground_truth)
234  std::cout << "cMo:\n" << cMo << std::endl;
235  std::cout << "cMo ground truth:\n" << cMo_ground_truth << std::endl;
236  if (!disable_depth) {
237  tracker.display(I, I_depth, cMo, depthMcolor * cMo, cam_color, cam_depth, vpColor::red, 2);
238  vpDisplay::displayFrame(I_depth, depthMcolor * cMo, cam_depth, 0.05, vpColor::none, 2);
239  } else {
240  tracker.display(I, cMo, cam_color, vpColor::red, 2);
241  }
242 
243  vpDisplay::displayFrame(I, cMo, cam_color, 0.05, vpColor::none, 2);
244  std::ostringstream oss;
245  oss << "Frame: " << frame_cpt;
246  vpDisplay::displayText(I, 20, 20, oss.str(), vpColor::red);
247 
248  if (!display_ground_truth) {
249  {
250  std::stringstream ss;
251  ss << "Nb features: " << tracker.getError().size();
252  vpDisplay::displayText(I, I.getHeight() - 50, 20, ss.str(), vpColor::red);
253  }
254  {
255  std::stringstream ss;
256  ss << "Features: edges " << tracker.getNbFeaturesEdge() << ", klt " << tracker.getNbFeaturesKlt()
257  << ", depth " << tracker.getNbFeaturesDepthDense();
258  vpDisplay::displayText(I, I.getHeight() - 30, 20, ss.str(), vpColor::red);
259  }
260  }
261 
262  vpDisplay::flush(I);
263  vpDisplay::flush(I_depth);
264 
266  if (vpDisplay::getClick(I, button, click)) {
267  switch (button) {
269  quit = !click;
270  break;
272  click = !click;
273  break;
274 
275  default:
276  break;
277  }
278  }
279 
280  frame_cpt++;
281  }
282 
283  vpDisplay::displayText(I, 40, 20, "Click to quit.", vpColor::red);
284  vpDisplay::flush(I);
286  } catch (std::exception &e) {
287  std::cerr << "Catch exception: " << e.what() << std::endl;
288  }
289 
290  return EXIT_SUCCESS;
291 }
292 #else
293 int main()
294 {
295  std::cout << "To run this tutorial, ViSP should be built with OpenCV and pugixml libraries." << std::endl;
296  return EXIT_SUCCESS;
297 }
298 #endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(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))
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:148
unsigned int getWidth() const
Definition: vpImage.h:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:799
unsigned int getHeight() const
Definition: vpImage.h:188
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:800
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)