Visual Servoing Platform  version 3.4.0
tutorial-mb-generic-tracker-rgbd.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/core/vpXmlParserCamera.h>
8 #include <visp3/gui/vpDisplayGDI.h>
9 #include <visp3/gui/vpDisplayX.h>
11 #include <visp3/mbt/vpMbGenericTracker.h>
13 
14 #if defined (VISP_HAVE_PCL)
15 #include <pcl/common/common.h>
16 #include <pcl/io/pcd_io.h>
17 
18 namespace {
19 struct rs_intrinsics {
20  float ppx;
22  float ppy;
24  float fx;
26  float fy;
28  float coeffs[5];
29 };
30 
31 void rs_deproject_pixel_to_point(float point[3], const rs_intrinsics &intrin, const float pixel[2], float depth) {
32  float x = (pixel[0] - intrin.ppx) / intrin.fx;
33  float y = (pixel[1] - intrin.ppy) / intrin.fy;
34 
35  float r2 = x * x + y * y;
36  float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
37  float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
38  float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
39 
40  x = ux;
41  y = uy;
42 
43  point[0] = depth * x;
44  point[1] = depth * y;
45  point[2] = depth;
46 }
47 
49 bool read_data(unsigned int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color,
50  vpImage<uint16_t> &I_depth_raw, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
52 {
53  char buffer[FILENAME_MAX];
54  // Read color
55  std::stringstream ss;
56  ss << input_directory << "/color_image_%04d.jpg";
57  sprintf(buffer, ss.str().c_str(), cpt);
58  std::string filename_color = buffer;
59 
60  if (!vpIoTools::checkFilename(filename_color)) {
61  std::cerr << "Cannot read: " << filename_color << std::endl;
62  return false;
63  }
64  vpImageIo::read(I_color, filename_color);
65 
66  // Read raw depth
67  ss.str("");
68  ss << input_directory << "/depth_image_%04d.bin";
69  sprintf(buffer, ss.str().c_str(), cpt);
70  std::string filename_depth = buffer;
71 
72  std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
73  if (!file_depth.is_open()) {
74  return false;
75  }
76 
77  unsigned int height = 0, width = 0;
78  vpIoTools::readBinaryValueLE(file_depth, height);
79  vpIoTools::readBinaryValueLE(file_depth, width);
80  I_depth_raw.resize(height, width);
81 
82  uint16_t depth_value = 0;
83  for (unsigned int i = 0; i < height; i++) {
84  for (unsigned int j = 0; j < width; j++) {
85  vpIoTools::readBinaryValueLE(file_depth, depth_value);
86  I_depth_raw[i][j] = depth_value;
87  }
88  }
89 
90  // Transform pointcloud
91  pointcloud->width = width;
92  pointcloud->height = height;
93  pointcloud->resize((size_t)width * height);
94 
95  // Only for Creative SR300
96  const float depth_scale = 0.00100000005f;
97  rs_intrinsics depth_intrinsic;
98  depth_intrinsic.ppx = 320.503509521484f;
99  depth_intrinsic.ppy = 235.602951049805f;
100  depth_intrinsic.fx = 383.970001220703f;
101  depth_intrinsic.fy = 383.970001220703f;
102  depth_intrinsic.coeffs[0] = 0.0f;
103  depth_intrinsic.coeffs[1] = 0.0f;
104  depth_intrinsic.coeffs[2] = 0.0f;
105  depth_intrinsic.coeffs[3] = 0.0f;
106  depth_intrinsic.coeffs[4] = 0.0f;
107 
108  for (unsigned int i = 0; i < height; i++) {
109  for (unsigned int j = 0; j < width; j++) {
110  float scaled_depth = I_depth_raw[i][j] * depth_scale;
111  float point[3];
112  float pixel[2] = {(float)j, (float)i};
113  rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
114  pointcloud->points[(size_t) (i*width + j)].x = point[0];
115  pointcloud->points[(size_t) (i*width + j)].y = point[1];
116  pointcloud->points[(size_t) (i*width + j)].z = point[2];
117  }
118  }
119 
120  return true;
121 }
122 }
123 
124 int main(int argc, char *argv[])
125 {
126  std::string input_directory = "data"; // location of the data (images, depth_map, point_cloud)
127  std::string config_color = "model/cube/cube.xml", config_depth = "model/cube/cube_depth.xml";
128  std::string model_color = "model/cube/cube.cao", model_depth = "model/cube/cube.cao";
129  std::string init_file = "model/cube/cube.init";
130  unsigned int frame_cpt = 0;
131  bool disable_depth = false;
132 
133  for (int i = 1; i < argc; i++) {
134  if (std::string(argv[i]) == "--input_directory" && i+1 < argc) {
135  input_directory = std::string(argv[i+1]);
136  } else if (std::string(argv[i]) == "--config_color" && i+1 < argc) {
137  config_color = std::string(argv[i+1]);
138  } else if (std::string(argv[i]) == "--config_depth" && i+1 < argc) {
139  config_depth = std::string(argv[i+1]);
140  } else if (std::string(argv[i]) == "--model_color" && i+1 < argc) {
141  model_color = std::string(argv[i+1]);
142  } else if (std::string(argv[i]) == "--model_depth" && i+1 < argc) {
143  model_depth = std::string(argv[i+1]);
144  } else if (std::string(argv[i]) == "--init_file" && i+1 < argc) {
145  init_file = std::string(argv[i+1]);
146  } else if (std::string(argv[i]) == "--disable_depth") {
147  disable_depth = true;
148  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
149  std::cout << "Usage: \n" << argv[0] << " --input_directory <data directory> --config_color <object.xml> --config_depth <object.xml>"
150  " --model_color <object.cao> --model_depth <object.cao> --init_file <object.init> --disable_depth" << std::endl;
151  std::cout << "\nExample:\n" << argv[0] << " --config_color model/cube/cube.xml --config_depth model/cube/cube.xml"
152  " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n" << std::endl;
153  return 0;
154  }
155  }
156 
157  std::cout << "Tracked features: " << std::endl;
158 #ifdef VISP_HAVE_OPENCV
159  std::cout << " Use edges : 1" << std::endl;
160  std::cout << " Use klt : 1" << std::endl;
161  std::cout << " Use depth : " << ! disable_depth << std::endl;
162 #else
163  std::cout << " Use edges : 1" << std::endl;
164  std::cout << " Use klt : 0" << std::endl;
165  std::cout << " Use depth : 0" << std::endl;
166 #endif
167  std::cout << "Config files: " << std::endl;
168  std::cout << " Input directory: " << "\"" << input_directory << "\"" << std::endl;
169  std::cout << " Config color: " << "\"" << config_color << "\"" << std::endl;
170  std::cout << " Config depth: " << "\"" << config_depth << "\"" << std::endl;
171  std::cout << " Model color : " << "\"" << model_color << "\"" << std::endl;
172  std::cout << " Model depth : " << "\"" << model_depth << "\"" << std::endl;
173  std::cout << " Init file : " << "\"" << init_file << "\"" << std::endl;
174 
175  vpImage<vpRGBa> I_color;
177  vpImage<unsigned char> I_gray, I_depth;
179  vpImage<uint16_t> I_depth_raw;
181  pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
183  vpCameraParameters cam_color, cam_depth;
184  unsigned int _posx = 100, _posy = 50, _posdx = 10;
185 
186  read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
187  vpImageConvert::convert(I_color, I_gray);
188  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
189 
190 #if defined(VISP_HAVE_X11)
191  vpDisplayX d1, d2;
192 #elif defined(VISP_HAVE_GDI)
193  vpDisplayGDI d1, d2;
194 #endif
195  d1.init(I_gray, _posx, _posy, "Color stream");
196  d2.init(I_depth, _posx + I_gray.getWidth()+_posdx, _posy, "Depth stream");
197  vpDisplay::display(I_gray);
198  vpDisplay::display(I_depth);
199  vpDisplay::flush(I_gray);
200  vpDisplay::flush(I_depth);
201 
203  std::vector<int> trackerTypes;
204 #ifdef VISP_HAVE_OPENCV
206 #else
207  trackerTypes.push_back(vpMbGenericTracker::EDGE_TRACKER);
208 #endif
209  trackerTypes.push_back(vpMbGenericTracker::DEPTH_DENSE_TRACKER);
210  vpMbGenericTracker tracker(trackerTypes);
213  tracker.loadConfigFile(config_color, config_depth);
216  tracker.loadModel(model_color, model_depth);
218 
219  tracker.getCameraParameters(cam_color, cam_depth);
220 
221  std::cout << "Camera parameters for color camera (from XML file): " << cam_color << std::endl;
222  std::cout << "Camera parameters for depth camera (from XML file): " << cam_depth << std::endl;
223 
225  tracker.setDisplayFeatures(true);
227 
229  vpHomogeneousMatrix depth_M_color;
230  {
231  std::ifstream file( (std::string(input_directory + "/depth_M_color.txt")).c_str() );
232  depth_M_color.load(file);
233  file.close();
234  }
235  std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
236  mapOfCameraTransformations["Camera1"] = vpHomogeneousMatrix();
237  mapOfCameraTransformations["Camera2"] = depth_M_color;
238  tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
240  std::cout << "depth_M_color: \n" << depth_M_color << std::endl;
241 
243  std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
244  mapOfImages["Camera1"] = &I_gray;
245  mapOfImages["Camera2"] = &I_depth;
247 
249  std::map<std::string, std::string> mapOfInitFiles;
250  mapOfInitFiles["Camera1"] = init_file;
251  tracker.initClick(mapOfImages, mapOfInitFiles, true);
253 
254  mapOfImages.clear();
255  pcl::PointCloud<pcl::PointXYZ>::Ptr empty_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
256  std::vector<double> times_vec;
257 
258  try {
259  bool quit = false;
260  while (! quit) {
261  double t = vpTime::measureTimeMs();
262  quit = ! read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
263  vpImageConvert::convert(I_color, I_gray);
264  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
265 
266  vpDisplay::display(I_gray);
267  vpDisplay::display(I_depth);
268 
269  mapOfImages["Camera1"] = &I_gray;
270  std::map<std::string, pcl::PointCloud< pcl::PointXYZ >::ConstPtr> mapOfPointclouds;
271  if (disable_depth)
272  mapOfPointclouds["Camera2"] = empty_pointcloud;
273  else
274  mapOfPointclouds["Camera2"] = pointcloud;
275 
277  tracker.track(mapOfImages, mapOfPointclouds);
279 
281  vpHomogeneousMatrix cMo = tracker.getPose();
283 
284  std::cout << "iter: " << frame_cpt << " cMo:\n" << cMo << std::endl;
285 
287  tracker.display(I_gray, I_depth, cMo, depth_M_color*cMo, cam_color, cam_depth, vpColor::red, 3);
288  vpDisplay::displayFrame(I_gray, cMo, cam_color, 0.05, vpColor::none, 3);
289  vpDisplay::displayFrame(I_depth, depth_M_color*cMo, cam_depth, 0.05, vpColor::none, 3);
291 
292  t = vpTime::measureTimeMs() - t;
293  times_vec.push_back(t);
294 
295  std::stringstream ss;
296  ss << "Computation time: " << t << " ms";
297  vpDisplay::displayText(I_gray, 20, 20, ss.str(), vpColor::red);
298  {
299  std::stringstream ss;
300  ss << "Nb features: " << tracker.getError().size();
301  vpDisplay::displayText(I_gray, I_gray.getHeight() - 50, 20, ss.str(), vpColor::red);
302  }
303  {
304  std::stringstream ss;
305  ss << "Features: edges " << tracker.getNbFeaturesEdge()
306  << ", klt " << tracker.getNbFeaturesKlt()
307  << ", depth " << tracker.getNbFeaturesDepthDense();
308  vpDisplay::displayText(I_gray, I_gray.getHeight() - 30, 20, ss.str(), vpColor::red);
309  }
310 
311  vpDisplay::flush(I_gray);
312  vpDisplay::flush(I_depth);
313 
315  if (vpDisplay::getClick(I_gray, button, false)) {
316  quit = true;
317  }
318 
319  frame_cpt ++;
320  }
321  } catch (const vpException &e) {
322  std::cout << "Catch exception: " << e.getStringMessage() << std::endl;
323  }
324 
325  std::cout << "\nProcessing time, Mean: " << vpMath::getMean(times_vec) << " ms ; Median: " << vpMath::getMedian(times_vec)
326  << " ; Std: " << vpMath::getStdev(times_vec) << " ms" << std::endl;
327 
328  vpDisplay::displayText(I_gray, 60, 20, "Click to quit", vpColor::red);
329  vpDisplay::flush(I_gray);
330  vpDisplay::getClick(I_gray);
331 
332  return EXIT_SUCCESS;
333 }
334 #else
335 int main()
336 {
337  std::cout << "To run this tutorial, ViSP should be build with PCL library."
338  " Install libpcl, configure and build again ViSP..." << std::endl;
339  return EXIT_SUCCESS;
340 }
341 #endif
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
unsigned int getWidth() const
Definition: vpImage.h:246
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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.
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:1769
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:220
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:250
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
error that can be emited by ViSP classes.
Definition: vpException.h:71
Real-time 6D object pose tracking using its CAD model.
static void flush(const vpImage< unsigned char > &I)
void load(std::ifstream &f)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static const vpColor red
Definition: vpColor.h:217
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:200
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
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 getHeight() const
Definition: vpImage.h:188
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:92
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)