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) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \ 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(
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;
146 std::cout <<
"Product line: " << product_line << std::endl;
148 if (product_line ==
"T200") {
149 std::cout <<
"This example doesn't support T200 product line family !" << std::endl;
153 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
154 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
155 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
162 std::cout <<
"Extrinsics cMd: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
163 std::cout <<
"Extrinsics dMc: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_COLOR) << std::endl;
164 std::cout <<
"Extrinsics cMi: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED) << std::endl;
165 std::cout <<
"Extrinsics dMi: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_INFRARED) << std::endl;
177 ViewerWorker viewer(
true, mutex);
178 std::thread viewer_thread(&ViewerWorker::run, &viewer);
181 #if defined(VISP_HAVE_X11) 183 vpDisplayX di(infrared, (
int)color.getWidth() + 80, 10,
"Infrared image");
184 vpDisplayX dd(depth_display, 10, (
int)color.getHeight() + 80,
"Depth image");
185 #elif defined(VISP_HAVE_GDI) 187 vpDisplayGDI di(infrared, color.getWidth() + 80, 10,
"Infrared image");
188 vpDisplayGDI dd(depth_display, 10, color.getHeight() + 80,
"Depth image");
196 std::lock_guard<std::mutex> lock(mutex);
197 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, pointcloud_color,
198 (
unsigned char *)infrared.bitmap);
199 update_pointcloud =
true;
202 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth.bitmap, NULL, (
unsigned char *)infrared.bitmap);
223 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
227 std::lock_guard<std::mutex> lock(mutex);
231 viewer_thread.join();
235 std::cerr <<
"RealSense error " << e.
what() << std::endl;
236 }
catch (
const std::exception &e) {
237 std::cerr << e.what() << std::endl;
245 #if !defined(VISP_HAVE_REALSENSE2) 246 std::cout <<
"You do not realsense2 SDK functionality enabled..." << std::endl;
247 std::cout <<
"Tip:" << std::endl;
248 std::cout <<
"- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
251 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) 252 std::cout <<
"You do not build ViSP with c++11 or higher compiler flag" << std::endl;
253 std::cout <<
"Tip:" << std::endl;
254 std::cout <<
"- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
std::string getProductLine()
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.
static void flush(const vpImage< unsigned char > &I)
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
const char * what() const
static void display(const vpImage< unsigned char > &I)
rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index=-1) const
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)