44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_REALSENSE2) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
48 #include <visp3/core/vpTime.h>
49 #include <visp3/core/vpImageConvert.h>
50 #include <visp3/gui/vpDisplayGDI.h>
51 #include <visp3/gui/vpDisplayX.h>
52 #include <visp3/gui/vpDisplayPCL.h>
53 #include <visp3/sensor/vpRealSense2.h>
61 std::cout <<
"Product line: " << product_line << std::endl;
63 if (product_line ==
"T200") {
64 std::cout <<
"This example doesn't support T200 product line family !" << std::endl;
68 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
69 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
70 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
77 std::cout <<
"Extrinsics cMd: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
78 std::cout <<
"Extrinsics dMc: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_COLOR) << std::endl;
79 std::cout <<
"Extrinsics cMi: \n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_INFRARED) << std::endl;
80 std::cout <<
"Extrinsics dMi: \n" << rs.
getTransformation(RS2_STREAM_DEPTH, RS2_STREAM_INFRARED) << std::endl;
90 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
92 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
93 #if defined(VISP_HAVE_PCL_VISUALIZATION)
94 vpDisplayPCL pcl_viewer(color.getWidth() + 80, color.getHeight() + 70,
"3D viewer " +
vpTime::getDateTime());
95 pcl_viewer.startThread(std::ref(mutex), pointcloud_color);
99 #if defined(VISP_HAVE_X11)
101 vpDisplayX di(infrared,
static_cast<int>(color.getWidth()) + 80, 10,
"Infrared image");
102 vpDisplayX dd(depth_display, 10,
static_cast<int>(color.getHeight()) + 70,
"Depth image");
103 #elif defined(VISP_HAVE_GDI)
105 vpDisplayGDI di(infrared, color.getWidth() + 80, 10,
"Infrared image");
106 vpDisplayGDI dd(depth_display, 10, color.getHeight() + 70,
"Depth image");
112 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
114 std::lock_guard<std::mutex> lock(mutex);
115 rs.
acquire(
reinterpret_cast<unsigned char *
>(color.bitmap),
reinterpret_cast<unsigned char *
>(depth.bitmap),
nullptr, pointcloud_color,
116 reinterpret_cast<unsigned char *
>(infrared.bitmap));
119 rs.
acquire(
reinterpret_cast<unsigned char *
>(color.bitmap),
reinterpret_cast<unsigned char *
>(depth.bitmap),
nullptr,
reinterpret_cast<unsigned char *
>(infrared.bitmap));
140 std::cout <<
"RealSense sensor characteristics: \n" << rs << std::endl;
143 std::cerr <<
"RealSense error " << e.
what() << std::endl;
145 catch (
const std::exception &e) {
146 std::cerr << e.what() << std::endl;
154 #if !defined(VISP_HAVE_REALSENSE2)
155 std::cout <<
"You do not have realsense2 SDK functionality enabled..." << std::endl;
156 std::cout <<
"Tip:" << std::endl;
157 std::cout <<
"- Install librealsense2, configure again ViSP using cmake 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)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index=-1) const
std::string getProductLine()
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
VISP_EXPORT double measureTimeMs()