Visual Servoing Platform  version 3.0.1
testRealSense.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
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 http://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  * Description:
31  * Test Intel RealSense acquisition.
32  *
33  *****************************************************************************/
34 
40 #include <iostream>
41 
42 #include <visp3/sensor/vpRealSense.h>
43 #include <visp3/gui/vpDisplayX.h>
44 #include <visp3/gui/vpDisplayGDI.h>
45 #include <visp3/io/vpImageIo.h>
46 #include <visp3/core/vpImageConvert.h>
47 #include <visp3/core/vpMutex.h>
48 #include <visp3/core/vpThread.h>
49 
50 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
51 # include <librealsense/rs.h>
52 
53 // Using a thread to display the pointcloud with PCL produces a segfault on OSX
54 #if( ! defined(__APPLE__) && ! defined(__MACH__) ) // Not OSX
55 # if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
56 # define USE_THREAD
57 # endif
58 #endif
59 
60 #ifdef VISP_HAVE_PCL
61 # include <pcl/visualization/cloud_viewer.h>
62 # include <pcl/visualization/pcl_visualizer.h>
63 #endif
64 
65 #ifdef VISP_HAVE_PCL
66 #ifdef USE_THREAD
67 // Shared vars
68 typedef enum {
69  capture_waiting,
70  capture_started,
71  capture_stopped
72 } t_CaptureState;
73 t_CaptureState s_capture_state = capture_waiting;
74 vpMutex s_mutex_capture;
75 
76 
77 vpThread::Return displayPointcloudFunction(vpThread::Args args)
78 {
79  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *) args);
80 
81  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
82  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
83  viewer->setBackgroundColor (0, 0, 0);
84  viewer->addCoordinateSystem (0.1);
85  viewer->initCameraParameters ();
86  viewer->setPosition(2*640+80, 480+80);
87  viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
88  viewer->setSize(640, 480);
89 
90  t_CaptureState capture_state_;
91 
92  do {
93  s_mutex_capture.lock();
94  capture_state_ = s_capture_state;
95  s_mutex_capture.unlock();
96 
97  if (capture_state_ == capture_started) {
98  static bool update = false;
99  if (! update) {
100  viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
101  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
102  update = true;
103  }
104  else {
105  viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
106  }
107 
108  viewer->spinOnce (10);
109  }
110  } while(capture_state_ != capture_stopped);
111 
112  std::cout << "End of point cloud display thread" << std::endl;
113  return 0;
114 }
115 #endif
116 #endif
117 
118 #endif
119 
120 
121 int main(
122 #if defined(VISP_HAVE_PCL)
123  int argc, char *argv[]
124 #endif
125  )
126 {
127 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && ( defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) )
128  try {
129  vpRealSense rs;
130 
131  rs.setEnableStream(rs::stream::color, false);
132  rs.open();
133  if ( rs_get_device_name((const rs_device *) rs.getHandler(), nullptr) != std::string("Intel RealSense SR300") ) {
134  std::cout << "This test file is used to test the Intel RealSense SR300 only." << std::endl;
135  return EXIT_SUCCESS;
136  }
137  rs.close();
138 
139 
140  rs.setEnableStream(rs::stream::color, false);
141  rs.setEnableStream(rs::stream::depth, true);
142  rs.setEnableStream(rs::stream::infrared, false);
143 
144  rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 240, rs::format::z16, 110));
145 
146  rs.open();
147  std::cout << "API version: " << rs_get_api_version(nullptr) << std::endl;
148  std::cout << "Firmware: " << rs_get_device_firmware_version((const rs_device *) rs.getHandler(), nullptr) << std::endl;
149  std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
150 
151  vpImage<uint16_t> depth((unsigned int) rs.getIntrinsics(rs::stream::depth).height, (unsigned int) rs.getIntrinsics(rs::stream::depth).width);
152  vpImage<vpRGBa> I_display_depth(depth.getHeight(), depth.getWidth());
153 
154  std::vector<double> time_vector;
155 
156 #if defined(VISP_HAVE_X11)
157  vpDisplayX dd(I_display_depth, 0, 0, "Depth image");
158 #elif defined(VISP_HAVE_GDI)
159  vpDisplayGDI dd(I_display_depth, 0, 0, "Depth image");
160 #endif
161 
162  //Test depth stream during 10 s
163  double t_begin = vpTime::measureTimeMs();
164  while (true) {
165  double t = vpTime::measureTimeMs();
166  rs.acquire( NULL, (unsigned char *) depth.bitmap, NULL, NULL );
167  vpImageConvert::createDepthHistogram(depth, I_display_depth);
168 
169  vpDisplay::display(I_display_depth);
170  vpDisplay::flush(I_display_depth);
171 
172  if (vpDisplay::getClick(I_display_depth, false)) {
173  break;
174  }
175 
176  time_vector.push_back(vpTime::measureTimeMs() - t);
177  if (vpTime::measureTimeMs() - t_begin >= 10000) {
178  break;
179  }
180  }
181  std::cout << "SR300_DEPTH_Z16_640x240_110FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
182  << vpMath::getMedian(time_vector) << " ms" << std::endl;
183 
184  dd.close(I_display_depth);
185 
186 
187  rs.setEnableStream(rs::stream::color, true);
188  rs.setEnableStream(rs::stream::depth, false);
189  rs.setEnableStream(rs::stream::infrared, false);
190  rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30));
191  rs.open();
192 
193  vpImage<vpRGBa> I_color((unsigned int) rs.getIntrinsics(rs::stream::color).height, (unsigned int) rs.getIntrinsics(rs::stream::color).width);
194 
195 #if defined(VISP_HAVE_X11)
196  vpDisplayX dc(I_color, 0, 0, "Color image");
197 #elif defined(VISP_HAVE_GDI)
198  vpDisplayGDI dc(I_color, 0, 0, "Color image");
199 #endif
200 
201  time_vector.clear();
202  //Test color stream during 10 s
203  t_begin = vpTime::measureTimeMs();
204  while (true) {
205  double t = vpTime::measureTimeMs();
206  rs.acquire( (unsigned char *) I_color.bitmap, NULL, NULL, NULL );
207 
208  vpDisplay::display(I_color);
209  vpDisplay::flush(I_color);
210 
211  if (vpDisplay::getClick(I_color, false)) {
212  break;
213  }
214 
215  time_vector.push_back(vpTime::measureTimeMs() - t);
216  if (vpTime::measureTimeMs() - t_begin >= 10000) {
217  break;
218  }
219  }
220  std::cout << "SR300_COLOR_RGBA8_1920x1080_30FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
221  << vpMath::getMedian(time_vector) << " ms" << std::endl;
222 
223  dc.close(I_color);
224 
225 
226  rs.setEnableStream(rs::stream::color, false);
227  rs.setEnableStream(rs::stream::depth, false);
228  rs.setEnableStream(rs::stream::infrared, true);
229  rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 200));
230  rs.open();
231 
232  vpImage<uint16_t> infrared((unsigned int) rs.getIntrinsics(rs::stream::infrared).height, (unsigned int) rs.getIntrinsics(rs::stream::infrared).width);
233  vpImage<unsigned char> I_display_infrared(infrared.getHeight(), infrared.getWidth());
234 
235 #if defined(VISP_HAVE_X11)
236  vpDisplayX di(I_display_infrared, 0, 0, "Infrared image");
237 #elif defined(VISP_HAVE_GDI)
238  vpDisplayGDI di(I_display_infrared, 0, 0, "Infrared image");
239 #endif
240 
241  time_vector.clear();
242  //Test infrared stream during 10 s
243  t_begin = vpTime::measureTimeMs();
244  while (true) {
245  double t = vpTime::measureTimeMs();
246  rs.acquire( NULL, NULL, NULL, (unsigned char *) infrared.bitmap );
247  vpImageConvert::convert(infrared, I_display_infrared);
248 
249  vpDisplay::display(I_display_infrared);
250  vpDisplay::flush(I_display_infrared);
251 
252  if (vpDisplay::getClick(I_display_infrared, false)) {
253  break;
254  }
255 
256  time_vector.push_back(vpTime::measureTimeMs() - t);
257  if (vpTime::measureTimeMs() - t_begin >= 10000) {
258  break;
259  }
260  }
261  std::cout << "SR300_INFRARED_Y16_640x480_200FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
262  << vpMath::getMedian(time_vector) << " ms" << std::endl;
263 
264  di.close(I_display_infrared);
265 
266 
267  rs.setEnableStream(rs::stream::color, false);
268  rs.setEnableStream(rs::stream::depth, true);
269  rs.setEnableStream(rs::stream::infrared, false);
270  rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60));
271  rs.open();
272 
273  depth.resize( (unsigned int) rs.getIntrinsics(rs::stream::depth).height, (unsigned int) rs.getIntrinsics(rs::stream::depth).width );
274  I_display_depth.resize( depth.getHeight(), depth.getWidth() );
275 
276 #if defined(VISP_HAVE_X11)
277  dd.init(I_display_depth, 0, 0, "Depth image");
278 #elif defined(VISP_HAVE_GDI)
279  dd.init(I_display_depth, 0, 0, "Depth image");
280 #endif
281 
282  time_vector.clear();
283  //Test depth stream during 10 s
284  t_begin = vpTime::measureTimeMs();
285  while (true) {
286  double t = vpTime::measureTimeMs();
287  rs.acquire( NULL, (unsigned char *) depth.bitmap, NULL, NULL );
288  vpImageConvert::createDepthHistogram(depth, I_display_depth);
289 
290  vpDisplay::display(I_display_depth);
291  vpDisplay::flush(I_display_depth);
292 
293  if (vpDisplay::getClick(I_display_depth, false)) {
294  break;
295  }
296 
297  time_vector.push_back(vpTime::measureTimeMs() - t);
298  if (vpTime::measureTimeMs() - t_begin >= 10000) {
299  break;
300  }
301  }
302  std::cout << "SR300_DEPTH_Z16_640x480_60FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
303  << vpMath::getMedian(time_vector) << " ms" << std::endl;
304 
305  dd.close(I_display_depth);
306 
307 
308  rs.setEnableStream(rs::stream::color, false);
309  rs.setEnableStream(rs::stream::depth, true);
310  rs.setEnableStream(rs::stream::infrared, true);
311  rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60));
312  rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60));
313  rs.open();
314 
315 #if defined(VISP_HAVE_X11)
316  dd.init(I_display_depth, 0, 0, "Depth image");
317  di.init(I_display_infrared, (int) I_display_depth.getWidth(), 0, "Infrared image");
318 #elif defined(VISP_HAVE_GDI)
319  dd.init(I_display_depth, 0, 0, "Depth image");
320  di.init(I_display_infrared, I_display_depth.getWidth(), 0, "Infrared image");
321 #endif
322 
323  time_vector.clear();
324  //Test depth stream during 10 s
325  t_begin = vpTime::measureTimeMs();
326  while (true) {
327  double t = vpTime::measureTimeMs();
328  rs.acquire( NULL, (unsigned char *) depth.bitmap, NULL, (unsigned char *) I_display_infrared.bitmap );
329  vpImageConvert::createDepthHistogram(depth, I_display_depth);
330 
331  vpDisplay::display(I_display_depth);
332  vpDisplay::display(I_display_infrared);
333  vpDisplay::flush(I_display_depth);
334  vpDisplay::flush(I_display_infrared);
335 
336  if (vpDisplay::getClick(I_display_depth, false) || vpDisplay::getClick(I_display_infrared, false)) {
337  break;
338  }
339 
340  time_vector.push_back(vpTime::measureTimeMs() - t);
341  if (vpTime::measureTimeMs() - t_begin >= 10000) {
342  break;
343  }
344  }
345  std::cout << "SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS - Mean time: " << vpMath::getMean(time_vector)
346  << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
347 
348 
349  dd.close(I_display_depth);
350  di.close(I_display_infrared);
351 
352 
353 
354 #ifdef VISP_HAVE_PCL
355  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
356 
357 #ifdef USE_THREAD
358  vpThread thread_display_pointcloud(displayPointcloudFunction, (vpThread::Args)&pointcloud);
359 #else
360  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
361  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
362  viewer->setBackgroundColor (0, 0, 0);
363  viewer->addCoordinateSystem (1.0);
364  viewer->initCameraParameters ();
365  viewer->setPosition(2*640+80, 480+80);
366  viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
367  viewer->setSize(640, 480);
368 #endif
369 
370  rs.setEnableStream(rs::stream::color, true);
371  rs.setEnableStream(rs::stream::depth, true);
372  rs.setEnableStream(rs::stream::infrared, true);
373  rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60));
374  rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60));
375  rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60));
376  rs.open();
377 
378  I_color.resize(480, 640);
379 
380 #if defined(VISP_HAVE_X11)
381  dc.init(I_color, 0, 0, "Color image");
382  dd.init(I_display_depth, 0, (int) I_color.getHeight()+80, "Depth image");
383  di.init(I_display_infrared, (int) I_display_depth.getWidth(), 0, "Infrared image");
384 #elif defined(VISP_HAVE_GDI)
385  dc.init(I_color, 0, 0, "Color image");
386  dd.init(I_display_depth, 0, (int) I_color.getHeight()+80, "Depth image");
387  di.init(I_display_infrared, (int) I_display_depth.getWidth(), 0, "Infrared image");
388 #endif
389 
390  //depth == 0 ; color == 1 ; rectified_color == 6 ; color_aligned_to_depth == 7 ; depth_aligned_to_color == 9 ; depth_aligned_to_rectified_color == 10
391  //argv[1] <==> color stream
392  rs::stream color_stream = argc > 1 ? (rs::stream) atoi(argv[1]) : rs::stream::color;
393  std::cout << "color_stream: " << color_stream << std::endl;
394  //argv[2] <==> depth stream
395  rs::stream depth_stream = argc > 2 ? (rs::stream) atoi(argv[2]) : rs::stream::depth;
396  std::cout << "depth_stream: " << depth_stream << std::endl;
397 
398  time_vector.clear();
399  //Test depth stream during 10 s
400  t_begin = vpTime::measureTimeMs();
401  while (true) {
402  double t = vpTime::measureTimeMs();
403  rs.acquire( (unsigned char *) I_color.bitmap, (unsigned char *) depth.bitmap, NULL, pointcloud, (unsigned char *) I_display_infrared.bitmap,
404  NULL, color_stream, depth_stream);
405  vpImageConvert::createDepthHistogram(depth, I_display_depth);
406 
407 #ifdef VISP_HAVE_PCL
408 #ifdef USE_THREAD
409  {
410  vpMutex::vpScopedLock lock(s_mutex_capture);
411  s_capture_state = capture_started;
412  }
413 #else
414  static bool update = false;
415  if (! update) {
416  viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
417  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
418  viewer->setPosition( (int) I_color.getWidth()+80, (int) I_color.getHeight()+80) ;
419  update = true;
420  }
421  else {
422  viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
423  }
424 
425  viewer->spinOnce (10);
426 #endif
427 #endif
428 
429  vpDisplay::display(I_color);
430  vpDisplay::display(I_display_depth);
431  vpDisplay::display(I_display_infrared);
432  vpDisplay::flush(I_color);
433  vpDisplay::flush(I_display_depth);
434  vpDisplay::flush(I_display_infrared);
435 
436  if (vpDisplay::getClick(I_color, false) || vpDisplay::getClick(I_display_depth, false) || vpDisplay::getClick(I_display_infrared, false)) {
437  break;
438  }
439 
440  time_vector.push_back(vpTime::measureTimeMs() - t);
441  if (vpTime::measureTimeMs() - t_begin >= 10000) {
442  break;
443  }
444  }
445  std::cout << "SR300_COLOR_RGBA8_640x480_60FPS + SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS - Mean time: " << vpMath::getMean(time_vector)
446  << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
447 
448 #ifdef VISP_HAVE_PCL
449 #ifdef USE_THREAD
450  {
451  vpMutex::vpScopedLock lock(s_mutex_capture);
452  s_capture_state = capture_stopped;
453  }
454 #endif
455 #endif
456 
457  dc.close(I_color);
458  dd.close(I_display_depth);
459  di.close(I_display_infrared);
460 
461 #endif
462 
463 
464  //Color stream aligned to depth
465  rs.setEnableStream(rs::stream::color, true);
466  rs.setEnableStream(rs::stream::depth, true);
467  rs.setEnableStream(rs::stream::infrared, false);
468  rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 30));
469  rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 30));
470  rs.open();
471 
472  dc.init(I_color, 0, 0, "Color aligned to depth");
473  di.init(I_display_depth, (int) I_color.getWidth(), 0, "Depth image");
474 
475  t_begin = vpTime::measureTimeMs();
476  while (true) {
477  double t = vpTime::measureTimeMs();
478  rs.acquire( (unsigned char *) I_color.bitmap, (unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color_aligned_to_depth );
479  vpImageConvert::createDepthHistogram(depth, I_display_depth);
480 
481  vpDisplay::display(I_color);
482  vpDisplay::display(I_display_depth);
483  vpDisplay::flush(I_color);
484  vpDisplay::flush(I_display_depth);
485 
486  if (vpDisplay::getClick(I_color, false) || vpDisplay::getClick(I_display_depth, false)) {
487  break;
488  }
489 
490  time_vector.push_back(vpTime::measureTimeMs() - t);
491  if (vpTime::measureTimeMs() - t_begin >= 10000) {
492  break;
493  }
494  }
495 
496  dc.close(I_color);
497  dd.close(I_display_depth);
498 
499 
500  //Depth stream aligned to color
501  dc.init(I_color, 0, 0, "Color image");
502  di.init(I_display_depth, (int) I_color.getWidth(), 0, "Depth aligned to color");
503 
504  t_begin = vpTime::measureTimeMs();
505  while (true) {
506  double t = vpTime::measureTimeMs();
507  rs.acquire( (unsigned char *) I_color.bitmap, (unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color, rs::stream::depth_aligned_to_color );
508  vpImageConvert::createDepthHistogram(depth, I_display_depth);
509 
510  vpDisplay::display(I_color);
511  vpDisplay::display(I_display_depth);
512  vpDisplay::flush(I_color);
513  vpDisplay::flush(I_display_depth);
514 
515  if (vpDisplay::getClick(I_color, false) || vpDisplay::getClick(I_display_depth, false)) {
516  break;
517  }
518 
519  time_vector.push_back(vpTime::measureTimeMs() - t);
520  if (vpTime::measureTimeMs() - t_begin >= 10000) {
521  break;
522  }
523  }
524 
525  dc.close(I_color);
526  dd.close(I_display_depth);
527 
528 
529 
530 #if VISP_HAVE_OPENCV_VERSION >= 0x020409
531  rs.setEnableStream(rs::stream::color, true);
532  rs.setEnableStream(rs::stream::depth, false);
533  rs.setEnableStream(rs::stream::infrared, true);
534  rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::bgr8, 60));
535  rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 200));
536  rs.open();
537 
538  cv::Mat color_mat(480, 640, CV_8UC3);
539  cv::Mat infrared_mat(480, 640, CV_8U);
540 
541  t_begin = vpTime::measureTimeMs();
542  while (true) {
543  double t = vpTime::measureTimeMs();
544  rs.acquire( color_mat.data, NULL, NULL, infrared_mat.data );
545 
546  cv::imshow("Color mat", color_mat);
547  cv::imshow("Infrared mat", infrared_mat);
548  char c = cv::waitKey(10);
549  if (c == 27) {
550  break;
551  }
552 
553  time_vector.push_back(vpTime::measureTimeMs() - t);
554  if (vpTime::measureTimeMs() - t_begin >= 10000) {
555  break;
556  }
557  }
558 #endif
559 
560  } catch(const vpException &e) {
561  std::cerr << "RealSense error " << e.what() << std::endl;
562  } catch(const rs::error & e) {
563  std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "): " << e.what() << std::endl;
564  } catch(const std::exception & e) {
565  std::cerr << e.what() << std::endl;
566  }
567 
568 #elif !defined(VISP_HAVE_REALSENSE)
569  std::cout << "Install RealSense SDK to make this test working. X11 or GDI are needed also." << std::endl;
570 #elif !defined(VISP_HAVE_CPP11_COMPATIBILITY)
571  std::cout << "Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) to make this test working" << std::endl;
572 #endif
573  return EXIT_SUCCESS;
574 }
Class that allows protection by mutex.
Definition: vpMutex.h:163
rs::device * getHandler() const
Get access to device handler.
Definition: vpRealSense.h:355
void lock()
Definition: vpMutex.h:90
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void * Return
Definition: vpThread.h:36
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void init(unsigned int height, unsigned int width)
Set the size of the image.
Definition: vpImage.h:664
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:217
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
void unlock()
Definition: vpMutex.h:106
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
error that can be emited by ViSP classes.
Definition: vpException.h:73
void acquire(std::vector< vpColVector > &pointcloud)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:197
void * Args
Definition: vpThread.h:35
static void display(const vpImage< unsigned char > &I)
const char * what() const
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void setEnableStream(const rs::stream &stream, const bool status)
rs::intrinsics getIntrinsics(const rs::stream &stream) const
Definition of the vpImage class member functions.
Definition: vpImage.h:117
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)