45#include <visp3/core/vpMeterPixelConversion.h>
46#include <visp3/gui/vpDisplayX.h>
47#include <visp3/gui/vpDisplayGDI.h>
48#include <visp3/sensor/vpRealSense2.h>
50#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
51 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && \
52 (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
62 unsigned int confidence;
64 std::list< std::pair<unsigned int, vpImagePoint> > frame_origins;
65 unsigned int display_scale = 2;
71 config.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
72 config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
73 config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
83 std::function<void(rs2::frame)> callback = [&](
const rs2::frame &frame)
85 if (rs2::frameset fs = frame.as<rs2::frameset>())
88 rs2::video_frame left_frame = fs.get_fisheye_frame(1);
89 size_t size = left_frame.get_width() * left_frame.get_height();
90 memcpy(I_left.
bitmap, left_frame.get_data(), size);
92 rs2::video_frame right_frame = fs.get_fisheye_frame(2);
93 size = right_frame.get_width() * right_frame.get_height();
94 memcpy(I_right.
bitmap, right_frame.get_data(), size);
96 rs2_pose pose_data = fs.get_pose_frame().get_pose_data();
99 static_cast<double>(pose_data.translation.y),
100 static_cast<double>(pose_data.translation.z));
102 static_cast<double>(pose_data.rotation.y),
103 static_cast<double>(pose_data.rotation.z),
104 static_cast<double>(pose_data.rotation.w));
109 odo_vel[0] =
static_cast<double>(pose_data.velocity.x);
110 odo_vel[1] =
static_cast<double>(pose_data.velocity.y);
111 odo_vel[2] =
static_cast<double>(pose_data.velocity.z);
112 odo_vel[3] =
static_cast<double>(pose_data.angular_velocity.x);
113 odo_vel[4] =
static_cast<double>(pose_data.angular_velocity.y);
114 odo_vel[5] =
static_cast<double>(pose_data.angular_velocity.z);
117 odo_acc[0] =
static_cast<double>(pose_data.acceleration.x);
118 odo_acc[1] =
static_cast<double>(pose_data.acceleration.y);
119 odo_acc[2] =
static_cast<double>(pose_data.acceleration.z);
120 odo_acc[3] =
static_cast<double>(pose_data.angular_acceleration.x);
121 odo_acc[4] =
static_cast<double>(pose_data.angular_acceleration.y);
122 odo_acc[5] =
static_cast<double>(pose_data.angular_acceleration.z);
124 confidence = pose_data.tracker_confidence;
129 rs2_pose pose_data = frame.as<rs2::pose_frame>().get_pose_data();
131 static_cast<double>(pose_data.translation.y),
132 static_cast<double>(pose_data.translation.z));
134 static_cast<double>(pose_data.rotation.y),
135 static_cast<double>(pose_data.rotation.z),
136 static_cast<double>(pose_data.rotation.w));
141 odo_vel[0] =
static_cast<double>(pose_data.velocity.x);
142 odo_vel[1] =
static_cast<double>(pose_data.velocity.y);
143 odo_vel[2] =
static_cast<double>(pose_data.velocity.z);
144 odo_vel[3] =
static_cast<double>(pose_data.angular_velocity.x);
145 odo_vel[4] =
static_cast<double>(pose_data.angular_velocity.y);
146 odo_vel[5] =
static_cast<double>(pose_data.angular_velocity.z);
149 odo_acc[0] =
static_cast<double>(pose_data.acceleration.x);
150 odo_acc[1] =
static_cast<double>(pose_data.acceleration.y);
151 odo_acc[2] =
static_cast<double>(pose_data.acceleration.z);
152 odo_acc[3] =
static_cast<double>(pose_data.angular_acceleration.x);
153 odo_acc[4] =
static_cast<double>(pose_data.angular_acceleration.y);
154 odo_acc[5] =
static_cast<double>(pose_data.angular_acceleration.z);
156 confidence = pose_data.tracker_confidence;
162 frame_origins.push_back(std::make_pair(confidence, frame_origin));
166 g.
open(config, callback);
174#if defined(VISP_HAVE_X11)
178#elif defined(VISP_HAVE_GDI)
184#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
187 display_left.
init(I_left, 10, 10,
"Left image");
188 display_right.
init(I_right,
static_cast<int>(I_left.
getWidth()/display_scale) + 80, 10,
"Right image");
189 display_pose.
init(I_pose, 10,
static_cast<int>(I_left.
getHeight()/display_scale) + 80,
"Pose visualizer");
194 frame_origins.push_back(std::make_pair(confidence, frame_origin));
198 std::this_thread::sleep_for(std::chrono::milliseconds(1));
206 frame_origins.push_back(std::make_pair(confidence, frame_origin));
217 std::list< std::pair<unsigned int, vpImagePoint> >::const_iterator it = frame_origins.begin();
218 std::pair<unsigned int, vpImagePoint> frame_origin_pair_prev = *(it++);
219 for (; it != frame_origins.end(); ++it) {
223 frame_origin_pair_prev = *it;
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;
250#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
251 std::cout <<
"You do not build ViSP with c++11 or higher compiler flag" << std::endl;
252 std::cout <<
"Tip:" << std::endl;
253 std::cout <<
"- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
254#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
255 std::cout <<
"You don't have X11 or GDI display capabilities" << std::endl;
256#elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
257 std::cout <<
"Install librealsense version > 2.31.0" << std::endl;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static const vpColor none
static const vpColor yellow
static const vpColor green
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
virtual void setDownScalingFactor(unsigned int scale)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
const char * what() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Implementation of a rotation vector as quaternion angle minimal representation.
bool open(const rs2::config &cfg=rs2::config())
rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index=-1) const
Class that consider the case of a translation vector.