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 (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);
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;
122 #
if defined(VISP_HAVE_PCL)
123 int argc,
char *argv[]
127 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && ( defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) ) 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;
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;
154 std::vector<double> time_vector;
156 #if defined(VISP_HAVE_X11) 157 vpDisplayX dd(I_display_depth, 0, 0,
"Depth image");
158 #elif defined(VISP_HAVE_GDI) 166 rs.
acquire( NULL, (
unsigned char *) depth.bitmap, NULL, NULL );
181 std::cout <<
"SR300_DEPTH_Z16_640x240_110FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: " 184 dd.close(I_display_depth);
195 #if defined(VISP_HAVE_X11) 197 #elif defined(VISP_HAVE_GDI) 206 rs.
acquire( (
unsigned char *) I_color.bitmap, NULL, NULL, NULL );
220 std::cout <<
"SR300_COLOR_RGBA8_1920x1080_30FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: " 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");
246 rs.
acquire( NULL, NULL, NULL, (
unsigned char *) infrared.bitmap );
261 std::cout <<
"SR300_INFRARED_Y16_640x480_200FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: " 264 di.close(I_display_infrared);
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() );
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");
287 rs.
acquire( NULL, (
unsigned char *) depth.bitmap, NULL, NULL );
302 std::cout <<
"SR300_DEPTH_Z16_640x480_60FPS - Mean time: " <<
vpMath::getMean(time_vector) <<
" ms ; Median time: " 305 dd.close(I_display_depth);
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");
328 rs.
acquire( NULL, (
unsigned char *) depth.bitmap, NULL, (
unsigned char *) I_display_infrared.bitmap );
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;
349 dd.close(I_display_depth);
350 di.close(I_display_infrared);
355 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
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);
378 I_color.resize(480, 640);
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");
392 rs::stream color_stream = argc > 1 ? (rs::stream) atoi(argv[1]) : rs::stream::color;
393 std::cout <<
"color_stream: " << color_stream << std::endl;
395 rs::stream depth_stream = argc > 2 ? (rs::stream) atoi(argv[2]) : rs::stream::depth;
396 std::cout <<
"depth_stream: " << depth_stream << std::endl;
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);
411 s_capture_state = capture_started;
414 static bool update =
false;
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) ;
422 viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb,
"sample cloud");
425 viewer->spinOnce (10);
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;
452 s_capture_state = capture_stopped;
458 dd.close(I_display_depth);
459 di.close(I_display_infrared);
472 dc.init(I_color, 0, 0,
"Color aligned to depth");
473 di.init(I_display_depth, (
int) I_color.getWidth(), 0,
"Depth image");
478 rs.
acquire( (
unsigned char *) I_color.bitmap, (
unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color_aligned_to_depth );
497 dd.close(I_display_depth);
501 dc.
init(I_color, 0, 0,
"Color image");
502 di.init(I_display_depth, (
int) I_color.getWidth(), 0,
"Depth aligned to color");
507 rs.
acquire( (
unsigned char *) I_color.bitmap, (
unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color, rs::stream::depth_aligned_to_color );
526 dd.close(I_display_depth);
530 #if VISP_HAVE_OPENCV_VERSION >= 0x020409 538 cv::Mat color_mat(480, 640, CV_8UC3);
539 cv::Mat infrared_mat(480, 640, CV_8U);
544 rs.
acquire( color_mat.data, NULL, NULL, infrared_mat.data );
546 cv::imshow(
"Color mat", color_mat);
547 cv::imshow(
"Infrared mat", infrared_mat);
548 char c = cv::waitKey(10);
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;
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;
Class that allows protection by mutex.
rs::device * getHandler() const
Get access to device handler.
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.
static double getMedian(const std::vector< double > &v)
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...
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)
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.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)