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