45#include <visp3/core/vpMeterPixelConversion.h>
46#include <visp3/gui/vpDisplayGDI.h>
47#include <visp3/gui/vpDisplayX.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)) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
61 unsigned int confidence;
63 std::list<std::pair<unsigned int, vpImagePoint> >
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) {
84 if (rs2::frameset fs = frame.as<rs2::frameset>()) {
86 rs2::video_frame left_frame = fs.get_fisheye_frame(1);
87 size_t size = left_frame.get_width() * left_frame.get_height();
88 memcpy(I_left.
bitmap, left_frame.get_data(), size);
90 rs2::video_frame right_frame = fs.get_fisheye_frame(2);
91 size = right_frame.get_width() * right_frame.get_height();
92 memcpy(I_right.
bitmap, right_frame.get_data(), size);
94 rs2_pose pose_data = fs.get_pose_frame().get_pose_data();
97 static_cast<double>(pose_data.translation.y),
98 static_cast<double>(pose_data.translation.z));
99 vpQuaternionVector cqw(
static_cast<double>(pose_data.rotation.x),
static_cast<double>(pose_data.rotation.y),
100 static_cast<double>(pose_data.rotation.z),
static_cast<double>(pose_data.rotation.w));
105 odo_vel[0] =
static_cast<double>(pose_data.velocity.x);
106 odo_vel[1] =
static_cast<double>(pose_data.velocity.y);
107 odo_vel[2] =
static_cast<double>(pose_data.velocity.z);
108 odo_vel[3] =
static_cast<double>(pose_data.angular_velocity.x);
109 odo_vel[4] =
static_cast<double>(pose_data.angular_velocity.y);
110 odo_vel[5] =
static_cast<double>(pose_data.angular_velocity.z);
113 odo_acc[0] =
static_cast<double>(pose_data.acceleration.x);
114 odo_acc[1] =
static_cast<double>(pose_data.acceleration.y);
115 odo_acc[2] =
static_cast<double>(pose_data.acceleration.z);
116 odo_acc[3] =
static_cast<double>(pose_data.angular_acceleration.x);
117 odo_acc[4] =
static_cast<double>(pose_data.angular_acceleration.y);
118 odo_acc[5] =
static_cast<double>(pose_data.angular_acceleration.z);
120 confidence = pose_data.tracker_confidence;
123 rs2_pose pose_data = frame.as<rs2::pose_frame>().get_pose_data();
125 static_cast<double>(pose_data.translation.y),
126 static_cast<double>(pose_data.translation.z));
127 vpQuaternionVector cqw(
static_cast<double>(pose_data.rotation.x),
static_cast<double>(pose_data.rotation.y),
128 static_cast<double>(pose_data.rotation.z),
static_cast<double>(pose_data.rotation.w));
133 odo_vel[0] =
static_cast<double>(pose_data.velocity.x);
134 odo_vel[1] =
static_cast<double>(pose_data.velocity.y);
135 odo_vel[2] =
static_cast<double>(pose_data.velocity.z);
136 odo_vel[3] =
static_cast<double>(pose_data.angular_velocity.x);
137 odo_vel[4] =
static_cast<double>(pose_data.angular_velocity.y);
138 odo_vel[5] =
static_cast<double>(pose_data.angular_velocity.z);
141 odo_acc[0] =
static_cast<double>(pose_data.acceleration.x);
142 odo_acc[1] =
static_cast<double>(pose_data.acceleration.y);
143 odo_acc[2] =
static_cast<double>(pose_data.acceleration.z);
144 odo_acc[3] =
static_cast<double>(pose_data.angular_acceleration.x);
145 odo_acc[4] =
static_cast<double>(pose_data.angular_acceleration.y);
146 odo_acc[5] =
static_cast<double>(pose_data.angular_acceleration.z);
148 confidence = pose_data.tracker_confidence;
154 frame_origins.push_back(std::make_pair(confidence, frame_origin));
158 g.
open(config, callback);
164#if defined(VISP_HAVE_X11)
168#elif defined(VISP_HAVE_GDI)
174#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
177 display_left.
init(I_left, 10, 10,
"Left image");
178 display_right.
init(I_right,
static_cast<int>(I_left.
getWidth() / display_scale) + 80, 10,
"Right image");
179 display_pose.
init(I_pose, 10,
static_cast<int>(I_left.
getHeight() / display_scale) + 80,
186 frame_origins.push_back(std::make_pair(confidence, frame_origin));
190 std::this_thread::sleep_for(std::chrono::milliseconds(1));
198 frame_origins.push_back(std::make_pair(confidence, frame_origin));
209 std::list<std::pair<unsigned int, vpImagePoint> >::const_iterator it = frame_origins.begin();
210 std::pair<unsigned int, vpImagePoint> frame_origin_pair_prev = *(it++);
211 for (; it != frame_origins.end(); ++it) {
214 I_pose, frame_origin_pair_prev.second, (*it).second,
216 frame_origin_pair_prev = *it;
229 std::cerr <<
"RealSense error " << e.
what() << std::endl;
230 }
catch (
const std::exception &e) {
231 std::cerr << e.what() << std::endl;
239#if !defined(VISP_HAVE_REALSENSE2)
240 std::cout <<
"You do not realsense2 SDK functionality enabled..." << std::endl;
241 std::cout <<
"Tip:" << std::endl;
242 std::cout <<
"- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
244#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
245 std::cout <<
"You do not build ViSP with c++11 or higher compiler flag" << std::endl;
246 std::cout <<
"Tip:" << std::endl;
247 std::cout <<
"- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
248#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
249 std::cout <<
"You don't have X11 or GDI display capabilities" << std::endl;
250#elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
251 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 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), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
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
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)
Definition of the vpImage class member functions.
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.