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>
50 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
51 # include <librealsense/rs.h>
54 #if( ! defined(__APPLE__) && ! defined(__MACH__) ) // Not OSX
55 # if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
61 # include <pcl/visualization/cloud_viewer.h>
62 # include <pcl/visualization/pcl_visualizer.h>
73 t_CaptureState s_capture_state = capture_waiting;
79 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *) args);
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 (1.0);
85 viewer->initCameraParameters ();
86 viewer->setPosition(640+80, 480+80);
87 viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
88 viewer->setSize(640, 480);
90 t_CaptureState capture_state_;
93 s_mutex_capture.
lock();
94 capture_state_ = s_capture_state;
97 if (capture_state_ == capture_started) {
98 static bool update =
false;
100 viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud_, rgb,
"sample cloud");
101 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
105 viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud_, rgb,
"sample cloud");
108 viewer->spinOnce (10);
110 }
while(capture_state_ != capture_stopped);
112 std::cout <<
"End of point cloud display thread" << std::endl;
123 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && ( defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) )
129 if ( rs_get_device_name((
const rs_device *) rs.
getHandler(),
nullptr) != std::string(
"Intel RealSense SR300") ) {
130 std::cout <<
"This test file is used to test the Intel RealSense SR300 only." << std::endl;
143 std::cout <<
"API version: " << rs_get_api_version(
nullptr) << std::endl;
144 std::cout <<
"Firmware: " << rs_get_device_firmware_version((
const rs_device *) rs.
getHandler(),
nullptr) << std::endl;
145 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
150 std::vector<double> time_vector;
152 #if defined(VISP_HAVE_X11)
153 vpDisplayX dd(I_display_depth, 0, 0,
"Depth image");
154 #elif defined(VISP_HAVE_GDI)
162 rs.
acquire( NULL, (
unsigned char *) depth.bitmap, NULL, NULL );
177 std::cout <<
"SR300_DEPTH_Z16_640x240_110FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: "
180 dd.close(I_display_depth);
191 #if defined(VISP_HAVE_X11)
193 #elif defined(VISP_HAVE_GDI)
202 rs.
acquire( (
unsigned char *) I_color.bitmap, NULL, NULL, NULL );
216 std::cout <<
"SR300_COLOR_RGBA8_1920x1080_30FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: "
231 #if defined(VISP_HAVE_X11)
232 vpDisplayX di(I_display_infrared, 0, 0,
"Infrared image");
233 #elif defined(VISP_HAVE_GDI)
234 vpDisplayGDI di(I_display_infrared, 0, 0,
"Infrared image");
242 rs.
acquire( NULL, NULL, NULL, (
unsigned char *) infrared.bitmap );
257 std::cout <<
"SR300_INFRARED_Y16_640x480_200FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: "
260 di.close(I_display_infrared);
269 depth.resize( (
unsigned int) rs.
getIntrinsics(rs::stream::depth).height, (
unsigned int) rs.
getIntrinsics(rs::stream::depth).width );
270 I_display_depth.resize( depth.getHeight(), depth.getWidth() );
272 #if defined(VISP_HAVE_X11)
273 dd.init(I_display_depth, 0, 0,
"Depth image");
274 #elif defined(VISP_HAVE_GDI)
275 dd.init(I_display_depth, 0, 0,
"Depth image");
283 rs.
acquire( NULL, (
unsigned char *) depth.bitmap, NULL, NULL );
298 std::cout <<
"SR300_DEPTH_Z16_640x480_60FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: "
301 dd.close(I_display_depth);
311 #if defined(VISP_HAVE_X11)
312 dd.init(I_display_depth, 0, 0,
"Depth image");
313 di.init(I_display_infrared, (
int) I_display_depth.getWidth(), 0,
"Infrared image");
314 #elif defined(VISP_HAVE_GDI)
315 dd.init(I_display_depth, 0, 0,
"Depth image");
316 di.init(I_display_infrared, I_display_depth.getWidth(), 0,
"Infrared image");
324 rs.
acquire( NULL, (
unsigned char *) depth.bitmap, NULL, (
unsigned char *) I_display_infrared.bitmap );
341 std::cout <<
"SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS - Mean time: " <<
vpMath::getMean(time_vector)
342 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
345 dd.close(I_display_depth);
346 di.close(I_display_infrared);
351 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
356 pcl::visualization::PCLVisualizer::Ptr viewer (
new pcl::visualization::PCLVisualizer (
"3D Viewer"));
357 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
358 viewer->setBackgroundColor (0, 0, 0);
359 viewer->addCoordinateSystem (1.0);
360 viewer->initCameraParameters ();
361 viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
362 viewer->setSize(640, 480);
373 I_color.resize(480, 640);
375 #if defined(VISP_HAVE_X11)
376 dc.init(I_color, 0, 0,
"Color image");
377 dd.init(I_display_depth, 0, (
int) I_color.getHeight()+80,
"Depth image");
378 di.init(I_display_infrared, (
int) I_display_depth.getWidth(), 0,
"Infrared image");
379 #elif defined(VISP_HAVE_GDI)
380 dc.init(I_color, 0, 0,
"Color image");
381 dd.init(I_display_depth, 0, (
int) I_color.getHeight()+80,
"Depth image");
382 di.init(I_display_infrared, (
int) I_display_depth.getWidth(), 0,
"Infrared image");
390 rs.
acquire( (
unsigned char *) I_color.bitmap, (
unsigned char *) depth.bitmap, NULL, pointcloud, (
unsigned char *) I_display_infrared.bitmap );
397 s_capture_state = capture_started;
400 static bool update =
false;
402 viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb,
"sample cloud");
403 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
404 viewer->setPosition( (
int) I_color.getWidth()+80, (int) I_color.getHeight()+80) ;
408 viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb,
"sample cloud");
411 viewer->spinOnce (10);
431 std::cout <<
"SR300_COLOR_RGBA8_640x480_60FPS + SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS - Mean time: " <<
vpMath::getMean(time_vector)
432 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
438 s_capture_state = capture_stopped;
444 dd.close(I_display_depth);
445 di.close(I_display_infrared);
458 dc.init(I_color, 0, 0,
"Color aligned to depth");
459 di.init(I_display_depth, (
int) I_color.getWidth(), 0,
"Depth image");
464 rs.
acquire( (
unsigned char *) I_color.bitmap, (
unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color_aligned_to_depth );
483 dd.close(I_display_depth);
487 dc.
init(I_color, 0, 0,
"Color image");
488 di.init(I_display_depth, (
int) I_color.getWidth(), 0,
"Depth aligned to color");
493 rs.
acquire( (
unsigned char *) I_color.bitmap, (
unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color, rs::stream::depth_aligned_to_color );
512 dd.close(I_display_depth);
516 #if VISP_HAVE_OPENCV_VERSION >= 0x020409
524 cv::Mat color_mat(480, 640, CV_8UC3);
525 cv::Mat infrared_mat(480, 640, CV_8U);
530 rs.
acquire( color_mat.data, NULL, NULL, infrared_mat.data );
532 cv::imshow(
"Color mat", color_mat);
533 cv::imshow(
"Infrared mat", infrared_mat);
534 char c = cv::waitKey(10);
547 std::cerr <<
"RealSense error " << e.
what() << std::endl;
548 }
catch(
const rs::error & e) {
549 std::cerr <<
"RealSense error calling " << e.get_failed_function() <<
"(" << e.get_failed_args() <<
"): " << e.what() << std::endl;
550 }
catch(
const std::exception & e) {
551 std::cerr << e.what() << std::endl;
554 #elif !defined(VISP_HAVE_REALSENSE)
555 std::cout <<
"Install RealSense SDK to make this test working. X11 or GDI are needed also." << std::endl;
556 #elif !defined(VISP_HAVE_CPP11_COMPATIBILITY)
557 std::cout <<
"Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) to make this test working" << std::endl;
Class that allows protection by mutex.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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.
rs::device * getHandler() const
Get access to device handler.
static double getMedian(const std::vector< double > &v)
Display for windows using GDI (available on any windows 32 platform).
Define the X11 console to display images.
error that can be emited by ViSP classes.
void acquire(std::vector< vpColVector > &pointcloud)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
static double getMean(const std::vector< double > &v)
const char * what() const
static void display(const vpImage< unsigned char > &I)
rs::intrinsics getIntrinsics(const rs::stream &stream) const
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void setEnableStream(const rs::stream &stream, const bool status)
Definition of the vpImage class member functions.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)