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