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