44 #include <visp3/core/vpImageConvert.h> 45 #include <visp3/gui/vpDisplayGDI.h> 46 #include <visp3/gui/vpDisplayX.h> 47 #include <visp3/sensor/vpRealSense2.h> 49 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \ 50 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) 53 #include <pcl/visualization/cloud_viewer.h> 54 #include <pcl/visualization/pcl_visualizer.h> 60 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
61 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
62 bool cancelled =
false, update_pointcloud =
false;
67 explicit ViewerWorker(
const bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {}
72 pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer(
"3D Viewer " + date));
73 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
74 pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
75 pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
77 viewer->setBackgroundColor(0, 0, 0);
78 viewer->initCameraParameters();
79 viewer->setPosition(640 + 80, 480 + 80);
80 viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
81 viewer->setSize(640, 480);
84 bool local_update =
false, local_cancelled =
false;
85 while (!local_cancelled) {
87 std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
89 if (lock.owns_lock()) {
90 local_update = update_pointcloud;
91 update_pointcloud =
false;
92 local_cancelled = cancelled;
96 local_pointcloud_color = pointcloud_color->makeShared();
98 local_pointcloud = pointcloud->makeShared();
104 if (local_update && !local_cancelled) {
105 local_update =
false;
109 viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
110 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
113 viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
114 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
119 viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
121 viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
129 std::cout <<
"End of point cloud display thread" << std::endl;
144 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
145 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
146 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
153 std::cout <<
"Extrinsics cMd: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
154 std::cout <<
"Extrinsics dMc: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_COLOR) << std::endl;
155 std::cout <<
"Extrinsics cMi: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED) << std::endl;
156 std::cout <<
"Extrinsics dMi: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_INFRARED) << std::endl;
168 ViewerWorker viewer(
true, mutex);
169 std::thread viewer_thread(&ViewerWorker::run, &viewer);
172 #if defined(VISP_HAVE_X11) 174 vpDisplayX di(infrared, (
int)color.getWidth() + 80, 10,
"Infrared image");
175 vpDisplayX dd(depth_display, 10, (
int)color.getHeight() + 80,
"Depth image");
176 #elif defined(VISP_HAVE_GDI) 178 vpDisplayGDI di(infrared, color.getWidth() + 80, 10,
"Infrared image");
179 vpDisplayGDI dd(depth_display, 10, color.getHeight() + 80,
"Depth image");
187 std::lock_guard<std::mutex> lock(mutex);
188 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, pointcloud_color,
189 (
unsigned char *)infrared.bitmap);
190 update_pointcloud =
true;
193 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, (
unsigned char *)infrared.bitmap);
214 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
218 std::lock_guard<std::mutex> lock(mutex);
222 viewer_thread.join();
226 std::cerr <<
"RealSense error " << e.
what() << std::endl;
227 }
catch (
const std::exception &e) {
228 std::cerr << e.what() << std::endl;
236 #if !defined(VISP_HAVE_REALSENSE2) 237 std::cout <<
"Install librealsense2." << std::endl;
239 #if !defined(VISP_HAVE_CPP11_COMPATIBILITY) 240 std::cout <<
"Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON)." << std::endl;
rs2_intrinsics getIntrinsics(const rs2_stream &stream) const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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 open(const rs2::config &cfg=rs2::config())
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
static void display(const vpImage< unsigned char > &I)
void acquire(vpImage< unsigned char > &grey)
const char * what() const
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to) const
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)