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