46 #include <visp3/sensor/vpRealSense.h> 47 #include <visp3/gui/vpDisplayX.h> 48 #include <visp3/gui/vpDisplayGDI.h> 49 #include <visp3/io/vpImageIo.h> 50 #include <visp3/core/vpImageConvert.h> 51 #include <visp3/core/vpMutex.h> 52 #include <visp3/core/vpThread.h> 54 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) 57 #if( ! defined(__APPLE__) && ! defined(__MACH__) ) // Not OSX 58 # if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available 64 # include <pcl/visualization/cloud_viewer.h> 65 # include <pcl/visualization/pcl_visualizer.h> 76 t_CaptureState s_capture_state = capture_waiting;
82 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *) args);
84 pcl::visualization::PCLVisualizer::Ptr viewer (
new pcl::visualization::PCLVisualizer (
"3D Viewer"));
85 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
86 viewer->setBackgroundColor (0, 0, 0);
87 viewer->addCoordinateSystem (1.0);
88 viewer->initCameraParameters ();
89 viewer->setPosition(640+80, 480+80);
90 viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
91 viewer->setSize(640, 480);
93 t_CaptureState capture_state_;
96 s_mutex_capture.
lock();
97 capture_state_ = s_capture_state;
100 if (capture_state_ == capture_started) {
101 static bool update =
false;
103 viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud_, rgb,
"sample cloud");
104 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
108 viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud_, rgb,
"sample cloud");
111 viewer->spinOnce (10);
113 }
while(capture_state_ != capture_stopped);
115 std::cout <<
"End of point cloud display thread" << std::endl;
124 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) 133 std::cout <<
"Extrinsics cMd: \n" << rs.
getTransformation(rs::stream::color, rs::stream::depth) << std::endl;
134 std::cout <<
"Extrinsics dMc: \n" << rs.
getTransformation(rs::stream::depth, rs::stream::color) << std::endl;
135 std::cout <<
"Extrinsics cMi: \n" << rs.
getTransformation(rs::stream::color, rs::stream::infrared) << std::endl;
136 std::cout <<
"Extrinsics dMi: \n" << rs.
getTransformation(rs::stream::depth, rs::stream::infrared) << std::endl;
145 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
150 pcl::visualization::PCLVisualizer::Ptr viewer (
new pcl::visualization::PCLVisualizer (
"3D Viewer"));
151 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
152 viewer->setBackgroundColor (0, 0, 0);
153 viewer->addCoordinateSystem (1.0);
154 viewer->initCameraParameters ();
155 viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
159 std::vector<vpColVector> pointcloud;
162 #if defined(VISP_HAVE_X11) 164 vpDisplayX di(infrared_display, (
int) color.getWidth()+80, 10,
"Infrared image");
165 vpDisplayX dd(depth_display, 10, (
int) color.getHeight()+80,
"Depth image");
166 #elif defined(VISP_HAVE_GDI) 168 vpDisplayGDI di(infrared_display, color.getWidth()+80, 10,
"Infrared image");
169 vpDisplayGDI dd(depth_display, 10, color.getHeight()+80,
"Depth image");
171 std::cout <<
"No image viewer is available..." << std::endl;
176 rs.
acquire(color, infrared, depth, pointcloud);
182 s_capture_state = capture_started;
185 static bool update =
false;
187 viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb,
"sample cloud");
188 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
189 viewer->setPosition(color.getWidth()+80, color.getHeight()+80);
193 viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb,
"sample cloud");
196 viewer->spinOnce (10);
218 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
224 s_capture_state = capture_stopped;
234 catch(
const rs::error & e) {
235 std::cerr <<
"RealSense error calling " << e.get_failed_function() <<
"(" << e.get_failed_args() <<
"): " << e.what() << std::endl;
237 catch(
const std::exception & e) {
238 std::cerr << e.what() << std::endl;
241 #elif !defined(VISP_HAVE_REALSENSE) 242 std::cout <<
"Install RealSense SDK to make this test working" << std::endl;
243 #elif !defined(VISP_HAVE_CPP11_COMPATIBILITY) 244 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)
Perspective projection without distortion model.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Display for windows using GDI (available on any windows 32 platform).
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
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 void display(const vpImage< unsigned char > &I)
Perspective projection with distortion model.
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
rs::intrinsics getIntrinsics(const rs::stream &stream) const
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).