44#include <visp3/core/vpImageConvert.h>
45#include <visp3/core/vpMutex.h>
46#include <visp3/core/vpThread.h>
47#include <visp3/gui/vpDisplayGDI.h>
48#include <visp3/gui/vpDisplayX.h>
49#include <visp3/sensor/vpRealSense.h>
51#if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
55#if (!defined(__APPLE__) && !defined(__MACH__))
56#if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32))
62#include <pcl/visualization/cloud_viewer.h>
63#include <pcl/visualization/pcl_visualizer.h>
69typedef enum { capture_waiting, capture_started, capture_stopped } t_CaptureState;
70t_CaptureState s_capture_state = capture_waiting;
75 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *)args);
77 pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer(
"3D Viewer"));
78 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
79 viewer->setBackgroundColor(0, 0, 0);
80 viewer->initCameraParameters();
81 viewer->setPosition(640 + 80, 480 + 80);
82 viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
83 viewer->setSize(640, 480);
85 t_CaptureState capture_state_;
88 s_mutex_capture.
lock();
89 capture_state_ = s_capture_state;
92 if (capture_state_ == capture_started) {
93 static bool update =
false;
95 viewer->addPointCloud<pcl::PointXYZRGB>(pointcloud_, rgb,
"sample cloud");
96 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
100 viewer->updatePointCloud<pcl::PointXYZRGB>(pointcloud_, rgb,
"sample cloud");
103 viewer->spinOnce(10);
105 }
while (capture_state_ != capture_stopped);
107 std::cout <<
"End of point cloud display thread" << std::endl;
116#if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
126 std::cout <<
"Extrinsics cMd: \n" << rs.
getTransformation(rs::stream::color, rs::stream::depth) << std::endl;
127 std::cout <<
"Extrinsics dMc: \n" << rs.
getTransformation(rs::stream::depth, rs::stream::color) << std::endl;
128 std::cout <<
"Extrinsics cMi: \n" << rs.
getTransformation(rs::stream::color, rs::stream::infrared) << std::endl;
129 std::cout <<
"Extrinsics dMi: \n" << rs.
getTransformation(rs::stream::depth, rs::stream::infrared) << std::endl;
138 bool use_infrared_y16 = dev->get_stream_format(rs::stream::infrared) == rs::format::y16;
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;
177 if (use_infrared_y16) {
178 rs.
acquire(color, infrared_y16, depth, pointcloud);
183 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, pointcloud,
184 (
unsigned char *)infrared_display.bitmap);
186 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL,
187 (
unsigned char *)infrared_display.bitmap);
195 s_capture_state = capture_started;
198 static bool update =
false;
200 viewer->addPointCloud<pcl::PointXYZRGB>(pointcloud, rgb,
"sample cloud");
201 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
202 viewer->setPosition(color.getWidth() + 80, color.getHeight() + 80);
206 viewer->updatePointCloud<pcl::PointXYZRGB>(pointcloud, rgb,
"sample cloud");
209 viewer->spinOnce(10);
231 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
237 s_capture_state = capture_stopped;
245 std::cerr <<
"RealSense error " << e.
what() << std::endl;
247 catch (
const rs::error &e) {
248 std::cerr <<
"RealSense error calling " << e.get_failed_function() <<
"(" << e.get_failed_args()
249 <<
"): " << e.what() << std::endl;
251 catch (
const std::exception &e) {
252 std::cerr << e.what() << std::endl;
255#elif !defined(VISP_HAVE_REALSENSE)
256 std::cout <<
"This deprecated example is only working with librealsense 1.x" << std::endl;
257#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
258 std::cout <<
"You do not build ViSP with c++11 or higher compiler flag" << std::endl;
259 std::cout <<
"Tip:" << std::endl;
260 std::cout <<
"- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
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...
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)
error that can be emitted by ViSP classes.
const char * what() 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)
Definition of the vpImage class member functions.
Class that allows protection by mutex.
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
rs::device * getHandler() const
Get access to device handler.
void acquire(std::vector< vpColVector > &pointcloud)
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
rs::intrinsics getIntrinsics(const rs::stream &stream) const
VISP_EXPORT double measureTimeMs()