Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
saveRealSenseData.cpp
1 #include <iostream>
2 
3 #include <visp3/core/vpConfig.h>
4 #if (defined (VISP_HAVE_REALSENSE) || defined (VISP_HAVE_REALSENSE2)) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
5 
6 #include <fstream>
7 #include <queue>
8 #include <mutex>
9 #include <thread>
10 #include <condition_variable>
11 
12 #if defined (VISP_HAVE_PCL)
13 #include <pcl/common/common.h>
14 #include <pcl/io/pcd_io.h>
15 #endif
16 
17 #include <visp3/core/vpImageConvert.h>
18 #include <visp3/core/vpIoTools.h>
19 #include <visp3/core/vpXmlParserCamera.h>
20 #include <visp3/gui/vpDisplayX.h>
21 #include <visp3/gui/vpDisplayGDI.h>
22 #include <visp3/io/vpParseArgv.h>
23 #include <visp3/io/vpImageIo.h>
24 #include <visp3/sensor/vpRealSense.h>
25 #include <visp3/sensor/vpRealSense2.h>
26 
27 //Priority to libRealSense2
28 #if defined (VISP_HAVE_REALSENSE2)
29 #define USE_REALSENSE2
30 #endif
31 
32 #define GETOPTARGS "so:acdpiCf:bh"
33 
34 namespace {
35  void usage(const char *name, const char *badparam) {
36  fprintf(stdout, "\n\
37  Save RealSense data.\n\
38  \n\
39  %s\
40  OPTIONS: \n\
41  -s \n\
42  Save data.\n\
43  \n\
44  -o <directory> \n\
45  Output directory.\n\
46  \n\
47  -a \n\
48  Use aligned streams.\n\
49  \n\
50  -c \n\
51  Save color stream.\n\
52  \n\
53  -d \n\
54  Save depth stream.\n\
55  \n\
56  -p \n\
57  Save pointcloud.\n\
58  \n\
59  -i \n\
60  Save infrared stream.\n\
61  \n\
62  -C \n\
63  Click to save.\n\
64  \n\
65  -f <fps> \n\
66  Set FPS.\n\
67  \n\
68  -b \n\
69  Save point cloud in binary format.\n\
70  \n\
71  -h \n\
72  Print the help.\n\n",
73  name);
74 
75  if (badparam)
76  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
77  }
78 
79  bool getOptions(int argc, char **argv,
80  bool &save,
81  std::string &output_directory,
82  bool &use_aligned_stream,
83  bool &save_color,
84  bool &save_depth,
85  bool &save_pointcloud,
86  bool &save_infrared,
87  bool &click_to_save,
88  int &stream_fps,
89  bool &save_pointcloud_binary_format
90  ) {
91  const char *optarg;
92  const char **argv1=(const char**)argv;
93  int c;
94  while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
95 
96  switch (c) {
97  case 's': save = true; break;
98  case 'o': output_directory = optarg; break;
99  case 'a': use_aligned_stream = true; break;
100  case 'c': save_color = true; break;
101  case 'd': save_depth = true; break;
102  case 'p': save_pointcloud = true; break;
103  case 'i': save_infrared = true; break;
104  case 'C': click_to_save = true; break;
105  case 'f': stream_fps = atoi(optarg); break;
106  case 'b': save_pointcloud_binary_format = true; break;
107 
108  case 'h': usage(argv[0], NULL); return false; break;
109 
110  default:
111  usage(argv[0], optarg);
112  return false; break;
113  }
114  }
115 
116  if ((c == 1) || (c == -1)) {
117  // standalone param or error
118  usage(argv[0], NULL);
119  std::cerr << "ERROR: " << std::endl;
120  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
121  return false;
122  }
123 
124  return true;
125  }
126 
127 class FrameQueue {
128 public:
129  struct cancelled {
130  };
131 
132  FrameQueue() : m_cancelled(false), m_cond(),
133  m_queueColor(), m_queueDepth(), m_queuePointCloud(),
134  m_queueInfrared(),
135  m_maxQueueSize(1024*8), m_mutex() {
136  }
137 
138  void cancel() {
139  std::lock_guard<std::mutex> lock(m_mutex);
140  m_cancelled = true;
141  m_cond.notify_all();
142  }
143 
144  //Push data to save in the queue (FIFO)
145  void push(const vpImage<vpRGBa> &colorImg, const vpImage<uint16_t> &depthImg,
146 #ifdef VISP_HAVE_PCL
147  const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
148 #else
149  const std::vector<vpColVector> &pointCloud,
150 #endif
151  const vpImage<unsigned char> &infraredImg) {
152  std::lock_guard<std::mutex> lock(m_mutex);
153 
154  m_queueColor.push(colorImg);
155  m_queueDepth.push(depthImg);
156  m_queuePointCloud.push(pointCloud);
157  m_queueInfrared.push(infraredImg);
158 
159  //Pop extra data in the queue
160  while(m_queueColor.size() > m_maxQueueSize) {
161  m_queueColor.pop();
162  }
163 
164  //Pop extra data in the queue
165  while(m_queueDepth.size() > m_maxQueueSize) {
166  m_queueDepth.pop();
167  }
168 
169  //Pop extra data in the queue
170  while(m_queuePointCloud.size() > m_maxQueueSize) {
171  m_queuePointCloud.pop();
172  }
173 
174  //Pop extra data in the queue
175  while(m_queueInfrared.size() > m_maxQueueSize) {
176  m_queueInfrared.pop();
177  }
178 
179  m_cond.notify_one();
180  }
181 
182  //Pop the image to save from the queue (FIFO)
183  void pop(vpImage<vpRGBa> &colorImg, vpImage<uint16_t> &depthImg,
184 #ifdef VISP_HAVE_PCL
185  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
186 #else
187  std::vector<vpColVector> &pointCloud,
188 #endif
189  vpImage<unsigned char> &infraredImg) {
190  std::unique_lock<std::mutex> lock(m_mutex);
191 
192  while (m_queueColor.empty() || m_queueDepth.empty() || m_queuePointCloud.empty() || m_queueInfrared.empty()) {
193  if (m_cancelled) {
194  throw cancelled();
195  }
196 
197  m_cond.wait(lock);
198 
199  if (m_cancelled) {
200  throw cancelled();
201  }
202  }
203 
204  colorImg = m_queueColor.front();
205  depthImg = m_queueDepth.front();
206  pointCloud = m_queuePointCloud.front();
207  infraredImg = m_queueInfrared.front();
208 
209  m_queueColor.pop();
210  m_queueDepth.pop();
211  m_queuePointCloud.pop();
212  m_queueInfrared.pop();
213  }
214 
215  void setMaxQueueSize(const size_t max_queue_size) {
216  m_maxQueueSize = max_queue_size;
217  }
218 
219 private:
220  bool m_cancelled;
221  std::condition_variable m_cond;
222  std::queue<vpImage<vpRGBa> > m_queueColor;
223  std::queue<vpImage<uint16_t> > m_queueDepth;
224 #ifdef VISP_HAVE_PCL
225  std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
226 #else
227  std::queue<std::vector<vpColVector> > m_queuePointCloud;
228 #endif
229  std::queue<vpImage<unsigned char> > m_queueInfrared;
230  size_t m_maxQueueSize;
231  std::mutex m_mutex;
232 };
233 
234 class StorageWorker {
235 public:
236  StorageWorker(FrameQueue &queue, const std::string &directory, bool save_color,
237  bool save_depth, bool save_pointcloud, bool save_infrared,
238  bool save_pointcloud_binary_format, int
239 #ifndef VISP_HAVE_PCL
240  width
241 #endif
242  , int
243 #ifndef VISP_HAVE_PCL
244  height
245 #endif
246  ) :
247  m_queue(queue), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
248  m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared),
249  m_save_pointcloud_binary_format(save_pointcloud_binary_format)
250 #ifndef VISP_HAVE_PCL
251  , m_size_height(height), m_size_width(width)
252 #endif
253  {
254  }
255 
256  //Thread main loop
257  void run() {
258  try {
259  vpImage<vpRGBa> colorImg;
260  vpImage<uint16_t> depthImg;
261 #ifdef VISP_HAVE_PCL
262  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud;
263 #else
264  std::vector<vpColVector> pointCloud;
265 #endif
266  vpImage<unsigned char> infraredImg;
267 
268  char buffer[256];
269  for (;;) {
270  m_queue.pop(colorImg, depthImg, pointCloud, infraredImg);
271 
272  if (!m_directory.empty()) {
273  std::stringstream ss;
274 
275  if (m_save_color) {
276  ss << m_directory << "/color_image_%04d.jpg";
277  sprintf(buffer, ss.str().c_str(), m_cpt);
278 
279  std::string filename_color = buffer;
280  vpImageIo::write(colorImg, filename_color);
281  }
282 
283  if (m_save_depth) {
284  ss.str("");
285  ss << m_directory << "/depth_image_%04d.bin";
286  sprintf(buffer, ss.str().c_str(), m_cpt);
287  std::string filename_depth = buffer;
288 
289  std::ofstream file_depth(filename_depth.c_str(), std::ios::out | std::ios::binary);
290  if (file_depth.is_open()) {
291  unsigned int height = depthImg.getHeight(), width = depthImg.getWidth();
292  vpIoTools::writeBinaryValueLE(file_depth, height);
293  vpIoTools::writeBinaryValueLE(file_depth, width);
294 
295  uint16_t value;
296  for (unsigned int i = 0; i < height; i++) {
297  for (unsigned int j = 0; j < width; j++) {
298  value = depthImg[i][j];
299  vpIoTools::writeBinaryValueLE(file_depth, value);
300  }
301  }
302  }
303  }
304 
305  if (m_save_pointcloud) {
306  ss.str("");
307  ss << m_directory << "/point_cloud_%04d" << (m_save_pointcloud_binary_format ? ".bin" : ".pcd");
308  sprintf(buffer, ss.str().c_str(), m_cpt);
309  std::string filename_point_cloud = buffer;
310 
311  if (m_save_pointcloud_binary_format) {
312  std::ofstream file_pointcloud(filename_point_cloud.c_str(), std::ios::out | std::ios::binary);
313 
314  if (file_pointcloud.is_open()) {
315 #ifdef VISP_HAVE_PCL
316  uint32_t width = pointCloud->width;
317  uint32_t height = pointCloud->height;
318  //true if pointcloud does not contain NaN or Inf, not handled currently
319  char is_dense = pointCloud->is_dense;
320 
321  vpIoTools::writeBinaryValueLE(file_pointcloud, height);
322  vpIoTools::writeBinaryValueLE(file_pointcloud, width);
323  file_pointcloud.write( (char *)(&is_dense), sizeof(is_dense) );
324 
325  for (uint32_t i = 0; i < height; i++) {
326  for (uint32_t j = 0; j < width; j++) {
327  pcl::PointXYZ pt = (*pointCloud)(j,i);
328 
329  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.x);
330  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.y);
331  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.z);
332  }
333  }
334 #else
335  uint32_t width = m_size_width;
336  uint32_t height = m_size_height;
337  //to be consistent with PCL version
338  char is_dense = 1;
339 
340  vpIoTools::writeBinaryValueLE(file_pointcloud, height);
341  vpIoTools::writeBinaryValueLE(file_pointcloud, width);
342  file_pointcloud.write( (char *)(&is_dense), sizeof(is_dense) );
343 
344  for (uint32_t i = 0; i < height; i++) {
345  for (uint32_t j = 0; j < width; j++) {
346  float x = (float) pointCloud[i*width + j][0];
347  float y = (float) pointCloud[i*width + j][1];
348  float z = (float) pointCloud[i*width + j][2];
349 
350  vpIoTools::writeBinaryValueLE(file_pointcloud, x);
351  vpIoTools::writeBinaryValueLE(file_pointcloud, y);
352  vpIoTools::writeBinaryValueLE(file_pointcloud, z);
353  }
354  }
355 #endif
356  }
357  } else {
358 #ifdef VISP_HAVE_PCL
359  pcl::io::savePCDFileBinary(filename_point_cloud, *pointCloud);
360 #endif
361  }
362  }
363 
364  if (m_save_infrared) {
365  ss.str("");
366  ss << m_directory << "/infrared_image_%04d.jpg";
367  sprintf(buffer, ss.str().c_str(), m_cpt);
368 
369  std::string filename_infrared = buffer;
370  vpImageIo::write(infraredImg, filename_infrared);
371  }
372 
373  m_cpt++;
374  }
375  }
376  } catch (const FrameQueue::cancelled &) {
377  std::cout << "Receive cancel FrameQueue." << std::endl;
378  }
379  }
380 
381 private:
382  FrameQueue &m_queue;
383  std::string m_directory;
384  unsigned int m_cpt;
385  bool m_save_color;
386  bool m_save_depth;
387  bool m_save_pointcloud;
388  bool m_save_infrared;
389  bool m_save_pointcloud_binary_format;
390 #ifndef VISP_HAVE_PCL
391  int m_size_height;
392  int m_size_width;
393 #endif
394 };
395 } //Namespace
396 
397 int main(int argc, char *argv[]) {
398  bool save = false;
399  std::string output_directory = vpTime::getDateTime("%Y_%m_%d_%H.%M.%S");
400  std::string output_directory_custom = "";
401  bool use_aligned_stream = false;
402  bool save_color = false;
403  bool save_depth = false;
404  bool save_pointcloud = false;
405  bool save_infrared = false;
406  bool click_to_save = false;
407  int stream_fps = 30;
408  bool save_pointcloud_binary_format = false;
409 
410  // Read the command line options
411  if (!getOptions(argc, argv, save, output_directory_custom, use_aligned_stream, save_color, save_depth,
412  save_pointcloud, save_infrared, click_to_save, stream_fps, save_pointcloud_binary_format)) {
413  return EXIT_FAILURE;
414  }
415 
416  if (!output_directory_custom.empty())
417  output_directory = output_directory_custom + "/" + output_directory;
418 
419 #ifndef VISP_HAVE_PCL
420  save_pointcloud_binary_format = true;
421 #endif
422 
423  std::cout << "save: " << save << std::endl;
424  std::cout << "output_directory: " << output_directory << std::endl;
425  std::cout << "use_aligned_stream: " << use_aligned_stream << std::endl;
426  std::cout << "save_color: " << save_color << std::endl;
427  std::cout << "save_depth: " << save_depth << std::endl;
428  std::cout << "save_pointcloud: " << save_pointcloud << std::endl;
429  std::cout << "save_infrared: " << save_infrared << std::endl;
430  std::cout << "stream_fps: " << stream_fps << std::endl;
431  std::cout << "save_pointcloud_binary_format: " << save_pointcloud_binary_format << std::endl;
432  std::cout << "click_to_save: " << click_to_save << std::endl;
433 
434  int width = 640, height = 480;
435 #ifdef USE_REALSENSE2
436  vpRealSense2 realsense;
437 
438  rs2::config config;
439  config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
440  config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
441  config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, stream_fps);
442  realsense.open(config);
443 #else
444  vpRealSense realsense;
445  realsense.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(width, height, rs::format::rgba8, stream_fps));
446  realsense.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(width, height, rs::format::z16, stream_fps));
447  realsense.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
448  realsense.setStreamSettings(rs::stream::infrared2, vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
449 
450  realsense.open();
451 #endif
452 
453  vpImage<vpRGBa> I_color(height, width);
454  vpImage<unsigned char> I_gray(height, width);
455  vpImage<unsigned char> I_depth(height, width);
456  vpImage<uint16_t> I_depth_raw(height, width);
457  vpImage<unsigned char> I_infrared(height, width);
458 
459 #ifdef VISP_HAVE_X11
460  vpDisplayX d1, d2, d3;
461 #else
462  vpDisplayGDI d1, d2, d3;
463 #endif
464  d1.init(I_gray, 0, 0, "RealSense color stream");
465  d2.init(I_depth, I_gray.getWidth()+80, 0, "RealSense depth stream");
466  d3.init(I_infrared, I_gray.getWidth()+80, I_gray.getHeight()+10, "RealSense infrared stream");
467 
468  while (true) {
469  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, NULL, NULL);
470  vpImageConvert::convert(I_color, I_gray);
471  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
472 
473  vpDisplay::display(I_gray);
474  vpDisplay::display(I_depth);
475  vpDisplay::displayText(I_gray, 20, 20, "Click when ready.", vpColor::red);
476  vpDisplay::flush(I_gray);
477  vpDisplay::flush(I_depth);
478 
479  if (vpDisplay::getClick(I_gray, false)) {
480  break;
481  }
482  }
483 
484  if (save) {
485  //Create output directory
486  vpIoTools::makeDirectory(output_directory);
487 
488  //Save intrinsics
489 #ifdef USE_REALSENSE2
490  vpCameraParameters cam_color = realsense.getCameraParameters(RS2_STREAM_COLOR);
491  vpXmlParserCamera xml_camera;
492  xml_camera.save(cam_color, output_directory+"/camera.xml", "color_camera", width, height);
493 
494  vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
495  xml_camera.save(cam_depth, output_directory+"/camera.xml", "depth_camera", width, height);
496 
497  vpCameraParameters cam_infrared = realsense.getCameraParameters(RS2_STREAM_INFRARED);
498  xml_camera.save(cam_infrared, output_directory+"/camera.xml", "infrared_camera", width, height);
499  vpHomogeneousMatrix depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
500 #else
501  vpCameraParameters cam_color = realsense.getCameraParameters(rs::stream::color);
502  vpXmlParserCamera xml_camera;
503  xml_camera.save(cam_color, output_directory+"/camera.xml", "color_camera", width, height);
504 
505  vpCameraParameters cam_color_rectified = realsense.getCameraParameters(rs::stream::rectified_color);
506  xml_camera.save(cam_color_rectified, output_directory+"/camera.xml", "color_camera_rectified", width, height);
507 
508  vpCameraParameters cam_depth = realsense.getCameraParameters(rs::stream::depth);
509  xml_camera.save(cam_depth, output_directory+"/camera.xml", "depth_camera", width, height);
510 
511  vpCameraParameters cam_depth_aligned_to_rectified_color = realsense.getCameraParameters(rs::stream::depth_aligned_to_rectified_color);
512  xml_camera.save(cam_depth_aligned_to_rectified_color, output_directory+"/camera.xml", "depth_camera_aligned_to_rectified_color", width, height);
513 
514  vpCameraParameters cam_infrared = realsense.getCameraParameters(rs::stream::infrared);
515  xml_camera.save(cam_infrared, output_directory+"/camera.xml", "infrared_camera", width, height);
516  vpHomogeneousMatrix depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
517 #endif
518  std::ofstream file( std::string(output_directory+"/depth_M_color.txt") );
519  depth_M_color.save(file);
520  file.close();
521  }
522 
523  FrameQueue save_queue;
524  StorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud, save_infrared,
525  save_pointcloud_binary_format, width, height);
526  std::thread storage_thread(&StorageWorker::run, &storage);
527 
528 #ifdef USE_REALSENSE2
529  rs2::align align_to(RS2_STREAM_COLOR);
530  if (use_aligned_stream && save_infrared) {
531  std::cerr << "Cannot use aligned streams with infrared acquisition currently."
532  "\nInfrared stream acquisition is disabled!" << std::endl;
533  }
534 #endif
535 
536  int nb_saves = 0;
537  bool quit = false;
538 #ifdef VISP_HAVE_PCL
539  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
540 #else
541  std::vector<vpColVector> pointCloud;
542 #endif
543  while (!quit) {
544  if (use_aligned_stream) {
545 #ifdef USE_REALSENSE2
546  #ifdef VISP_HAVE_PCL
547  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, NULL, pointCloud, NULL,
548  &align_to);
549  #else
550  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, &pointCloud, NULL,
551  &align_to);
552  #endif
553 #else
554  #ifdef VISP_HAVE_PCL
555  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, NULL, pointCloud, (unsigned char *) I_infrared.bitmap,
556  NULL, rs::stream::rectified_color, rs::stream::depth_aligned_to_rectified_color);
557  #else
558  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, &pointCloud, (unsigned char *) I_infrared.bitmap,
559  NULL, rs::stream::rectified_color, rs::stream::depth_aligned_to_rectified_color);
560  #endif
561 #endif
562  } else {
563 #ifdef VISP_HAVE_PCL
564  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, NULL, pointCloud, (unsigned char *) I_infrared.bitmap,
565  NULL);
566 #else
567  realsense.acquire((unsigned char *) I_color.bitmap, (unsigned char *) I_depth_raw.bitmap, &pointCloud, (unsigned char *) I_infrared.bitmap);
568 #endif
569  }
570 
571  vpImageConvert::convert(I_color, I_gray);
572  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
573 
574  vpDisplay::display(I_gray);
575  vpDisplay::display(I_depth);
576  vpDisplay::display(I_infrared);
577 
578  if (!click_to_save) {
579  vpDisplay::displayText(I_gray, 20, 20, "Click to quit.", vpColor::red);
580  } else {
581  std::stringstream ss;
582  ss << "Images saved:" << nb_saves;
583  vpDisplay::displayText(I_gray, 20, 20, ss.str(), vpColor::red);
584  }
585 
586  vpDisplay::flush(I_gray);
587  vpDisplay::flush(I_depth);
588  vpDisplay::flush(I_infrared);
589 
590  if (save && !click_to_save) {
591 #ifdef VISP_HAVE_PCL
592  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
593  save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
594 #else
595  save_queue.push(I_color, I_depth_raw, pointCloud, I_infrared);
596 #endif
597  }
598 
600  if (vpDisplay::getClick(I_gray, button, false)) {
601  if (!click_to_save) {
602  save_queue.cancel();
603  quit = true;
604  } else {
605  switch (button) {
607  if (save) {
608  nb_saves++;
609 #ifdef VISP_HAVE_PCL
610  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
611  save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
612 #else
613  save_queue.push(I_color, I_depth_raw, pointCloud, I_infrared);
614 #endif
615  }
616  break;
617 
620  default:
621  save_queue.cancel();
622  quit = true;
623  break;
624  }
625  }
626  }
627  }
628 
629  storage_thread.join();
630 
631  return EXIT_SUCCESS;
632 }
633 #else
634 int main() {
635  std::cerr << "Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl;
636  return 0;
637 }
638 #endif
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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:134
void acquire(std::vector< vpColVector > &pointcloud)
XML parser to load and save intrinsic camera parameters.
static void flush(const vpImage< unsigned char > &I)
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:293
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
Definition: vpTime.cpp:358
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
static const vpColor red
Definition: vpColor.h:217
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
static void writeBinaryValueLE(std::ofstream &file, const int16_t short_value)
Definition: vpIoTools.cpp:2071
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
unsigned int getHeight() const
Definition: vpImage.h:188
int save(const vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, unsigned int image_width=0, unsigned int image_height=0, const std::string &additionalInfo="")
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void save(std::ofstream &f) const
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
unsigned int getWidth() const
Definition: vpImage.h:246
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)