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>
61 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
62 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
63 bool cancelled =
false, update_pointcloud =
false;
68 explicit ViewerWorker(
const bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {}
73 pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer(
"3D Viewer " + date));
74 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
75 pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
76 pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
78 viewer->setBackgroundColor(0, 0, 0);
79 viewer->initCameraParameters();
80 viewer->setPosition(640 + 80, 480 + 80);
81 viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
82 viewer->setSize(640, 480);
85 bool local_update =
false, local_cancelled =
false;
86 while (!local_cancelled) {
88 std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
90 if (lock.owns_lock()) {
91 local_update = update_pointcloud;
92 update_pointcloud =
false;
93 local_cancelled = cancelled;
97 local_pointcloud_color = pointcloud_color->makeShared();
99 local_pointcloud = pointcloud->makeShared();
105 if (local_update && !local_cancelled) {
106 local_update =
false;
110 viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
111 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
114 viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
115 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
120 viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
122 viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
130 std::cout <<
"End of point cloud display thread" << std::endl;
145 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
146 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
147 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
154 std::cout <<
"Extrinsics cMd: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
155 std::cout <<
"Extrinsics dMc: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_COLOR) << std::endl;
156 std::cout <<
"Extrinsics cMi: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED) << std::endl;
157 std::cout <<
"Extrinsics dMi: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_INFRARED) << std::endl;
169 ViewerWorker viewer(
true, mutex);
170 std::thread viewer_thread(&ViewerWorker::run, &viewer);
173 #if defined(VISP_HAVE_X11)
175 vpDisplayX di(infrared, (
int)color.getWidth() + 80, 10,
"Infrared image");
176 vpDisplayX dd(depth_display, 10, (
int)color.getHeight() + 80,
"Depth image");
177 #elif defined(VISP_HAVE_GDI)
179 vpDisplayGDI di(infrared, color.getWidth() + 80, 10,
"Infrared image");
180 vpDisplayGDI dd(depth_display, 10, color.getHeight() + 80,
"Depth image");
188 std::lock_guard<std::mutex> lock(mutex);
189 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, pointcloud_color,
190 (
unsigned char *)infrared.bitmap);
191 update_pointcloud =
true;
194 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, (
unsigned char *)infrared.bitmap);
215 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
219 std::lock_guard<std::mutex> lock(mutex);
223 viewer_thread.join();
227 std::cerr <<
"RealSense error " << e.
what() << std::endl;
228 }
catch (
const std::exception &e) {
229 std::cerr << e.what() << std::endl;
237 #if !defined(VISP_HAVE_REALSENSE2)
238 std::cout <<
"You do not realsense2 SDK functionality enabled..." << std::endl;
239 std::cout <<
"Tip:" << std::endl;
240 std::cout <<
"- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
243 #if !defined(VISP_HAVE_CPP11_COMPATIBILITY)
244 std::cout <<
"You do not build ViSP with C++11 compiler flag" << std::endl;
245 std::cout <<
"Tip:" << std::endl;
246 std::cout <<
"- Configure ViSP again using cmake -DUSE_CPP11=ON, and build again this example" << std::endl;