Visual Servoing Platform  version 3.6.1 under development (2024-05-15)
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 "se:o:acdpzijCf:bvh"
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  << " [-e <filename pattern (e.g. %06d)>]"
87  << " [-a]"
88  << " [-c]"
89  << " [-d]"
90  << " [-p]"
91  << " [-b]"
92  << " [-z]"
93  << " [-i]"
94  << " [-j]"
95  << " [-C]"
96  << " [-f <fps>]"
97  << " [-v]"
98  << " [-o <directory>]"
99  << " [--help,-h]"
100  << std::endl;
101  std::cout << "\nOPTIONS " << std::endl
102  << " -s" << std::endl
103  << " Flag to enable data saving." << std::endl
104  << std::endl
105  << " -e <pattern>" << std::endl
106  << " Filename pattern when saving data." << std::endl
107  << std::endl
108  << " -a" << std::endl
109  << " Color and depth are aligned." << std::endl
110  << std::endl
111  << " -c" << std::endl
112  << " Add color stream to saved data when -s option is enable." << std::endl
113  << std::endl
114  << " -d" << std::endl
115  << " Add depth stream to saved data when -s option is enable." << std::endl
116  << std::endl
117  << " -p" << std::endl
118  << " Add point cloud stream to saved data when -s option is enabled." << std::endl
119  << " By default (if available), the point cloud is saved in Point Cloud Data file format (.PCD extension file)."
120  << std::endl
121  << " You can also use the -z option to save the point cloud in .npz (NumPy)." << std::endl
122  << std::endl
123  << " -b" << std::endl
124  << " Force depth and pointcloud to be saved in (little-endian) binary format." << std::endl
125  << std::endl
126  << " -z" << std::endl
127  << " Pointcloud is saved in NPZ format." << std::endl
128  << std::endl
129  << " -i" << std::endl
130  << " Add infrared stream to saved data when -s option is enabled." << std::endl
131  << std::endl
132  << " -j" << std::endl
133  << " Save image data using JPEG format (otherwise PNG is used)." << std::endl
134  << std::endl
135  << " -C" << std::endl
136  << " Trigger one shot data saver after each user click." << std::endl
137  << std::endl
138  << " -f <fps>" << std::endl
139  << " Set camera framerate." << std::endl
140  << " Default: " << fps << std::endl
141  << std::endl
142  << " -v" << std::endl
143  << " Display depth using a cumulative histogram." << std::endl
144  << " Warning: this operation is time consuming" << std::endl
145  << std::endl
146  << " -o <directory>" << std::endl
147  << " Output directory that will host saved data." << std::endl
148  << std::endl
149  << " --help, -h" << std::endl
150  << " Display this helper message." << std::endl
151  << std::endl;
152  std::cout << "\nEXAMPLE " << std::endl
153  << "- Save aligned color + depth + point cloud in data folder" << std::endl
154  << " " << name << " -s -a -c -d -p -o data" << std::endl
155  << "- Save color + IR + depth + point cloud in NPZ format in data folder" << std::endl
156  << " " << name << " -s -c -d -i -p -z -o data" << std::endl
157  << std::endl;
158 
159  if (badparam) {
160  std::cout << "\nERROR: Bad parameter " << badparam << std::endl;
161  }
162 }
163 
164 bool getOptions(int argc, const char *argv[], bool &save, std::string &pattern, std::string &output_directory,
165  bool &use_aligned_stream, bool &save_color, bool &save_depth, bool &save_pointcloud,
166  bool &save_infrared, bool &click_to_save, int &stream_fps, bool &save_pcl_npz_format,
167  bool &save_force_binary_format, bool &save_jpeg, bool &depth_hist_visu)
168 {
169  const char *optarg;
170  const char **argv1 = (const char **)argv;
171  int c;
172  while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
173 
174  switch (c) {
175  case 's':
176  save = true;
177  break;
178  case 'e':
179  pattern = optarg;
180  break;
181  case 'o':
182  output_directory = optarg;
183  break;
184  case 'a':
185  use_aligned_stream = true;
186  break;
187  case 'c':
188  save_color = true;
189  break;
190  case 'd':
191  save_depth = true;
192  break;
193  case 'p':
194  save_pointcloud = true;
195  break;
196  case 'i':
197  save_infrared = true;
198  break;
199  case 'j':
200  save_jpeg = true;
201  break;
202  case 'C':
203  click_to_save = true;
204  break;
205  case 'f':
206  stream_fps = atoi(optarg);
207  break;
208  case 'v':
209  depth_hist_visu = true;
210  break;
211  case 'z':
212  save_pcl_npz_format = true;
213  break;
214  case 'b':
215  save_force_binary_format = true;
216  break;
217 
218  case 'h':
219  usage(argv[0], nullptr, stream_fps);
220  return false;
221  break;
222 
223  default:
224  usage(argv[0], optarg, stream_fps);
225  return false;
226  break;
227  }
228  }
229 
230  if ((c == 1) || (c == -1)) {
231  // standalone param or error
232  usage(argv[0], nullptr, stream_fps);
233  std::cerr << "ERROR: " << std::endl;
234  std::cerr << " Bad argument " << optarg << std::endl
235  << std::endl;
236  return false;
237  }
238 
239  return true;
240 }
241 
242 // Code adapted from: https://stackoverflow.com/a/37146523
243 class vpFrameQueue
244 {
245 public:
246  struct vpCancelled_t
247  { };
248 
249  vpFrameQueue()
250  : m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(),
251  m_maxQueueSize(1024 * 8), m_mutex()
252  { }
253 
254  void cancel()
255  {
256  std::lock_guard<std::mutex> lock(m_mutex);
257  m_cancelled = true;
258  m_cond.notify_all();
259  }
260 
261  // Push data to save in the queue (FIFO)
262  void push(const std::unique_ptr<vpImage<vpRGBa>> &ptr_colorImg,
263  const std::unique_ptr<vpImage<uint16_t>> &ptr_depthImg,
264 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
265  const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
266 #else
267  const std::unique_ptr<std::vector<vpColVector>> &ptr_pointCloud,
268 #endif
269  const std::unique_ptr<vpImage<unsigned char>> &ptr_infraredImg)
270  {
271  std::lock_guard<std::mutex> lock(m_mutex);
272 
273  if (ptr_colorImg) {
274  m_queueColor.push(*ptr_colorImg);
275  }
276  if (ptr_depthImg) {
277  m_queueDepth.push(*ptr_depthImg);
278  }
279 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
280  if (pointCloud) {
281  m_queuePointCloud.push(pointCloud);
282  }
283 #else
284  if (ptr_pointCloud) {
285  m_queuePointCloud.push(*ptr_pointCloud);
286  }
287 #endif
288  if (ptr_infraredImg) {
289  m_queueInfrared.push(*ptr_infraredImg);
290  }
291 
292  // Pop extra data in the queue
293  while (m_queueColor.size() > m_maxQueueSize) {
294  m_queueColor.pop();
295  }
296 
297  // Pop extra data in the queue
298  while (m_queueDepth.size() > m_maxQueueSize) {
299  m_queueDepth.pop();
300  }
301 
302  // Pop extra data in the queue
303  while (m_queuePointCloud.size() > m_maxQueueSize) {
304  m_queuePointCloud.pop();
305  }
306 
307  // Pop extra data in the queue
308  while (m_queueInfrared.size() > m_maxQueueSize) {
309  m_queueInfrared.pop();
310  }
311 
312  m_cond.notify_one();
313  }
314 
315  // Pop the image to save from the queue (FIFO)
316  void pop(std::unique_ptr<vpImage<vpRGBa>> &ptr_colorImg,
317  std::unique_ptr<vpImage<uint16_t>> &ptr_depthImg,
318 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
319  pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
320 #else
321  std::unique_ptr<std::vector<vpColVector>> &ptr_pointCloud,
322 #endif
323  std::unique_ptr<vpImage<unsigned char>> &ptr_infraredImg)
324  {
325  std::unique_lock<std::mutex> lock(m_mutex);
326 
327  // Since we push all 4 data at a time, there should be no situation where a queue size is different from the others
328  while (m_queueColor.empty() && m_queueDepth.empty() && m_queuePointCloud.empty() && m_queueInfrared.empty()) {
329  if (m_cancelled) {
330  throw vpCancelled_t();
331  }
332 
333  m_cond.wait(lock);
334 
335  if (m_cancelled) {
336  throw vpCancelled_t();
337  }
338  }
339 
340  if (!m_queueColor.empty()) {
341  ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(m_queueColor.front());
342  m_queueColor.pop();
343  }
344  if (!m_queueDepth.empty()) {
345  ptr_depthImg = std::make_unique<vpImage<uint16_t>>(m_queueDepth.front());
346  m_queueDepth.pop();
347  }
348 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
349  if (!m_queuePointCloud.empty()) {
350  pointCloud = m_queuePointCloud.front();
351  m_queuePointCloud.pop();
352  }
353 #else
354  if (!m_queuePointCloud.empty()) {
355  ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(m_queuePointCloud.front());
356  m_queuePointCloud.pop();
357  }
358 #endif
359  if (!m_queueInfrared.empty()) {
360  ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(m_queueInfrared.front());
361  m_queueInfrared.pop();
362  }
363  }
364 
365  bool empty()
366  {
367  std::lock_guard<std::mutex> lock(m_mutex);
368  return m_queueColor.empty() && m_queueDepth.empty() && m_queuePointCloud.empty() && m_queueInfrared.empty();
369  }
370 
371  void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
372 
373 private:
374  bool m_cancelled;
375  std::condition_variable m_cond;
376  std::queue<vpImage<vpRGBa>> m_queueColor;
377  std::queue<vpImage<uint16_t>> m_queueDepth;
378 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
379  std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
380 #else
381  std::queue<std::vector<vpColVector>> m_queuePointCloud;
382 #endif
383  std::queue<vpImage<unsigned char>> m_queueInfrared;
384  size_t m_maxQueueSize;
385  std::mutex m_mutex;
386 };
387 
388 class vpStorageWorker
389 {
390 public:
391  vpStorageWorker(vpFrameQueue &queue, const std::string &save_pattern, const std::string &directory, bool save_color,
392  bool save_depth, bool save_pointcloud, bool save_infrared, bool save_pcl_npz_format,
393  bool save_force_binary_format, bool save_jpeg,
394  int
395 #ifndef VISP_HAVE_PCL
396  width
397 #endif
398  ,
399  int
400 #ifndef VISP_HAVE_PCL
401  height
402 #endif
403  )
404  : m_queue(queue), m_save_pattern(save_pattern), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
405  m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared), m_save_pcl_npz_format(save_pcl_npz_format),
406  m_save_force_binary_format(save_force_binary_format), m_save_jpeg(save_jpeg)
407 #ifndef VISP_HAVE_PCL
408  ,
409  m_size_height(height), m_size_width(width)
410 #endif
411  { }
412 
413  // Thread main loop
414  void run()
415  {
416  try {
417  std::unique_ptr<vpImage<vpRGBa>> ptr_colorImg;
418  std::unique_ptr<vpImage<uint16_t>> ptr_depthImg;
419 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
420  pcl::PointCloud<pcl::PointXYZ>::Ptr ptr_pointCloud;
421 #else
422  std::unique_ptr<std::vector<vpColVector>> ptr_pointCloud;
423 #endif
424  std::unique_ptr<vpImage<unsigned char>> ptr_infraredImg;
425 
426  std::vector<float> vec_pcl;
427 
428  char buffer[FILENAME_MAX];
429  std::string image_filename_ext = m_save_jpeg ? ".jpg" : ".png";
430  for (;;) {
431  m_queue.pop(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
432 
433  if (!m_directory.empty()) {
434  std::string current_time = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
435  std::stringstream ss;
436 
437  if (m_save_color && ptr_colorImg) {
438  ss << m_directory << "/color_image_" << m_save_pattern << image_filename_ext;
439  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
440 
441  std::string filename_color = buffer;
442  vpImageIo::write(*ptr_colorImg, filename_color);
443  }
444 
445  if (m_save_depth && ptr_depthImg) {
446  ss.str("");
447 
448  if (m_save_force_binary_format) {
449  ss << m_directory << "/depth_image_" << m_save_pattern << ".bin";
450  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
451  std::string filename_depth = buffer;
452 
453  std::ofstream file_depth(filename_depth.c_str(), std::ios::out | std::ios::binary);
454  if (file_depth.is_open()) {
455  unsigned int height = ptr_depthImg->getHeight(), width = ptr_depthImg->getWidth();
456  vpIoTools::writeBinaryValueLE(file_depth, height);
457  vpIoTools::writeBinaryValueLE(file_depth, width);
458 
459  uint16_t value;
460  for (unsigned int i = 0; i < height; i++) {
461  for (unsigned int j = 0; j < width; j++) {
462  value = (*ptr_depthImg)[i][j];
463  vpIoTools::writeBinaryValueLE(file_depth, value);
464  }
465  }
466  }
467  }
468  else {
469  ss << m_directory << "/depth_image_" << m_save_pattern << ".npz";
470  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
471  std::string filename_depth = buffer;
472 
473  // Write Npz headers
474  std::vector<char> vec_filename(filename_depth.begin(), filename_depth.end());
475  // Null-terminated character is handled at reading
476  // For null-terminated character handling, see:
477  // https://stackoverflow.com/a/8247804
478  // https://stackoverflow.com/a/45491652
479  visp::cnpy::npz_save(filename_depth, "filename", &vec_filename[0], { vec_filename.size() }, "w");
480 
481  std::vector<char> vec_current_time(current_time.begin(), current_time.end());
482  visp::cnpy::npz_save(filename_depth, "timestamp", &vec_current_time, { vec_current_time.size() }, "a");
483 
484  unsigned int height = ptr_depthImg->getHeight();
485  unsigned int width = ptr_depthImg->getWidth();
486  unsigned int channel = 1;
487  visp::cnpy::npz_save(filename_depth, "height", &height, { 1 }, "a");
488  visp::cnpy::npz_save(filename_depth, "width", &width, { 1 }, "a");
489  visp::cnpy::npz_save(filename_depth, "channel", &channel, { 1 }, "a");
490 
491  // Write data
492  std::vector<uint16_t> I_depth_raw_vec(ptr_depthImg->bitmap, ptr_depthImg->bitmap + ptr_depthImg->getSize());
493  visp::cnpy::npz_save(filename_depth, "data", I_depth_raw_vec.data(), { height, width }, "a");
494  }
495  }
496 
497  if (m_save_pointcloud && ptr_pointCloud) {
498  ss.str("");
499  std::string pcl_extension = m_save_force_binary_format ? ".bin" : (m_save_pcl_npz_format ? ".npz" : ".pcd");
500  ss << m_directory << "/point_cloud_" << m_save_pattern << pcl_extension;
501  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
502  std::string filename_point_cloud = buffer;
503 
504 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
505  uint32_t width = ptr_pointCloud->width;
506  uint32_t height = ptr_pointCloud->height;
507 #else
508  uint32_t width = m_size_width;
509  uint32_t height = m_size_height;
510 #endif
511 
512  if (m_save_force_binary_format) {
513  std::ofstream file_pointcloud(filename_point_cloud.c_str(), std::ios::out | std::ios::binary);
514 
515  if (file_pointcloud.is_open()) {
516 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
517  // true if ptr_pointCloud does not contain NaN or Inf, not handled currently
518  char is_dense = ptr_pointCloud->is_dense;
519 
520  vpIoTools::writeBinaryValueLE(file_pointcloud, height);
521  vpIoTools::writeBinaryValueLE(file_pointcloud, width);
522  file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
523 
524  for (uint32_t i = 0; i < height; i++) {
525  for (uint32_t j = 0; j < width; j++) {
526  pcl::PointXYZ pt = (*ptr_pointCloud)(j, i);
527 
528  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.x);
529  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.y);
530  vpIoTools::writeBinaryValueLE(file_pointcloud, pt.z);
531  }
532  }
533 #else
534  // to be consistent with PCL version
535  const char is_dense = 1;
536 
537  vpIoTools::writeBinaryValueLE(file_pointcloud, height);
538  vpIoTools::writeBinaryValueLE(file_pointcloud, width);
539  file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
540 
541  for (uint32_t i = 0; i < height; i++) {
542  for (uint32_t j = 0; j < width; j++) {
543  float x = (float)(*ptr_pointCloud)[i * width + j][0];
544  float y = (float)(*ptr_pointCloud)[i * width + j][1];
545  float z = (float)(*ptr_pointCloud)[i * width + j][2];
546 
547  vpIoTools::writeBinaryValueLE(file_pointcloud, x);
548  vpIoTools::writeBinaryValueLE(file_pointcloud, y);
549  vpIoTools::writeBinaryValueLE(file_pointcloud, z);
550  }
551  }
552 #endif
553  }
554  }
555  else if (m_save_pcl_npz_format) {
556  // Write Npz headers
557  std::vector<char> vec_filename(filename_point_cloud.begin(), filename_point_cloud.end());
558  // Null-terminated character is handled at reading
559  // For null-terminated character handling, see:
560  // https://stackoverflow.com/a/8247804
561  // https://stackoverflow.com/a/45491652
562  visp::cnpy::npz_save(filename_point_cloud, "filename", &vec_filename[0], { vec_filename.size() }, "w");
563 
564  std::vector<char> vec_current_time(current_time.begin(), current_time.end());
565  visp::cnpy::npz_save(filename_point_cloud, "timestamp", &vec_current_time, { vec_current_time.size() }, "a");
566 
567  const uint32_t channels = 3;
568  visp::cnpy::npz_save(filename_point_cloud, "height", &height, { 1 }, "a");
569  visp::cnpy::npz_save(filename_point_cloud, "width", &width, { 1 }, "a");
570  visp::cnpy::npz_save(filename_point_cloud, "channel", &channels, { 1 }, "a");
571 
572 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
573  // can probably be optimized by assuming channel=1 and use data of type XYZ
574  // but this should probably not work with the Python script for display
575  for (uint32_t i = 0; i < height; i++) {
576  for (uint32_t j = 0; j < width; j++) {
577  pcl::PointXYZ pt = (*ptr_pointCloud)(j, i);
578  vec_pcl[channels * (i*width + j) + 0] = pt.x;
579  vec_pcl[channels * (i*width + j) + 1] = pt.y;
580  vec_pcl[channels * (i*width + j) + 2] = pt.z;
581  }
582  }
583 #else
584  vec_pcl.resize(height * width * channels);
585  for (uint32_t i = 0; i < height; i++) {
586  for (uint32_t j = 0; j < width; j++) {
587  vec_pcl[channels * (i*width + j) + 0] = (*ptr_pointCloud)[i*width + j][0];
588  vec_pcl[channels * (i*width + j) + 1] = (*ptr_pointCloud)[i*width + j][1];
589  vec_pcl[channels * (i*width + j) + 2] = (*ptr_pointCloud)[i*width + j][2];
590  }
591  }
592 #endif
593  // Write data
594  visp::cnpy::npz_save(filename_point_cloud, "data", vec_pcl.data(), { height, width, channels }, "a");
595  }
596  else {
597 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
598  pcl::io::savePCDFileBinary(filename_point_cloud, *ptr_pointCloud);
599 #endif
600  }
601  }
602 
603  if (m_save_infrared && ptr_infraredImg) {
604  ss.str("");
605  ss << m_directory << "/infrared_image_" << m_save_pattern << image_filename_ext;
606  snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
607 
608  std::string filename_infrared = buffer;
609  vpImageIo::write(*ptr_infraredImg, filename_infrared);
610  }
611 
612  m_cpt++;
613  }
614  }
615  }
616  catch (const vpFrameQueue::vpCancelled_t &) {
617  std::cout << "Receive cancel vpFrameQueue." << std::endl;
618  }
619  }
620 
621 private:
622  vpFrameQueue &m_queue;
623  std::string m_save_pattern;
624  std::string m_directory;
625  unsigned int m_cpt;
626  bool m_save_color;
627  bool m_save_depth;
628  bool m_save_pointcloud;
629  bool m_save_infrared;
630  bool m_save_pcl_npz_format;
631  bool m_save_force_binary_format;
632  bool m_save_jpeg;
633 #ifndef VISP_HAVE_PCL
634  int m_size_height;
635  int m_size_width;
636 #endif
637 };
638 } // Namespace
639 
640 int main(int argc, const char *argv[])
641 {
642  bool save = false;
643  std::string save_pattern = "%04d";
644  std::string output_directory = vpTime::getDateTime("%Y_%m_%d_%H.%M.%S");
645  std::string output_directory_custom = "";
646  bool use_aligned_stream = false;
647  bool save_color = false;
648  bool save_depth = false;
649  bool save_pointcloud = false;
650  bool save_infrared = false;
651  bool click_to_save = false;
652  int stream_fps = 30;
653  bool save_pcl_npz_format = false;
654  bool save_force_binary_format = false;
655  bool save_jpeg = false;
656  bool depth_hist_visu = false;
657 
658  // Read the command line options
659  if (!getOptions(argc, argv, save, save_pattern, output_directory_custom, use_aligned_stream, save_color, save_depth,
660  save_pointcloud, save_infrared, click_to_save, stream_fps, save_pcl_npz_format,
661  save_force_binary_format, save_jpeg, depth_hist_visu)) {
662  return EXIT_FAILURE;
663  }
664 
665  if (!output_directory_custom.empty())
666  output_directory = output_directory_custom + "/" + output_directory;
667 
668 #ifndef VISP_HAVE_PCL
669  save_pcl_npz_format = !save_force_binary_format ? true : false;
670 #endif
671 
672  std::cout << "save: " << save << std::endl;
673  std::cout << "save_pattern: " << save_pattern << std::endl;
674  std::cout << "output_directory: " << output_directory << std::endl;
675  std::cout << "use_aligned_stream: " << use_aligned_stream << std::endl;
676  std::cout << "save_color: " << save_color << std::endl;
677  std::cout << "save_depth: " << save_depth << std::endl;
678  std::cout << "save_pointcloud: " << save_pointcloud << std::endl;
679  std::cout << "save_infrared: " << save_infrared << std::endl;
680  std::cout << "save_jpeg: " << save_jpeg << std::endl;
681  std::cout << "stream_fps: " << stream_fps << std::endl;
682  std::cout << "depth_hist_visu: " << depth_hist_visu << std::endl;
683  std::cout << "save_pcl_npz_format: " << save_pcl_npz_format << std::endl;
684  std::cout << "save_force_binary_format: " << save_force_binary_format << std::endl;
685  std::cout << "click_to_save: " << click_to_save << std::endl;
686 
687  int width = 640, height = 480;
688 #ifdef USE_REALSENSE2
689  vpRealSense2 realsense;
690 
691  rs2::config config;
692  config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
693  config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
694  config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, stream_fps);
695  realsense.open(config);
696 #else
697  vpRealSense realsense;
698  realsense.setStreamSettings(rs::stream::color,
699  vpRealSense::vpRsStreamParams(width, height, rs::format::rgba8, stream_fps));
700  realsense.setStreamSettings(rs::stream::depth,
701  vpRealSense::vpRsStreamParams(width, height, rs::format::z16, stream_fps));
702  realsense.setStreamSettings(rs::stream::infrared,
703  vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
704  realsense.setStreamSettings(rs::stream::infrared2,
705  vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
706 
707  realsense.open();
708 #endif
709 
710  vpImage<vpRGBa> I_color(height, width);
711  vpImage<unsigned char> I_depth(height, width);
712  vpImage<uint16_t> I_depth_raw(height, width);
713  vpImage<unsigned char> I_infrared(height, width);
714 
715 #ifdef VISP_HAVE_X11
716  vpDisplayX d1, d2, d3;
717 #else
718  vpDisplayGDI d1, d2, d3;
719 #endif
720  d1.init(I_color, 0, 0, "RealSense color stream");
721  d2.init(I_depth, I_color.getWidth() + 80, 0, "RealSense depth stream");
722  d3.init(I_infrared, I_color.getWidth() + 80, I_color.getHeight() + 70, "RealSense infrared stream");
723 
724  while (true) {
725  double start = vpTime::measureTimeMs();
726  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, nullptr);
727  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
728 
729  vpDisplay::display(I_color);
730  vpDisplay::display(I_depth);
731 
732  double delta_time = vpTime::measureTimeMs() - start;
733  std::ostringstream oss_time;
734  oss_time << delta_time << " ms ; fps=" << 1000/delta_time;
735  vpDisplay::displayText(I_color, 40, 20, oss_time.str(), vpColor::red);
736  vpDisplay::displayText(I_color, 20, 20, "Click when ready.", vpColor::red);
737 
738  vpDisplay::flush(I_color);
739  vpDisplay::flush(I_depth);
740 
741  if (vpDisplay::getClick(I_color, false)) {
742  break;
743  }
744  }
745 
746  if (save) {
747  // Create output directory
748  vpIoTools::makeDirectory(output_directory);
749 
750  // Save intrinsics
751 #ifdef USE_REALSENSE2
752  vpCameraParameters cam_color = realsense.getCameraParameters(RS2_STREAM_COLOR);
753  vpXmlParserCamera xml_camera;
754  xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
755 
756  if (use_aligned_stream) {
757  xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
758  }
759  else {
760  vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
761  xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
762  }
763 
764  vpCameraParameters cam_infrared = realsense.getCameraParameters(RS2_STREAM_INFRARED);
765  xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
766  vpHomogeneousMatrix depth_M_color;
767  if (!use_aligned_stream) {
768  depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
769  }
770 #else
771  vpCameraParameters cam_color = realsense.getCameraParameters(rs::stream::color);
772  vpXmlParserCamera xml_camera;
773  xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
774 
775  vpCameraParameters cam_color_rectified = realsense.getCameraParameters(rs::stream::rectified_color);
776  xml_camera.save(cam_color_rectified, output_directory + "/camera.xml", "color_camera_rectified", width, height);
777 
778  if (use_aligned_stream) {
779  vpCameraParameters cam_depth = realsense.getCameraParameters(rs::stream::depth);
780  xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
781  }
782  else {
783  xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
784  }
785 
786  vpCameraParameters cam_depth_aligned_to_rectified_color =
787  realsense.getCameraParameters(rs::stream::depth_aligned_to_rectified_color);
788  xml_camera.save(cam_depth_aligned_to_rectified_color, output_directory + "/camera.xml",
789  "depth_camera_aligned_to_rectified_color", width, height);
790 
791  vpCameraParameters cam_infrared = realsense.getCameraParameters(rs::stream::infrared);
792  xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
793  vpHomogeneousMatrix depth_M_color;
794  if (!use_aligned_stream) {
795  depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
796  }
797 #endif
798  std::ofstream file(std::string(output_directory + "/depth_M_color.txt"));
799  depth_M_color.save(file);
800  file.close();
801  }
802 
803  vpFrameQueue save_queue;
804  vpStorageWorker storage(std::ref(save_queue), save_pattern, std::cref(output_directory), save_color, save_depth,
805  save_pointcloud, save_infrared, save_pcl_npz_format, save_force_binary_format, save_jpeg, width, height);
806  std::thread storage_thread(&vpStorageWorker::run, &storage);
807 
808 #ifdef USE_REALSENSE2
809  rs2::align align_to(RS2_STREAM_COLOR);
810  if (use_aligned_stream && save_infrared) {
811  std::cerr << "Cannot use aligned streams with infrared acquisition currently."
812  << "\nInfrared stream acquisition is disabled!"
813  << std::endl;
814  }
815 #endif
816 
817  int nb_saves = 0;
818  bool quit = false;
819  // If PCL is available, always use PCL datatype even if we will save in NPZ or BIN file format
820 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
821  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
822 #else
823  std::vector<vpColVector> pointCloud;
824 #endif
825 
826  std::unique_ptr<vpImage<vpRGBa>> ptr_colorImg;
827  std::unique_ptr<vpImage<uint16_t>> ptr_depthImg;
828  std::unique_ptr<std::vector<vpColVector>> ptr_pointCloud;
829  std::unique_ptr<vpImage<unsigned char>> ptr_infraredImg;
830 
831  std::vector<double> vec_delta_time;
832  while (!quit) {
833  double start = vpTime::measureTimeMs();
834  if (use_aligned_stream) {
835 #ifdef USE_REALSENSE2
836 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
837  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud, nullptr,
838  &align_to);
839 #else
840  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud, nullptr,
841  &align_to);
842 #endif
843 #else
844 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
845  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
846  (unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
847  rs::stream::depth_aligned_to_rectified_color);
848 #else
849  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
850  (unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
851  rs::stream::depth_aligned_to_rectified_color);
852 #endif
853 #endif
854  }
855  else {
856 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
857  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
858  (unsigned char *)I_infrared.bitmap, nullptr);
859 #else
860  realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
861  (unsigned char *)I_infrared.bitmap);
862 #endif
863  }
864 
865  if (depth_hist_visu) {
866  vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
867  }
868  else {
869  // Seems like sometimes using createDepthHistogram takes lots of time?
870  // so we simply perform bit shift from uint16_t to uint8_t
871  for (unsigned int i = 0; i < I_depth_raw.getRows(); i++) {
872  for (unsigned int j = 0; j < I_depth_raw.getCols(); j++) {
873  I_depth[i][j] = I_depth_raw[i][j] >> 8;
874  }
875  }
876  }
877 
878  vpDisplay::display(I_color);
879  vpDisplay::display(I_depth);
880  vpDisplay::display(I_infrared);
881 
882  if (!click_to_save) {
883  vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
884  }
885  else {
886  std::stringstream ss;
887  ss << "Images saved: " << nb_saves;
888  vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
889  }
890 
891  if (save && !click_to_save) {
892  if (save_color) {
893  ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(I_color);
894  }
895  if (save_depth) {
896  ptr_depthImg = std::make_unique<vpImage<uint16_t>>(I_depth_raw);
897  }
898  if (save_infrared) {
899  ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(I_infrared);
900  }
901 
902 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
903  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy;
904  if (save_pointcloud) {
905  pointCloud_copy = pointCloud->makeShared();
906  }
907  save_queue.push(ptr_colorImg, ptr_depthImg, pointCloud_copy, ptr_infraredImg);
908 #else
909  if (save_pointcloud) {
910  ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(pointCloud);
911  }
912  save_queue.push(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
913 #endif
914  }
915 
916  double delta_time = vpTime::measureTimeMs() - start;
917  vec_delta_time.push_back(delta_time);
918  std::ostringstream oss_time;
919  oss_time << delta_time << " ms ; fps=" << 1000/delta_time;
920  vpDisplay::displayText(I_color, 40, 20, oss_time.str(), vpColor::red);
921 
922  vpDisplay::flush(I_color);
923  vpDisplay::flush(I_depth);
924  vpDisplay::flush(I_infrared);
925 
926  vpMouseButton::vpMouseButtonType button;
927  if (vpDisplay::getClick(I_color, button, false)) {
928  if (!click_to_save) {
929  save = false;
930  quit = true;
931  save_queue.cancel();
932  }
933  else {
934  switch (button) {
935  case vpMouseButton::button1:
936  if (save) {
937  nb_saves++;
938 
939  if (save_color) {
940  ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(I_color);
941  }
942  if (save_depth) {
943  ptr_depthImg = std::make_unique<vpImage<uint16_t>>(I_depth_raw);
944  }
945  if (save_infrared) {
946  ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(I_infrared);
947  }
948 
949 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
950  pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy;
951  if (save_pointcloud) {
952  pointCloud_copy = pointCloud->makeShared();
953  }
954  save_queue.push(ptr_colorImg, ptr_depthImg, pointCloud_copy, ptr_infraredImg);
955 #else
956  if (save_pointcloud) {
957  ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(pointCloud);
958  }
959  save_queue.push(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
960 #endif
961  }
962  break;
963 
964  case vpMouseButton::button2:
965  case vpMouseButton::button3:
966  default:
967  save = false;
968  quit = true;
969  save_queue.cancel();
970  break;
971  }
972  }
973  }
974  }
975 
976  double mean_vec_delta_time = vpMath::getMean(vec_delta_time);
977  double median_vec_delta_time = vpMath::getMedian(vec_delta_time);
978  double std_vec_delta_time = vpMath::getStdev(vec_delta_time);
979  std::cout << "Acquisition time, mean=" << mean_vec_delta_time << " ms ; median="
980  << median_vec_delta_time << " ms ; std=" << std_vec_delta_time << " ms" << std::endl;
981  std::cout << "FPS, mean=" << 1000/mean_vec_delta_time << " fps ; median="
982  << 1000/median_vec_delta_time << " fps" << std::endl;
983 
984  storage_thread.join();
985 
986  return EXIT_SUCCESS;
987 }
988 #else
989 int main()
990 {
991  std::cerr << "Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl;
992 
993 #if !defined(VISP_HAVE_PUGIXML)
994  std::cout << "pugixml built-in 3rdparty is requested." << std::endl;
995 #endif
996  return EXIT_SUCCESS;
997 }
998 #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:198
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
vpIoTools::makeDirectory
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:983
vpIoTools::writeBinaryValueLE
static void writeBinaryValueLE(std::ofstream &file, const int16_t short_value)
Definition: vpIoTools.cpp:2600
vpMath::getMedian
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:323
vpMath::getStdev
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:354
vpMath::getMean
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:303
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
visp::cnpy::npz_save
void npz_save(std::string zipname, std::string fname, const T *data, const std::vector< size_t > &shape, std::string mode="w")
Definition: vpIoTools.h:233
vpTime::getDateTime
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
vpTime::measureTimeMs
VISP_EXPORT double measureTimeMs()