Visual Servoing Platform  version 3.6.1 under development (2024-05-11)
saveRealSenseData.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  *****************************************************************************/
32 
39 #include <iostream>
40 
41 #include <visp3/core/vpConfig.h>
42 #if (defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2)) && defined(VISP_HAVE_THREADS) \
43  && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PUGIXML)
44 
45 #include <condition_variable>
46 #include <fstream>
47 #include <mutex>
48 #include <queue>
49 #include <thread>
50 
51 #if defined(VISP_HAVE_PCL)
52 #include <pcl/pcl_config.h>
53 #if defined(VISP_HAVE_PCL_COMMON)
54 #include <pcl/point_types.h>
55 #include <pcl/point_cloud.h>
56 #endif
57 #if defined(VISP_HAVE_PCL_IO)
58 #include <pcl/io/pcd_io.h>
59 #endif
60 #endif
61 
62 #include <visp3/core/vpImageConvert.h>
63 #include <visp3/core/vpIoTools.h>
64 #include <visp3/core/vpXmlParserCamera.h>
65 #include <visp3/gui/vpDisplayGDI.h>
66 #include <visp3/gui/vpDisplayX.h>
67 #include <visp3/io/vpImageIo.h>
68 #include <visp3/io/vpParseArgv.h>
69 #include <visp3/sensor/vpRealSense.h>
70 #include <visp3/sensor/vpRealSense2.h>
71 
72  // Priority to libRealSense2
73 #if defined(VISP_HAVE_REALSENSE2)
74 #define USE_REALSENSE2
75 #endif
76 
77 #define GETOPTARGS "so:acdpiCf:bh"
78 
79 namespace
80 {
81 void usage(const char *name, const char *badparam, int fps)
82 {
83  std::cout << "\nSYNOPSIS " << std::endl
84  << " " << name
85  << " [-s]"
86  << " [-a]"
87  << " [-c]"
88  << " [-d]"
89  << " [-p]"
90  << " [-b]"
91  << " [-i]"
92  << " [-C]"
93  << " [-f <fps>]"
94  << " [-o <directory>]"
95  << " [--help,-h]"
96  << std::endl;
97  std::cout << "\nOPTIONS " << std::endl
98  << " -s" << std::endl
99  << " Flag to enable data saving." << std::endl
100  << std::endl
101  << " -a" << std::endl
102  << " Color and depth are aligned." << std::endl
103  << std::endl
104  << " -c" << std::endl
105  << " Add color stream to saved data when -s option is enable." << std::endl
106  << std::endl
107  << " -d" << std::endl
108  << " Add depth stream to saved data when -s option is enable." << std::endl
109  << std::endl
110  << " -p" << std::endl
111  << " Add point cloud stream to saved data when -s option is enabled." << std::endl
112  << " By default, the point cloud is saved in Point Cloud Data file format (.PCD extension file)." << std::endl
113  << " You can also use -b option to save the point cloud in binary format." << std::endl
114  << std::endl
115  << " -b" << std::endl
116  << " Point cloud stream is saved in binary format." << std::endl
117  << std::endl
118  << " -i" << std::endl
119  << " Add infrared stream to saved data when -s option is enabled." << std::endl
120  << std::endl
121  << " -C" << std::endl
122  << " Trigger one shot data saver after each user click." << std::endl
123  << std::endl
124  << " -f <fps>" << std::endl
125  << " Set camera framerate." << std::endl
126  << " Default: " << fps << std::endl
127  << std::endl
128  << " -o <directory>" << std::endl
129  << " Output directory that will host saved data." << std::endl
130  << std::endl
131  << " --help, -h" << std::endl
132  << " Display this helper message." << std::endl
133  << std::endl;
134  std::cout << "\nEXAMPLE " << std::endl
135  << "- Save aligned color + depth + point cloud in data folder" << std::endl
136  << " " << name << " -s -a -c -d -p -b -o data" << std::endl
137  << std::endl;
138 
139  if (badparam) {
140  std::cout << "\nERROR: Bad parameter " << badparam << std::endl;
141  }
142 }
143 
144 bool getOptions(int argc, const char *argv[], bool &save, std::string &output_directory, bool &use_aligned_stream,
145  bool &save_color, bool &save_depth, bool &save_pointcloud, bool &save_infrared, bool &click_to_save,
146  int &stream_fps, bool &save_pointcloud_binary_format)
147 {
148  const char *optarg;
149  const char **argv1 = (const char **)argv;
150  int c;
151  while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
152 
153  switch (c) {
154  case 's':
155  save = true;
156  break;
157  case 'o':
158  output_directory = optarg;
159  break;
160  case 'a':
161  use_aligned_stream = true;
162  break;
163  case 'c':
164  save_color = true;
165  break;
166  case 'd':
167  save_depth = true;
168  break;
169  case 'p':
170  save_pointcloud = true;
171  break;
172  case 'i':
173  save_infrared = true;
174  break;
175  case 'C':
176  click_to_save = true;
177  break;
178  case 'f':
179  stream_fps = atoi(optarg);
180  break;
181  case 'b':
182  save_pointcloud_binary_format = true;
183  break;
184 
185  case 'h':
186  usage(argv[0], nullptr, stream_fps);
187  return false;
188  break;
189 
190  default:
191  usage(argv[0], optarg, stream_fps);
192  return false;
193  break;
194  }
195  }
196 
197  if ((c == 1) || (c == -1)) {
198  // standalone param or error
199  usage(argv[0], nullptr, stream_fps);
200  std::cerr << "ERROR: " << std::endl;
201  std::cerr << " Bad argument " << optarg << std::endl
202  << std::endl;
203  return false;
204  }
205 
206  return true;
207 }
208 
209 class vpFrameQueue
210 {
211 public:
212  struct vpCancelled_t
213  { };
214 
215  vpFrameQueue()
216  : m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(),
217  m_maxQueueSize(1024 * 8), m_mutex()
218  { }
219 
220  void cancel()
221  {
222  std::lock_guard<std::mutex> lock(m_mutex);
223  m_cancelled = true;
224  m_cond.notify_all();
225  }
226 
227  // Push data to save in the queue (FIFO)
228  void push(const vpImage<vpRGBa> &colorImg, const vpImage<uint16_t> &depthImg,
229 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
230  const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
231 #else
232  const std::vector<vpColVector> &pointCloud,
233 #endif
234  const vpImage<unsigned char> &infraredImg)
235  {
236  std::lock_guard<std::mutex> lock(m_mutex);
237 
238  m_queueColor.push(colorImg);
239  m_queueDepth.push(depthImg);
240  m_queuePointCloud.push(pointCloud);
241  m_queueInfrared.push(infraredImg);
242 
243  // Pop extra data in the queue
244  while (m_queueColor.size() > m_maxQueueSize) {
245  m_queueColor.pop();
246  }
247 
248  // Pop extra data in the queue
249  while (m_queueDepth.size() > m_maxQueueSize) {
250  m_queueDepth.pop();
251  }
252 
253  // Pop extra data in the queue
254  while (m_queuePointCloud.size() > m_maxQueueSize) {
255  m_queuePointCloud.pop();
256  }
257 
258  // Pop extra data in the queue
259  while (m_queueInfrared.size() > m_maxQueueSize) {
260  m_queueInfrared.pop();
261  }
262 
263  m_cond.notify_one();
264  }
265 
266  // Pop the image to save from the queue (FIFO)
267  void pop(vpImage<vpRGBa> &colorImg, vpImage<uint16_t> &depthImg,
268 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
269  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
270 #else
271  std::vector<vpColVector> &pointCloud,
272 #endif
273  vpImage<unsigned char> &infraredImg)
274  {
275  std::unique_lock<std::mutex> lock(m_mutex);
276 
277  while (m_queueColor.empty() || m_queueDepth.empty() || m_queuePointCloud.empty() || m_queueInfrared.empty()) {
278  if (m_cancelled) {
279  throw vpCancelled_t();
280  }
281 
282  m_cond.wait(lock);
283 
284  if (m_cancelled) {
285  throw vpCancelled_t();
286  }
287  }
288 
289  colorImg = m_queueColor.front();
290  depthImg = m_queueDepth.front();
291  pointCloud = m_queuePointCloud.front();
292  infraredImg = m_queueInfrared.front();
293 
294  m_queueColor.pop();
295  m_queueDepth.pop();
296  m_queuePointCloud.pop();
297  m_queueInfrared.pop();
298  }
299 
300  void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
301 
302 private:
303  bool m_cancelled;
304  std::condition_variable m_cond;
305  std::queue<vpImage<vpRGBa>> m_queueColor;
306  std::queue<vpImage<uint16_t>> m_queueDepth;
307 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
308  std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
309 #else
310  std::queue<std::vector<vpColVector>> m_queuePointCloud;
311 #endif
312  std::queue<vpImage<unsigned char>> m_queueInfrared;
313  size_t m_maxQueueSize;
314  std::mutex m_mutex;
315 };
316 
317 class vpStorageWorker
318 {
319 public:
320  vpStorageWorker(vpFrameQueue &queue, const std::string &directory, bool save_color, bool save_depth, bool save_pointcloud,
321  bool save_infrared, bool save_pointcloud_binary_format,
322  int
323 #ifndef VISP_HAVE_PCL
324  width
325 #endif
326  ,
327  int
328 #ifndef VISP_HAVE_PCL
329  height
330 #endif
331  )
332  : m_queue(queue), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
333  m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared),
334  m_save_pointcloud_binary_format(save_pointcloud_binary_format)
335 #ifndef VISP_HAVE_PCL
336  ,
337  m_size_height(height), m_size_width(width)
338 #endif
339  { }
340 
341  // Thread main loop
342  void run()
343  {
344  try {
345  vpImage<vpRGBa> colorImg;
346  vpImage<uint16_t> depthImg;
347 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
348  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud;
349 #else
350  std::vector<vpColVector> pointCloud;
351 #endif
352  vpImage<unsigned char> infraredImg;
353 
354  char buffer[FILENAME_MAX];
355  for (;;) {
356  m_queue.pop(colorImg, depthImg, pointCloud, infraredImg);
357 
358  if (!m_directory.empty()) {
359  std::stringstream ss;
360 
361  if (m_save_color) {
362  ss << m_directory << "/color_image_%04d.jpg";
363  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
364 
365  std::string filename_color = buffer;
366  vpImageIo::write(colorImg, filename_color);
367  }
368 
369  if (m_save_depth) {
370  ss.str("");
371  ss << m_directory << "/depth_image_%04d.bin";
372  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
373  std::string filename_depth = buffer;
374 
375  std::ofstream file_depth(filename_depth.c_str(), std::ios::out | std::ios::binary);
376  if (file_depth.is_open()) {
377  unsigned int height = depthImg.getHeight(), width = depthImg.getWidth();
378  vpIoTools::writeBinaryValueLE(file_depth, height);
379  vpIoTools::writeBinaryValueLE(file_depth, width);
380 
381  uint16_t value;
382  for (unsigned int i = 0; i < height; i++) {
383  for (unsigned int j = 0; j < width; j++) {
384  value = depthImg[i][j];
385  vpIoTools::writeBinaryValueLE(file_depth, value);
386  }
387  }
388  }
389  }
390 
391  if (m_save_pointcloud) {
392  ss.str("");
393  ss << m_directory << "/point_cloud_%04d" << (m_save_pointcloud_binary_format ? ".bin" : ".pcd");
394  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
395  std::string filename_point_cloud = buffer;
396 
397  if (m_save_pointcloud_binary_format) {
398  std::ofstream file_pointcloud(filename_point_cloud.c_str(), std::ios::out | std::ios::binary);
399 
400  if (file_pointcloud.is_open()) {
401 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
402  uint32_t width = pointCloud->width;
403  uint32_t height = pointCloud->height;
404  // true if pointcloud does not contain NaN or Inf, not handled currently
405  char is_dense = pointCloud->is_dense;
406 
407  vpIoTools::writeBinaryValueLE(file_pointcloud, height);
408  vpIoTools::writeBinaryValueLE(file_pointcloud, width);
409  file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
410 
411  for (uint32_t i = 0; i < height; i++) {
412  for (uint32_t j = 0; j < width; j++) {
413  pcl::PointXYZ pt = (*pointCloud)(j, i);
414 
415  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.x);
416  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.y);
417  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.z);
418  }
419  }
420 #else
421  uint32_t width = m_size_width;
422  uint32_t height = m_size_height;
423  // to be consistent with PCL version
424  char is_dense = 1;
425 
426  vpIoTools::writeBinaryValueLE(file_pointcloud, height);
427  vpIoTools::writeBinaryValueLE(file_pointcloud, width);
428  file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
429 
430  for (uint32_t i = 0; i < height; i++) {
431  for (uint32_t j = 0; j < width; j++) {
432  float x = (float)pointCloud[i * width + j][0];
433  float y = (float)pointCloud[i * width + j][1];
434  float z = (float)pointCloud[i * width + j][2];
435 
436  vpIoTools::writeBinaryValueLE(file_pointcloud, x);
437  vpIoTools::writeBinaryValueLE(file_pointcloud, y);
438  vpIoTools::writeBinaryValueLE(file_pointcloud, z);
439  }
440  }
441 #endif
442  }
443  }
444  else {
445 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
446  pcl::io::savePCDFileBinary(filename_point_cloud, *pointCloud);
447 #endif
448  }
449  }
450 
451  if (m_save_infrared) {
452  ss.str("");
453  ss << m_directory << "/infrared_image_%04d.jpg";
454  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
455 
456  std::string filename_infrared = buffer;
457  vpImageIo::write(infraredImg, filename_infrared);
458  }
459 
460  m_cpt++;
461  }
462  }
463  }
464  catch (const vpFrameQueue::vpCancelled_t &) {
465  std::cout << "Receive cancel vpFrameQueue." << std::endl;
466  }
467  }
468 
469 private:
470  vpFrameQueue &m_queue;
471  std::string m_directory;
472  unsigned int m_cpt;
473  bool m_save_color;
474  bool m_save_depth;
475  bool m_save_pointcloud;
476  bool m_save_infrared;
477  bool m_save_pointcloud_binary_format;
478 #ifndef VISP_HAVE_PCL
479  int m_size_height;
480  int m_size_width;
481 #endif
482 };
483 } // Namespace
484 
485 int main(int argc, const char *argv[])
486 {
487  bool save = false;
488  std::string output_directory = vpTime::getDateTime("%Y_%m_%d_%H.%M.%S");
489  std::string output_directory_custom = "";
490  bool use_aligned_stream = false;
491  bool save_color = false;
492  bool save_depth = false;
493  bool save_pointcloud = false;
494  bool save_infrared = false;
495  bool click_to_save = false;
496  int stream_fps = 30;
497  bool save_pointcloud_binary_format = false;
498 
499  // Read the command line options
500  if (!getOptions(argc, argv, save, output_directory_custom, use_aligned_stream, save_color, save_depth,
501  save_pointcloud, save_infrared, click_to_save, stream_fps, save_pointcloud_binary_format)) {
502  return EXIT_FAILURE;
503  }
504 
505  if (!output_directory_custom.empty())
506  output_directory = output_directory_custom + "/" + output_directory;
507 
508 #ifndef VISP_HAVE_PCL
509  save_pointcloud_binary_format = true;
510 #endif
511 
512  std::cout << "save: " << save << std::endl;
513  std::cout << "output_directory: " << output_directory << std::endl;
514  std::cout << "use_aligned_stream: " << use_aligned_stream << std::endl;
515  std::cout << "save_color: " << save_color << std::endl;
516  std::cout << "save_depth: " << save_depth << std::endl;
517  std::cout << "save_pointcloud: " << save_pointcloud << std::endl;
518  std::cout << "save_infrared: " << save_infrared << std::endl;
519  std::cout << "stream_fps: " << stream_fps << std::endl;
520  std::cout << "save_pointcloud_binary_format: " << save_pointcloud_binary_format << std::endl;
521  std::cout << "click_to_save: " << click_to_save << std::endl;
522 
523  int width = 640, height = 480;
524 #ifdef USE_REALSENSE2
525  vpRealSense2 realsense;
526 
527  rs2::config config;
528  config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
529  config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
530  config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, stream_fps);
531  realsense.open(config);
532 #else
533  vpRealSense realsense;
534  realsense.setStreamSettings(rs::stream::color,
535  vpRealSense::vpRsStreamParams(width, height, rs::format::rgba8, stream_fps));
536  realsense.setStreamSettings(rs::stream::depth,
537  vpRealSense::vpRsStreamParams(width, height, rs::format::z16, stream_fps));
538  realsense.setStreamSettings(rs::stream::infrared,
539  vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
540  realsense.setStreamSettings(rs::stream::infrared2,
541  vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
542 
543  realsense.open();
544 #endif
545 
546  vpImage<vpRGBa> I_color(height, width);
547  vpImage<unsigned char> I_depth(height, width);
548  vpImage<uint16_t> I_depth_raw(height, width);
549  vpImage<unsigned char> I_infrared(height, width);
550 
551 #ifdef VISP_HAVE_X11
552  vpDisplayX d1, d2, d3;
553 #else
554  vpDisplayGDI d1, d2, d3;
555 #endif
556  d1.init(I_color, 0, 0, "RealSense color stream");
557  d2.init(I_depth, I_color.getWidth() + 80, 0, "RealSense depth stream");
558  d3.init(I_infrared, I_color.getWidth() + 80, I_color.getHeight() + 70, "RealSense infrared stream");
559 
560  while (true) {
561  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, nullptr);
562  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
563 
564  vpDisplay::display(I_color);
565  vpDisplay::display(I_depth);
566  vpDisplay::displayText(I_color, 20, 20, "Click when ready.", vpColor::red);
567  vpDisplay::flush(I_color);
568  vpDisplay::flush(I_depth);
569 
570  if (vpDisplay::getClick(I_color, false)) {
571  break;
572  }
573  }
574 
575  if (save) {
576  // Create output directory
577  vpIoTools::makeDirectory(output_directory);
578 
579  // Save intrinsics
580 #ifdef USE_REALSENSE2
581  vpCameraParameters cam_color = realsense.getCameraParameters(RS2_STREAM_COLOR);
582  vpXmlParserCamera xml_camera;
583  xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
584 
585  if (use_aligned_stream) {
586  xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
587  }
588  else {
589  vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
590  xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
591  }
592 
593  vpCameraParameters cam_infrared = realsense.getCameraParameters(RS2_STREAM_INFRARED);
594  xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
595  vpHomogeneousMatrix depth_M_color;
596  if (!use_aligned_stream) {
597  depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
598  }
599 #else
600  vpCameraParameters cam_color = realsense.getCameraParameters(rs::stream::color);
601  vpXmlParserCamera xml_camera;
602  xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
603 
604  vpCameraParameters cam_color_rectified = realsense.getCameraParameters(rs::stream::rectified_color);
605  xml_camera.save(cam_color_rectified, output_directory + "/camera.xml", "color_camera_rectified", width, height);
606 
607  if (use_aligned_stream) {
608  vpCameraParameters cam_depth = realsense.getCameraParameters(rs::stream::depth);
609  xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
610  }
611  else {
612  xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
613  }
614 
615  vpCameraParameters cam_depth_aligned_to_rectified_color =
616  realsense.getCameraParameters(rs::stream::depth_aligned_to_rectified_color);
617  xml_camera.save(cam_depth_aligned_to_rectified_color, output_directory + "/camera.xml",
618  "depth_camera_aligned_to_rectified_color", width, height);
619 
620  vpCameraParameters cam_infrared = realsense.getCameraParameters(rs::stream::infrared);
621  xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
622  vpHomogeneousMatrix depth_M_color;
623  if (!use_aligned_stream) {
624  depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
625  }
626 #endif
627  std::ofstream file(std::string(output_directory + "/depth_M_color.txt"));
628  depth_M_color.save(file);
629  file.close();
630  }
631 
632  vpFrameQueue save_queue;
633  vpStorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud,
634  save_infrared, save_pointcloud_binary_format, width, height);
635  std::thread storage_thread(&vpStorageWorker::run, &storage);
636 
637 #ifdef USE_REALSENSE2
638  rs2::align align_to(RS2_STREAM_COLOR);
639  if (use_aligned_stream && save_infrared) {
640  std::cerr << "Cannot use aligned streams with infrared acquisition currently."
641  << "\nInfrared stream acquisition is disabled!"
642  << std::endl;
643  }
644 #endif
645 
646  int nb_saves = 0;
647  bool quit = false;
648 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
649  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
650 #else
651  std::vector<vpColVector> pointCloud;
652 #endif
653  while (!quit) {
654  if (use_aligned_stream) {
655 #ifdef USE_REALSENSE2
656 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
657  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud, nullptr,
658  &align_to);
659 #else
660  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud, nullptr,
661  &align_to);
662 #endif
663 #else
664 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
665  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
666  (unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
667  rs::stream::depth_aligned_to_rectified_color);
668 #else
669  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
670  (unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
671  rs::stream::depth_aligned_to_rectified_color);
672 #endif
673 #endif
674  }
675  else {
676 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
677  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
678  (unsigned char *)I_infrared.bitmap, nullptr);
679 #else
680  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
681  (unsigned char *)I_infrared.bitmap);
682 #endif
683  }
684 
685  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
686 
687  vpDisplay::display(I_color);
688  vpDisplay::display(I_depth);
689  vpDisplay::display(I_infrared);
690 
691  if (!click_to_save) {
692  vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
693  }
694  else {
695  std::stringstream ss;
696  ss << "Images saved: " << nb_saves;
697  vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
698  }
699 
700  vpDisplay::flush(I_color);
701  vpDisplay::flush(I_depth);
702  vpDisplay::flush(I_infrared);
703 
704  if (save && !click_to_save) {
705 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
706  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
707  save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
708 #else
709  save_queue.push(I_color, I_depth_raw, pointCloud, I_infrared);
710 #endif
711  }
712 
713  vpMouseButton::vpMouseButtonType button;
714  if (vpDisplay::getClick(I_color, button, false)) {
715  if (!click_to_save) {
716  save_queue.cancel();
717  quit = true;
718  }
719  else {
720  switch (button) {
721  case vpMouseButton::button1:
722  if (save) {
723  nb_saves++;
724 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
725  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
726  save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
727 #else
728  save_queue.push(I_color, I_depth_raw, pointCloud, I_infrared);
729 #endif
730  }
731  break;
732 
733  case vpMouseButton::button2:
734  case vpMouseButton::button3:
735  default:
736  save_queue.cancel();
737  quit = true;
738  break;
739  }
740  }
741  }
742  }
743 
744  storage_thread.join();
745 
746  return EXIT_SUCCESS;
747 }
748 #else
749 int main()
750 {
751  std::cerr << "Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl;
752 
753 #if !defined(VISP_HAVE_PUGIXML)
754  std::cout << "pugixml built-in 3rdparty is requested." << std::endl;
755 #endif
756  return EXIT_SUCCESS;
757 }
758 #endif
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:304
vpColor::red
static const vpColor red
Definition: vpColor.h:211
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
vpDisplayX::init
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
Definition: vpDisplayX.cpp:1798
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:850
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:825
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:801
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay_uchar.cpp:749
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:199
vpHomogeneousMatrix::save
void save(std::ofstream &f) const
Definition: vpHomogeneousMatrix.cpp:942
vpImageConvert::createDepthHistogram
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
Definition: vpImageConvert.cpp:252
vpImageIo::write
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:287
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:245
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:184
vpIoTools::makeDirectory
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:981
vpIoTools::writeBinaryValueLE
static void writeBinaryValueLE(std::ofstream &file, const int16_t short_value)
Definition: vpIoTools.cpp:2598
vpParseArgv::parse
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
vpRealSense2::open
bool open(const rs2::config &cfg=rs2::config())
Definition: vpRealSense2.cpp:1355
vpRealSense::setStreamSettings
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
Definition: vpRealSense.cpp:609
vpRealSense::getCameraParameters
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
Definition: vpRealSense.cpp:288
vpRealSense::acquire
void acquire(std::vector< vpColVector > &pointcloud)
Definition: vpRealSense.cpp:423
vpRealSense::getTransformation
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
Definition: vpRealSense.cpp:359
vpXmlParserCamera
XML parser to load and save intrinsic camera parameters.
Definition: vpXmlParserCamera.h:164
vpXmlParserCamera::save
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="", bool verbose=true)
Definition: vpXmlParserCamera.cpp:1203
vpTime::getDateTime
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")