Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
visp-save-rs-dataset.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 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
38#include <iostream>
39
40#include <visp3/core/vpConfig.h>
41#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_THREADS) \
42 && defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_PUGIXML) \
43 && ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
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/vpIoException.h>
64#include <visp3/core/vpIoTools.h>
65#include <visp3/core/vpXmlParserCamera.h>
66#include <visp3/gui/vpDisplayFactory.h>
67#include <visp3/io/vpImageIo.h>
68#include <visp3/sensor/vpRealSense2.h>
69
70#ifdef ENABLE_VISP_NAMESPACE
71using namespace VISP_NAMESPACE_NAME;
72#endif
73
74namespace
75{
76void usage(const char *argv[], int error, int stream_width, int stream_height, int stream_fps)
77{
78 std::cout << "\nNAME " << std::endl
79 << " " << vpIoTools::getName(argv[0])
80 << " - Record data (color, depth, infrared, point cloud) with a Realsense device." << std::endl;
81
82 std::cout << "\nDESCRIPTION " << std::endl
83 << " This app allows to record a dataset (color, depth, infrared, point cloud) acquired" << std::endl
84 << " with a Realsense device. Once acquired, the dataset can be visualized using visp-read-rs-dataset.cpp app."
85 << std::endl;
86
87 std::cout << "\nSYNOPSIS " << std::endl
88 << " " << vpIoTools::getName(argv[0])
89 << " [--save,-s]"
90 << " [--output-folder,-o <output folder>]"
91 << " [--pattern,-e <filename numbering pattern (e.g. %06d)>]"
92 << " [--step-by-step,-C]"
93 << " [--aligned,-a]"
94 << " [--save-color,-c]"
95 << " [--save-depth,-d]"
96 << " [--save-infrared,-i]"
97 << " [--save-pcl,-p]"
98 << " [--img-in-jpeg-fmt,-j]"
99 << " [--depth-in-bin-fmt,-depth-bin]"
100 << " [--pcl-in-bin-fmt,-pcl-bin]"
101#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
102 << " [--pcl-in-npz-fmt,-pcl-npz]"
103#endif
104 << " [--stream-width,-sw <stream width>]"
105 << " [--stream-height,-sh <stream height>]"
106 << " [--stream-fps,-f <framerate>]"
107 << " [--display-colored-depth,-colored-depth]"
108 << " [--help,-h]"
109 << std::endl;
110
111 std::cout << "\nOPTIONS " << std::endl
112 << " --save, -s" << std::endl
113 << " Flag to enable data saving in output folder." << std::endl
114 << " When this flag is not used, we only display data." << std::endl
115 << std::endl
116 << " --output-folder,-o <custom output folder>" << std::endl
117 << " Custom output folder that will host saved data." << std::endl
118 << " By default, for each data acquisition, a new output folder is" << std::endl
119 << " created with the current date in the following format" << std::endl
120 << " YYYY_MM_DD_HH:MM:SS, for example 2025_11_07_15:44:34." << std::endl
121 << std::endl
122 << " --step-by-step,-C" << std::endl
123 << " Trigger one shot data saver after each user click." << std::endl
124 << std::endl
125 << " --pattern,-e <filename numbering pattern (e.g. %06d)>" << std::endl
126 << " Filename numbering pattern to use when saving data." << std::endl
127 << std::endl
128 << " --aligned,-a" << std::endl
129 << " Color and depth are aligned." << std::endl
130 << std::endl
131 << " --save-color,-c" << std::endl
132 << " Add color stream to saved data when --save option is used." << std::endl
133 << " By default, images are saved in png format. To save in jpeg format" << std::endl
134 << " use --img-in-jpeg-fmt flag." << std::endl
135 << std::endl
136 << " --save-depth,-d" << std::endl
137 << " Add depth stream to saved data when --save option is enabled." << std::endl
138 << " By default, depth images are saved in NumPy (.npz) format." << std::endl
139 << " To save in little-endian binary format use --depth-in-bin-fmt flag." << std::endl
140 << std::endl
141 << " --save-infrared" << std::endl
142 << " Add infrared stream to saved data when --save option is enabled." << std::endl
143 << " By default, images are saved in png format. To save in jpeg format" << std::endl
144 << " use --img-in-jpeg-fmt flag." << std::endl
145 << std::endl
146 << " --save-pcl,-p" << std::endl
147 << " Add point cloud stream to saved data when --save option is enabled." << std::endl
148#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
149 << " Since PCL library is available, by default the point cloud is saved in" << std::endl
150 << " Point Cloud Data file format (.pcd extension file)." << std::endl
151#endif
152#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
153 << " There is also the possibility to save the point cloud in NumPy" << std::endl
154 << " format (.npy extension file) enabling --pcl-in-npz-fmt option" << std::endl
155 << " or in little-endian binary format enabling --pcl-in-bin-fmt option." << std::endl
156#else
157 << " Point clould could be saved in little-endian binary format enabling" << std::endl
158 << " --pcl-in-bin-fmt option." << std::endl
159#endif
160 << std::endl
161 << " --img-in-jpeg-fmt,-j" << std::endl
162 << " Save image data using jpeg format (otherwise PNG is used)." << std::endl
163 << std::endl
164 << " --depth-in-bin-fmt,-depth-bin" << std::endl
165 << " When this flag is enabled, depth is saved in little-endian binary format." << std::endl
166 << " By default, depth is saved in NumPy (.npz) format." << std::endl
167 << std::endl
168 << " --pcl-in-bin-fmt,-pcl-bin" << std::endl
169 << " When this flag is enabled, point cloud is saved in little-endian binary format." << std::endl
170#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
171 << " There is also the possibility to save the point cloud in NumPy (npz) format" << std::endl
172 << " using --pcl-in-npz-fmt flag." << std::endl
173#endif
174#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
175 << " By default, point cloud is saved in pcd format using PCL 3rd-party library." << std::endl
176#endif
177 << std::endl
178#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
179 << " --pcl-in-npz-fmt,-pcl-npz" << std::endl
180 << " When this flag is enabled, point cloud is saved in NumPy (npz) format." << std::endl
181 << " There is also the possibility to save the point cloud in little-endian binary format" << std::endl
182 << " using --pcl-in-bin-fmt flag." << std::endl
183#endif
184#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
185 << " By default, point cloud is saved in pcd format using PCL 3rd-party library." << std::endl
186#endif
187 << std::endl
188 << " --stream-width,-sw <stream width>" << std::endl
189 << " Set camera image width resolution." << std::endl
190 << " Default: " << stream_width << std::endl
191 << std::endl
192 << " --stream-height,-sh <stream height>" << std::endl
193 << " Set camera image height resolution." << std::endl
194 << " Default: " << stream_height << std::endl
195 << std::endl
196 << " --stream-fps,-f <fps>" << std::endl
197 << " Set camera framerate in Hz." << std::endl
198 << " Default: " << stream_fps << std::endl
199 << std::endl
200 << " --display-colored-depth,-colored-depth" << std::endl
201 << " Display depth using a cumulative histogram." << std::endl
202 << " Warning: this operation is time consuming" << std::endl
203 << std::endl
204 << " --help,-h" << std::endl
205 << " Display this helper message." << std::endl
206 << std::endl;
207 std::cout << "EXAMPLE " << std::endl
208 << "- Save aligned color + depth + point cloud in data folder" << std::endl
209 << " " << argv[0] << " --save --save-color --save-depth --save-pcl --display-colored-depth --output-folder data --aligned" << std::endl
210 << "- Save color + infrared + depth + point cloud in bin format in data folder" << std::endl
211 << " " << argv[0] << " --save --save-color --save-depth --save-infrared --save-pcl --display-colored-depth --depth-in-bin-fmt --pcl-in-bin-fmt --output-folder data" << std::endl
212 << std::endl;
213
214 if (error) {
215 std::cout << "Error" << std::endl
216 << " "
217 << "Unsupported parameter " << argv[error] << std::endl;
218 }
219}
220
221bool getOptions(int argc, const char *argv[], bool &save, std::string &numbering_pattern, std::string &output_folder,
222 bool &use_aligned_stream, bool &save_color, bool &save_depth, bool &save_pcl,
223 bool &save_infrared, bool &step_by_step, int &stream_width, int &stream_height, int &stream_fps,
224 bool &depth_bin_fmt, bool &pcl_bin_fmt, bool &pcl_npz_fmt, bool &img_jpeg_fmt,
225 bool &display_colored_depth)
226{
227 for (int i = 1; i < argc; i++) {
228 if ((std::string(argv[i]) == "--save") || (std::string(argv[i]) == "-s")) {
229 save = true;
230 }
231 else if (((std::string(argv[i]) == "--pattern") || (std::string(argv[i]) == "-e")) && (i + 1 < argc)) {
232 numbering_pattern = std::string(argv[++i]);
233 }
234 else if ((std::string(argv[i]) == "--aligned") || (std::string(argv[i]) == "-a")) {
235 use_aligned_stream = true;
236 }
237 else if (((std::string(argv[i]) == "--output-folder") || (std::string(argv[i]) == "-o")) && (i + 1 < argc)) {
238 output_folder = std::string(argv[++i]);
239 }
240 else if ((std::string(argv[i]) == "--save-color") || (std::string(argv[i]) == "-c")) {
241 save_color = true;
242 }
243 else if ((std::string(argv[i]) == "--save-depth") || (std::string(argv[i]) == "-d")) {
244 save_depth = true;
245 }
246 else if ((std::string(argv[i]) == "--save-infrared") || (std::string(argv[i]) == "-i")) {
247 save_infrared = true;
248 }
249 else if ((std::string(argv[i]) == "--save-pcl") || (std::string(argv[i]) == "-p")) {
250 save_pcl = true;
251 }
252 else if ((std::string(argv[i]) == "--img-in-jpeg-fmt") || (std::string(argv[i]) == "-j")) {
253 img_jpeg_fmt = true;
254 }
255 else if ((std::string(argv[i]) == "--depth-in-bin-fmt") || (std::string(argv[i]) == "-depth-bin")) {
256 depth_bin_fmt = true;
257 }
258 else if ((std::string(argv[i]) == "--pcl-in-bin-fmt") || (std::string(argv[i]) == "-pcl-bin")) {
259 pcl_bin_fmt = true;
260 }
261#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
262 else if ((std::string(argv[i]) == "--pcl-in-npz-fmt") || (std::string(argv[i]) == "-pcl-npz")) {
263 pcl_npz_fmt = true;
264 }
265#endif
266 else if ((std::string(argv[i]) == "--step-by-step") || (std::string(argv[i]) == "-C")) {
267 step_by_step = true;
268 }
269 else if (((std::string(argv[i]) == "--stream-width") || (std::string(argv[i]) == "-sw")) && (i + 1 < argc)) {
270 stream_width = std::atoi(argv[++i]);
271 }
272 else if (((std::string(argv[i]) == "--stream-height") || (std::string(argv[i]) == "-sh")) && (i + 1 < argc)) {
273 stream_height = std::atoi(argv[++i]);
274 }
275 else if (((std::string(argv[i]) == "--stream-fps") || (std::string(argv[i]) == "-f")) && (i + 1 < argc)) {
276 stream_fps = std::atoi(argv[++i]);
277 }
278 else if ((std::string(argv[i]) == "--display-colored-depth") || (std::string(argv[i]) == "--colored-depth")) {
279 display_colored_depth = true;
280 }
281 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
282 usage(argv, 0, stream_width, stream_height, stream_fps);
283 return false;
284 }
285 else {
286 usage(argv, i, stream_width, stream_height, stream_fps);
287 return false;
288 }
289 }
290
291 // If unable to save point cloud in pcd format, and in NumPy format switch to bin format
292#if !(defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON))
293#if !(defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX))
294 pcl_bin_fmt = true;
295#else
296 if (!pcl_bin_fmt) {
297 pcl_npz_fmt = true;
298 }
299#endif
300#endif
301
302 if (pcl_npz_fmt && pcl_bin_fmt) {
303 std::cout << "\nError: Cannot save point clound in NumPy (npz) and in little-endian binary format (bin) simultaneously." << std::endl;
304 std::cout << "Command line options --pcl-in-bin-fmt and --pcl-in-npz-fmt are exclusive." << std::endl;
305 std::cout << "Use only one of these two options." << std::endl;
306 return false;
307 }
308
309 // If unable to save depth in NumPy format, force binary format
310#if !(defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX))
311 depth_bin_format = true;
312#endif
313
314#if !(defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX))
315 (void)pcl_npz_fmt;
316#endif
317
318 return true;
319}
320
321#ifndef DOXYGEN_SHOULD_SKIP_THIS
322
323// Code adapted from: https://stackoverflow.com/a/37146523
324class vpFrameQueue
325{
326public:
327 struct vpCancelled_t
328 { };
329
330 vpFrameQueue()
331 : m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(),
332 m_maxQueueSize(1024 * 8), m_mutex()
333 { }
334
335 void cancel()
336 {
337 std::lock_guard<std::mutex> lock(m_mutex);
338 m_cancelled = true;
339 m_cond.notify_all();
340 }
341
342 // Push data to save in the queue (FIFO)
343 void push(const std::unique_ptr<vpImage<vpRGBa>> &ptr_colorImg,
344 const std::unique_ptr<vpImage<uint16_t>> &ptr_depthImg,
345#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
346 const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
347#else
348 const std::unique_ptr<std::vector<vpColVector>> &ptr_pointCloud,
349#endif
350 const std::unique_ptr<vpImage<unsigned char>> &ptr_infraredImg)
351 {
352 std::lock_guard<std::mutex> lock(m_mutex);
353
354 if (ptr_colorImg) {
355 m_queueColor.push(*ptr_colorImg);
356 }
357 if (ptr_depthImg) {
358 m_queueDepth.push(*ptr_depthImg);
359 }
360#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
361 if (pointCloud) {
362 m_queuePointCloud.push(pointCloud);
363 }
364#else
365 if (ptr_pointCloud) {
366 m_queuePointCloud.push(*ptr_pointCloud);
367 }
368#endif
369 if (ptr_infraredImg) {
370 m_queueInfrared.push(*ptr_infraredImg);
371 }
372
373 // Pop extra data in the queue
374 while (m_queueColor.size() > m_maxQueueSize) {
375 m_queueColor.pop();
376 }
377
378 // Pop extra data in the queue
379 while (m_queueDepth.size() > m_maxQueueSize) {
380 m_queueDepth.pop();
381 }
382
383 // Pop extra data in the queue
384 while (m_queuePointCloud.size() > m_maxQueueSize) {
385 m_queuePointCloud.pop();
386 }
387
388 // Pop extra data in the queue
389 while (m_queueInfrared.size() > m_maxQueueSize) {
390 m_queueInfrared.pop();
391 }
392
393 m_cond.notify_one();
394 }
395
396 // Pop the image to save from the queue (FIFO)
397 void pop(std::unique_ptr<vpImage<vpRGBa>> &ptr_colorImg,
398 std::unique_ptr<vpImage<uint16_t>> &ptr_depthImg,
399#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
400 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
401#else
402 std::unique_ptr<std::vector<vpColVector>> &ptr_pointCloud,
403#endif
404 std::unique_ptr<vpImage<unsigned char>> &ptr_infraredImg)
405 {
406 std::unique_lock<std::mutex> lock(m_mutex);
407
408 // Since we push all 4 data at a time, there should be no situation where a queue size is different from the others
409 while (m_queueColor.empty() && m_queueDepth.empty() && m_queuePointCloud.empty() && m_queueInfrared.empty()) {
410 if (m_cancelled) {
411 throw vpCancelled_t();
412 }
413
414 m_cond.wait(lock);
415
416 if (m_cancelled) {
417 throw vpCancelled_t();
418 }
419 }
420
421 if (!m_queueColor.empty()) {
422 ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(m_queueColor.front());
423 m_queueColor.pop();
424 }
425 if (!m_queueDepth.empty()) {
426 ptr_depthImg = std::make_unique<vpImage<uint16_t>>(m_queueDepth.front());
427 m_queueDepth.pop();
428 }
429#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
430 if (!m_queuePointCloud.empty()) {
431 pointCloud = m_queuePointCloud.front();
432 m_queuePointCloud.pop();
433 }
434#else
435 if (!m_queuePointCloud.empty()) {
436 ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(m_queuePointCloud.front());
437 m_queuePointCloud.pop();
438 }
439#endif
440 if (!m_queueInfrared.empty()) {
441 ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(m_queueInfrared.front());
442 m_queueInfrared.pop();
443 }
444 }
445
446 bool empty()
447 {
448 std::lock_guard<std::mutex> lock(m_mutex);
449 return m_queueColor.empty() && m_queueDepth.empty() && m_queuePointCloud.empty() && m_queueInfrared.empty();
450 }
451
452 void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
453
454private:
455 bool m_cancelled;
456 std::condition_variable m_cond;
457 std::queue<vpImage<vpRGBa>> m_queueColor;
458 std::queue<vpImage<uint16_t>> m_queueDepth;
459#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
460 std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
461#else
462 std::queue<std::vector<vpColVector>> m_queuePointCloud;
463#endif
464 std::queue<vpImage<unsigned char>> m_queueInfrared;
465 size_t m_maxQueueSize;
466 std::mutex m_mutex;
467};
468
469class vpStorageWorker
470{
471public:
472 vpStorageWorker(vpFrameQueue &queue, const std::string &output_pattern, const std::string &directory, bool save_color,
473 bool save_depth, bool save_pcl, bool save_infrared, bool depth_bin_fmt,
474 bool pcl_bin_fmt, bool pcl_npz_fmt, bool img_jpeg_fmt,
475 int
476#ifndef VISP_HAVE_PCL
477 width
478#endif
479 ,
480 int
481#ifndef VISP_HAVE_PCL
482 height
483#endif
484 )
485 : m_queue(queue), m_output_pattern(output_pattern), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
486 m_save_pcl(save_pcl), m_save_infrared(save_infrared), m_depth_bin_fmt(depth_bin_fmt),
487 m_pcl_bin_fmt(pcl_bin_fmt), m_pcl_npz_fmt(pcl_npz_fmt),
488 m_img_jpeg_fmt(img_jpeg_fmt)
489#ifndef VISP_HAVE_PCL
490 ,
491 m_size_height(height), m_size_width(width)
492#endif
493 { }
494
495 // Thread main loop
496 void run()
497 {
498 try {
499 std::unique_ptr<vpImage<vpRGBa>> ptr_colorImg;
500 std::unique_ptr<vpImage<uint16_t>> ptr_depthImg;
501#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
502 pcl::PointCloud<pcl::PointXYZ>::Ptr ptr_pointCloud;
503#else
504 std::unique_ptr<std::vector<vpColVector>> ptr_pointCloud;
505#endif
506 std::unique_ptr<vpImage<unsigned char>> ptr_infraredImg;
507
508 std::vector<float> vec_pcl;
509
510 std::string image_filename_ext = m_img_jpeg_fmt ? ".jpg" : ".png";
511 for (;;) {
512 m_queue.pop(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
513
514 if (!m_directory.empty()) {
515 std::string current_time = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
516
517 if (m_save_color && ptr_colorImg) {
518 std::string filename_color = vpIoTools::formatString(m_directory + "/color_image_" + m_output_pattern + image_filename_ext, m_cpt);
519 vpImageIo::write(*ptr_colorImg, filename_color);
520 }
521
522 if (m_save_depth && ptr_depthImg) {
523 if (m_depth_bin_fmt) { // Use binary format
524
525 std::string filename_depth = vpIoTools::formatString(m_directory + "/depth_image_" + m_output_pattern + ".bin", m_cpt);
526
527 std::ofstream file_depth(filename_depth.c_str(), std::ios::out | std::ios::binary);
528 if (file_depth.is_open()) {
529 unsigned int height = ptr_depthImg->getHeight(), width = ptr_depthImg->getWidth();
530 vpIoTools::writeBinaryValueLE(file_depth, height);
531 vpIoTools::writeBinaryValueLE(file_depth, width);
532
533 uint16_t value;
534 for (unsigned int i = 0; i < height; i++) {
535 for (unsigned int j = 0; j < width; j++) {
536 value = (*ptr_depthImg)[i][j];
537 vpIoTools::writeBinaryValueLE(file_depth, value);
538 }
539 }
540 }
541 }
542#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
543 else {
544 std::string filename_depth = vpIoTools::formatString(m_directory + "/depth_image_" + m_output_pattern + ".npz", m_cpt);
545
546 // Write Npz headers
547 std::vector<char> vec_filename(filename_depth.begin(), filename_depth.end());
548 // Null-terminated character is handled at reading
549 // For null-terminated character handling, see:
550 // https://stackoverflow.com/a/8247804
551 // https://stackoverflow.com/a/45491652
552 visp::cnpy::npz_save(filename_depth, "filename", &vec_filename[0], { vec_filename.size() }, "w");
553
554 std::vector<char> vec_current_time(current_time.begin(), current_time.end());
555 visp::cnpy::npz_save(filename_depth, "timestamp", &vec_current_time, { vec_current_time.size() }, "a");
556
557 unsigned int height = ptr_depthImg->getHeight();
558 unsigned int width = ptr_depthImg->getWidth();
559 unsigned int channel = 1;
560 visp::cnpy::npz_save(filename_depth, "height", &height, { 1 }, "a");
561 visp::cnpy::npz_save(filename_depth, "width", &width, { 1 }, "a");
562 visp::cnpy::npz_save(filename_depth, "channel", &channel, { 1 }, "a");
563
564 // Write data
565 std::vector<uint16_t> I_depth_raw_vec(ptr_depthImg->bitmap, ptr_depthImg->bitmap + ptr_depthImg->getSize());
566 visp::cnpy::npz_save(filename_depth, "data", I_depth_raw_vec.data(), { height, width }, "a");
567 }
568#else
569 else {
570 throw(vpIoException(vpIoException::ioError, "Cannot manage non-binary files when npz I/O functions are disabled."));
571 }
572#endif
573 }
574
575 if (m_save_pcl && ptr_pointCloud) {
576
577#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
578 uint32_t width = ptr_pointCloud->width;
579 uint32_t height = ptr_pointCloud->height;
580#else
581 uint32_t width = m_size_width;
582 uint32_t height = m_size_height;
583#endif
584
585 if (m_pcl_npz_fmt) {
586#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
587 std::string filename_pcl = vpIoTools::formatString(m_directory + "/point_cloud_" + m_output_pattern + ".npz", m_cpt);
588
589 // Write Npz headers
590 std::vector<char> vec_filename(filename_pcl.begin(), filename_pcl.end());
591 // Null-terminated character is handled at reading
592 // For null-terminated character handling, see:
593 // https://stackoverflow.com/a/8247804
594 // https://stackoverflow.com/a/45491652
595 visp::cnpy::npz_save(filename_pcl, "filename", &vec_filename[0], { vec_filename.size() }, "w");
596
597 std::vector<char> vec_current_time(current_time.begin(), current_time.end());
598 visp::cnpy::npz_save(filename_pcl, "timestamp", &vec_current_time, { vec_current_time.size() }, "a");
599
600 const uint32_t channels = 3;
601 visp::cnpy::npz_save(filename_pcl, "height", &height, { 1 }, "a");
602 visp::cnpy::npz_save(filename_pcl, "width", &width, { 1 }, "a");
603 visp::cnpy::npz_save(filename_pcl, "channel", &channels, { 1 }, "a");
604
605 vec_pcl.resize(height * width * channels);
606#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
607 // can probably be optimized by assuming channel=1 and use data of type XYZ
608 // but this should probably not work with the Python script for display
609 for (uint32_t i = 0; i < height; i++) {
610 for (uint32_t j = 0; j < width; j++) {
611 pcl::PointXYZ pt = (*ptr_pointCloud)(j, i);
612 vec_pcl[channels * (i*width + j) + 0] = pt.x;
613 vec_pcl[channels * (i*width + j) + 1] = pt.y;
614 vec_pcl[channels * (i*width + j) + 2] = pt.z;
615 }
616 }
617#else
618 for (uint32_t i = 0; i < height; i++) {
619 for (uint32_t j = 0; j < width; j++) {
620 vec_pcl[channels * (i*width + j) + 0] = static_cast<float>((*ptr_pointCloud)[i*width + j][0]);
621 vec_pcl[channels * (i*width + j) + 1] = static_cast<float>((*ptr_pointCloud)[i*width + j][1]);
622 vec_pcl[channels * (i*width + j) + 2] = static_cast<float>((*ptr_pointCloud)[i*width + j][2]);
623 }
624 }
625#endif
626 // Write data
627 visp::cnpy::npz_save(filename_pcl, "data", vec_pcl.data(), { height, width, channels }, "a");
628#else
629 throw(vpIoException(vpIoException::fatalError, "Cannot save in npz format when npz I/O functions are disabled."));
630#endif
631 }
632 else if (m_pcl_bin_fmt) {
633 std::string filename_pcl = vpIoTools::formatString(m_directory + "/point_cloud_" + m_output_pattern + ".bin", m_cpt);
634
635 std::ofstream file_pointcloud(filename_pcl.c_str(), std::ios::out | std::ios::binary);
636
637 if (file_pointcloud.is_open()) {
638#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
639 // true if ptr_pointCloud does not contain NaN or Inf, not handled currently
640 char is_dense = ptr_pointCloud->is_dense;
641
642 vpIoTools::writeBinaryValueLE(file_pointcloud, height);
643 vpIoTools::writeBinaryValueLE(file_pointcloud, width);
644 file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
645
646 for (uint32_t i = 0; i < height; i++) {
647 for (uint32_t j = 0; j < width; j++) {
648 pcl::PointXYZ pt = (*ptr_pointCloud)(j, i);
649
650 vpIoTools::writeBinaryValueLE(file_pointcloud, pt.x);
651 vpIoTools::writeBinaryValueLE(file_pointcloud, pt.y);
652 vpIoTools::writeBinaryValueLE(file_pointcloud, pt.z);
653 }
654 }
655#else
656 // to be consistent with PCL version
657 const char is_dense = 1;
658
659 vpIoTools::writeBinaryValueLE(file_pointcloud, height);
660 vpIoTools::writeBinaryValueLE(file_pointcloud, width);
661 file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
662
663 for (uint32_t i = 0; i < height; i++) {
664 for (uint32_t j = 0; j < width; j++) {
665 float x = static_cast<float>((*ptr_pointCloud)[i * width + j][0]);
666 float y = static_cast<float>((*ptr_pointCloud)[i * width + j][1]);
667 float z = static_cast<float>((*ptr_pointCloud)[i * width + j][2]);
668
669 vpIoTools::writeBinaryValueLE(file_pointcloud, x);
670 vpIoTools::writeBinaryValueLE(file_pointcloud, y);
671 vpIoTools::writeBinaryValueLE(file_pointcloud, z);
672 }
673 }
674#endif
675 }
676 }
677 else {
678#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
679 std::string filename_pcl = vpIoTools::formatString(m_directory + "/point_cloud_" + m_output_pattern + ".pcd", m_cpt);
680 pcl::io::savePCDFileBinary(filename_pcl, *ptr_pointCloud);
681#else
682 std::cout << "Error: unable to save point cloud in pcd format. PCL io module not available." << std::endl;
683#endif
684 }
685 }
686
687 if (m_save_infrared && ptr_infraredImg) {
688 std::string filename_infrared = vpIoTools::formatString(m_directory + "/infrared_image_" + m_output_pattern + image_filename_ext, m_cpt);
689
690 vpImageIo::write(*ptr_infraredImg, filename_infrared);
691 }
692
693 m_cpt++;
694 }
695 }
696 }
697 catch (const vpFrameQueue::vpCancelled_t &) {
698 std::cout << "Receive cancel vpFrameQueue." << std::endl;
699 }
700 }
701
702private:
703 vpFrameQueue &m_queue;
704 std::string m_output_pattern;
705 std::string m_directory;
706 unsigned int m_cpt;
707 bool m_save_color;
708 bool m_save_depth;
709 bool m_save_pcl;
710 bool m_save_infrared;
711 bool m_depth_bin_fmt;
712 bool m_pcl_bin_fmt;
713 bool m_pcl_npz_fmt;
714 bool m_img_jpeg_fmt;
715#ifndef VISP_HAVE_PCL
716 int m_size_height;
717 int m_size_width;
718#endif
719};
720} // Namespace
721
722#endif // DOXYGEN_SHOULD_SKIP_THIS
723
724int main(int argc, const char *argv[])
725{
726 bool save = false;
727 std::string output_pattern = "%04d";
728 std::string output_folder = vpTime::getDateTime("%Y_%m_%d_%H.%M.%S");
729 std::string output_folder_custom = "";
730 bool use_aligned_stream = false;
731 bool save_color = false;
732 bool save_depth = false;
733 bool save_pcl = false;
734 bool save_infrared = false;
735 bool step_by_step = false;
736 int stream_width = 640;
737 int stream_height = 480;
738 int stream_fps = 30;
739 bool depth_bin_fmt = false; // By default depth in npz format, when true save depth in bin format
740 bool pcl_bin_fmt = false; // By default, pcl in pcd format, when true save to bin format
741 bool pcl_npz_fmt = false; // By default, pcl in pcd format, when true save to npz format
742 bool img_jpeg_fmt = false;
743 bool display_colored_depth = false;
744
745 // Read the command line options
746 if (!getOptions(argc, argv, save, output_pattern, output_folder_custom, use_aligned_stream, save_color, save_depth,
747 save_pcl, save_infrared, step_by_step, stream_width, stream_height, stream_fps,
748 depth_bin_fmt, pcl_bin_fmt, pcl_npz_fmt, img_jpeg_fmt, display_colored_depth)) {
749 return EXIT_FAILURE;
750 }
751
752 if (!output_folder_custom.empty()) {
753 output_folder = output_folder_custom + "/" + output_folder;
754 }
755
756
757 std::cout << "Options summary" << std::endl;
758 std::cout << " Realsense device " << std::endl;
759 std::cout << " Stream width : " << stream_width << std::endl;
760 std::cout << " Stream height : " << stream_height << std::endl;
761 std::cout << " Stream framerate : " << stream_fps << std::endl;
762 std::cout << " Considered data " << std::endl;
763 std::cout << " Color stream : " << (save_color ? "yes" : "no") << std::endl;
764 std::cout << " Depth stream : " << (save_depth ? "yes" : "no") << std::endl;
765 std::cout << " Infrared stream : " << (save_infrared ? "yes" : "no") << std::endl;
766 std::cout << " Point cloud : " << (save_pcl ? "yes" : "no") << std::endl;
767 std::cout << " Color and depth aligned: " << (use_aligned_stream ? "yes" : "no") << std::endl;
768 std::cout << " Data visualization " << std::endl;
769 std::cout << " Colored depth : " << (display_colored_depth ? "yes" : "no") << std::endl;
770 std::cout << " Save dataset " << (save ? "yes" : "no") << std::endl;
771 if (save) {
772 std::cout << " Output folder : " << output_folder << std::endl;
773 std::cout << " File numbering pattern : " << output_pattern << std::endl;
774 std::cout << " Color images format : " << (save_color ? (img_jpeg_fmt ? "jpeg" : "png") : "N/A") << std::endl;
775 std::cout << " Depth images format : " << (save_depth ? (depth_bin_fmt ? "bin" : "npz") : "N/A") << std::endl;
776 std::cout << " Infrared images format : " << (save_infrared ? (img_jpeg_fmt ? "jpeg" : "png") : "N/A") << std::endl;
777 std::cout << " Point cloud format : " << (save_pcl ? (pcl_npz_fmt ? "npz" : (pcl_bin_fmt ? "bin" : "pcd")) : "N/A") << std::endl;
778 std::cout << " Save after each click : " << (step_by_step ? "yes" : "no") << std::endl << std::endl;
779 }
780
781 vpRealSense2 realsense;
782
783 rs2::config config;
784 config.enable_stream(RS2_STREAM_COLOR, stream_width, stream_height, RS2_FORMAT_RGBA8, stream_fps);
785 config.enable_stream(RS2_STREAM_DEPTH, stream_width, stream_height, RS2_FORMAT_Z16, stream_fps);
786 config.enable_stream(RS2_STREAM_INFRARED, stream_width, stream_height, RS2_FORMAT_Y8, stream_fps);
787 realsense.open(config);
788
789 vpImage<vpRGBa> I_color(stream_height, stream_width);
790 vpImage<unsigned char> I_depth_gray(stream_height, stream_width);
791 vpImage<vpRGBa> I_depth_color(stream_height, stream_width);
792 vpImage<uint16_t> I_depth_raw(stream_height, stream_width);
793 vpImage<unsigned char> I_infrared(stream_height, stream_width);
794
795#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
796 std::shared_ptr<vpDisplay> d1 = vpDisplayFactory::createDisplay();
797 std::shared_ptr<vpDisplay> d2 = vpDisplayFactory::createDisplay();
798 std::shared_ptr<vpDisplay> d3 = vpDisplayFactory::createDisplay();
799#else
800 vpDisplay *d1 = vpDisplayFactory::allocateDisplay();
801 vpDisplay *d2 = vpDisplayFactory::allocateDisplay();
802 vpDisplay *d3 = vpDisplayFactory::allocateDisplay();
803#endif
804 d1->init(I_color, 0, 0, "RealSense color stream");
805 if (display_colored_depth) {
806 d2->init(I_depth_color, I_color.getWidth() + 80, 0, "RealSense depth stream");
807 }
808 else {
809 d2->init(I_depth_gray, I_color.getWidth() + 80, 0, "RealSense depth stream");
810 }
811
812 rs2::align align_to(RS2_STREAM_COLOR);
813 if (use_aligned_stream && save_infrared) {
814 std::cerr << "Cannot use aligned streams with infrared acquisition currently."
815 << "\nInfrared stream acquisition is disabled!"
816 << std::endl;
817 save_infrared = false;
818 }
819 if (save_infrared) {
820 d3->init(I_infrared, I_color.getWidth() + 80, I_color.getHeight() + 70, "RealSense infrared stream");
821 }
822
823 // If PCL is available, always use PCL datatype even if we will save in NPZ or BIN file format
824#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
825 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
826#else
827 std::vector<vpColVector> pointCloud;
828#endif
829
830 bool quit = false;
831 while (!quit) {
832 double start = vpTime::measureTimeMs();
833 if (use_aligned_stream) {
834#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
835 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud, nullptr,
836 &align_to);
837#else
838 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud, nullptr,
839 &align_to);
840#endif
841 }
842 else {
843#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
844 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
845 (unsigned char *)I_infrared.bitmap, nullptr);
846#else
847 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
848 (unsigned char *)I_infrared.bitmap);
849#endif
850 }
851
852 vpDisplay::display(I_color);
853
854 if (display_colored_depth) {
855 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth_color);
856 vpDisplay::display(I_depth_color);
857 }
858 else {
859 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth_gray);
860 vpDisplay::display(I_depth_gray);
861 }
862 if (save_infrared) {
863 vpDisplay::display(I_infrared);
864 }
865
866 double delta_time = vpTime::measureTimeMs() - start;
867 std::ostringstream oss_time;
868 oss_time << delta_time << " ms ; fps=" << 1000/delta_time;
869 vpDisplay::displayText(I_color, 40, 20, oss_time.str(), vpColor::red);
870 if (save) {
871 vpDisplay::displayText(I_color, 20, 20, "Click to start recording dataset", vpColor::red);
872 }
873 else {
874 vpDisplay::displayText(I_color, 20, 20, "Click to stop recording", vpColor::red);
875 }
876
877 vpDisplay::flush(I_color);
878 if (display_colored_depth) {
879 vpDisplay::flush(I_depth_color);
880 }
881 else {
882 vpDisplay::flush(I_depth_gray);
883 }
884 if (save_infrared) {
885 vpDisplay::flush(I_infrared);
886 }
887
888 if (vpDisplay::getClick(I_color, false)) {
889 quit = true;
890 }
891 }
892
893 if (save) {
894 // Create output directory
895 vpIoTools::makeDirectory(output_folder);
896
897 // Save intrinsics
898 vpCameraParameters cam_color = realsense.getCameraParameters(RS2_STREAM_COLOR);
899 vpXmlParserCamera xml_camera;
900 xml_camera.save(cam_color, output_folder + "/camera.xml", "color_camera", stream_width, stream_height);
901
902 if (use_aligned_stream) {
903 xml_camera.save(cam_color, output_folder + "/camera.xml", "depth_camera", stream_width, stream_height);
904 }
905 else {
906 vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
907 xml_camera.save(cam_depth, output_folder + "/camera.xml", "depth_camera", stream_width, stream_height);
908 }
909
910 vpCameraParameters cam_infrared = realsense.getCameraParameters(RS2_STREAM_INFRARED);
911 xml_camera.save(cam_infrared, output_folder + "/camera.xml", "infrared_camera", stream_width, stream_height);
912 if (save_depth) {
913 vpHomogeneousMatrix depth_M_color;
914 if (!use_aligned_stream) {
915 depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
916 }
917 std::ofstream file(std::string(output_folder + "/depth_M_color.txt"));
918 depth_M_color.save(file);
919 file.close();
920 }
921 if (save_infrared) {
922 vpHomogeneousMatrix infrared_M_color;
923 infrared_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED);
924 std::ofstream file(std::string(output_folder + "/infrared_M_color.txt"));
925 infrared_M_color.save(file);
926 file.close();
927 }
928
929 vpFrameQueue save_queue;
930 vpStorageWorker storage(std::ref(save_queue), output_pattern, std::cref(output_folder), save_color, save_depth,
931 save_pcl, save_infrared, depth_bin_fmt, pcl_bin_fmt, pcl_npz_fmt, img_jpeg_fmt, stream_width, stream_height);
932 std::thread storage_thread(&vpStorageWorker::run, &storage);
933
934 int nb_saves = 0;
935 quit = false;
936
937 std::unique_ptr<vpImage<vpRGBa>> ptr_colorImg;
938 std::unique_ptr<vpImage<uint16_t>> ptr_depthImg;
939 std::unique_ptr<std::vector<vpColVector>> ptr_pointCloud;
940 std::unique_ptr<vpImage<unsigned char>> ptr_infraredImg;
941
942 std::vector<double> vec_delta_time;
943 while (!quit) {
944 double start = vpTime::measureTimeMs();
945 if (use_aligned_stream) {
946#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
947 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud, nullptr,
948 &align_to);
949#else
950 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud, nullptr,
951 &align_to);
952#endif
953 }
954 else {
955#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
956 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
957 (unsigned char *)I_infrared.bitmap, nullptr);
958#else
959 realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
960 (unsigned char *)I_infrared.bitmap);
961#endif
962 }
963
964 if (display_colored_depth) {
965 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth_color);
966 }
967 else {
968 // Seems like sometimes using createDepthHistogram() takes lots of time?
969 // so we simply perform bit shift from uint16_t to uint8_t
970 for (unsigned int i = 0; i < I_depth_raw.getRows(); i++) {
971 for (unsigned int j = 0; j < I_depth_raw.getCols(); j++) {
972 I_depth_gray[i][j] = I_depth_raw[i][j] >> 8;
973 }
974 }
975 }
976
977 vpDisplay::display(I_color);
978 if (display_colored_depth) {
979 vpDisplay::display(I_depth_color);
980 }
981 else {
982 vpDisplay::display(I_depth_gray);
983 }
984 vpDisplay::display(I_infrared);
985
986 if (!step_by_step) {
987 vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
988 }
989 else {
990 std::stringstream ss;
991 ss << "Images saved: " << nb_saves;
992 vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
993 }
994
995 if (save && !step_by_step) {
996 if (save_color) {
997 ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(I_color);
998 }
999 if (save_depth) {
1000 ptr_depthImg = std::make_unique<vpImage<uint16_t>>(I_depth_raw);
1001 }
1002 if (save_infrared) {
1003 ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(I_infrared);
1004 }
1005
1006#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
1007 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy;
1008 if (save_pcl) {
1009 pointCloud_copy = pointCloud->makeShared();
1010 }
1011 save_queue.push(ptr_colorImg, ptr_depthImg, pointCloud_copy, ptr_infraredImg);
1012#else
1013 if (save_pcl) {
1014 ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(pointCloud);
1015 }
1016 save_queue.push(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
1017#endif
1018 }
1019
1020 double delta_time = vpTime::measureTimeMs() - start;
1021 vec_delta_time.push_back(delta_time);
1022 std::ostringstream oss_time;
1023 oss_time << delta_time << " ms ; fps=" << 1000/delta_time;
1024 vpDisplay::displayText(I_color, 40, 20, oss_time.str(), vpColor::red);
1025
1026 vpDisplay::flush(I_color);
1027 if (display_colored_depth) {
1028 vpDisplay::flush(I_depth_color);
1029 }
1030 else {
1031 vpDisplay::flush(I_depth_gray);
1032 }
1033 vpDisplay::flush(I_infrared);
1034
1035 vpMouseButton::vpMouseButtonType button;
1036 if (vpDisplay::getClick(I_color, button, false)) {
1037 if (!step_by_step) {
1038 save = false;
1039 quit = true;
1040 save_queue.cancel();
1041 }
1042 else {
1043 switch (button) {
1044 case vpMouseButton::button1:
1045 if (save) {
1046 nb_saves++;
1047
1048 if (save_color) {
1049 ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(I_color);
1050 }
1051 if (save_depth) {
1052 ptr_depthImg = std::make_unique<vpImage<uint16_t>>(I_depth_raw);
1053 }
1054 if (save_infrared) {
1055 ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(I_infrared);
1056 }
1057
1058#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
1059 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy;
1060 if (save_pcl) {
1061 pointCloud_copy = pointCloud->makeShared();
1062 }
1063 save_queue.push(ptr_colorImg, ptr_depthImg, pointCloud_copy, ptr_infraredImg);
1064#else
1065 if (save_pcl) {
1066 ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(pointCloud);
1067 }
1068 save_queue.push(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
1069#endif
1070 }
1071 break;
1072
1073 case vpMouseButton::button2:
1074 case vpMouseButton::button3:
1075 default:
1076 save = false;
1077 quit = true;
1078 save_queue.cancel();
1079 break;
1080 }
1081 }
1082 }
1083 }
1084
1085 std::cout << "\nSaving dataset in " << output_folder << " folder in progress...\n" << std::endl;
1086 double mean_vec_delta_time = vpMath::getMean(vec_delta_time);
1087 double median_vec_delta_time = vpMath::getMedian(vec_delta_time);
1088 double std_vec_delta_time = vpMath::getStdev(vec_delta_time);
1089 std::cout << "Acquisition time, mean=" << mean_vec_delta_time << " ms ; median="
1090 << median_vec_delta_time << " ms ; std=" << std_vec_delta_time << " ms" << std::endl;
1091 std::cout << "FPS, mean=" << 1000/mean_vec_delta_time << " fps ; median="
1092 << 1000/median_vec_delta_time << " fps" << std::endl;
1093
1094 storage_thread.join();
1095
1096 std::cout << "\nDataset was successfully saved in " << output_folder << " folder\n" << std::endl;
1097 }
1098
1099#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
1100 if (d1 != nullptr) {
1101 delete d1;
1102 }
1103 if (d2 != nullptr) {
1104 delete d2;
1105 }
1106 if (d3 != nullptr) {
1107 delete d3;
1108 }
1109#endif
1110
1111 return EXIT_SUCCESS;
1112}
1113#else
1114int main()
1115{
1116 std::cerr << "Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl;
1117
1118#if !defined(VISP_HAVE_PUGIXML)
1119 std::cout << "pugixml built-in 3rdparty is requested." << std::endl;
1120#elif !((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
1121 std::cout << "At least c++14 standard is requested." << std::endl;
1122#endif
1123 return EXIT_SUCCESS;
1124}
1125#endif
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition vpCameraParameters.h:311
vpColor::red
static const vpColor red
Definition vpColor.h:198
vpDisplay
Class that defines generic functionalities for display.
Definition vpDisplay.h:171
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition vpDisplay_uchar.cpp:894
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition vpDisplay_uchar.cpp:869
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition vpDisplay_uchar.cpp:845
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition vpDisplay_uchar.cpp:788
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:1078
vpImageConvert::createDepthHistogram
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
Definition vpImageConvert.cpp:257
vpImageIo::write
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition vpImageIo.cpp:336
vpImage
Definition of the vpImage class member functions.
Definition vpImage.h:131
vpIoTools::formatString
static std::string formatString(const std::string &name, unsigned int val)
Definition vpIoTools.cpp:1436
vpIoTools::makeDirectory
static void makeDirectory(const std::string &dirname)
Definition vpIoTools.cpp:561
vpIoTools::writeBinaryValueLE
static void writeBinaryValueLE(std::ofstream &file, const int16_t short_value)
Definition vpIoTools_config_file.cpp:547
vpIoTools::getName
static std::string getName(const std::string &pathname)
Definition vpIoTools.cpp:1230
vpMath::getMedian
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:343
vpMath::getStdev
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:374
vpMath::getMean
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:323
vpRealSense2::getCameraParameters
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
Definition vpRealSense2.cpp:679
vpRealSense2::acquire
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
Definition vpRealSense2.cpp:88
vpRealSense2::open
bool open(const rs2::config &cfg=rs2::config())
Definition vpRealSense2.cpp:1362
vpRealSense2::getTransformation
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
Definition vpRealSense2.cpp:1151
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
gen_java.config
config
Definition gen_java.py:18
visp::cnpy::npz_save
VISP_EXPORT void npz_save(const std::string &zipname, std::string fname, const std::vector< std::string > &data_vec, const std::vector< size_t > &shape, const std::string &mode="w")
Definition vpIoTools_npy.cpp:758
vpDisplayFactory::createDisplay
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
Definition vpDisplayFactory.h:142
vpDisplayFactory::allocateDisplay
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
Definition vpDisplayFactory.h:68
vpTime::measureTimeMs
VISP_EXPORT double measureTimeMs()
vpTime::getDateTime
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")