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) && defined(VISP_HAVE_THREADS) \
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;
124 rs2_pose pose_data = frame.as<rs2::pose_frame>().get_pose_data();
126 static_cast<double>(pose_data.translation.y),
127 static_cast<double>(pose_data.translation.z));
128 vpQuaternionVector cqw(
static_cast<double>(pose_data.rotation.x),
static_cast<double>(pose_data.rotation.y),
129 static_cast<double>(pose_data.rotation.z),
static_cast<double>(pose_data.rotation.w));
134 odo_vel[0] =
static_cast<double>(pose_data.velocity.x);
135 odo_vel[1] =
static_cast<double>(pose_data.velocity.y);
136 odo_vel[2] =
static_cast<double>(pose_data.velocity.z);
137 odo_vel[3] =
static_cast<double>(pose_data.angular_velocity.x);
138 odo_vel[4] =
static_cast<double>(pose_data.angular_velocity.y);
139 odo_vel[5] =
static_cast<double>(pose_data.angular_velocity.z);
142 odo_acc[0] =
static_cast<double>(pose_data.acceleration.x);
143 odo_acc[1] =
static_cast<double>(pose_data.acceleration.y);
144 odo_acc[2] =
static_cast<double>(pose_data.acceleration.z);
145 odo_acc[3] =
static_cast<double>(pose_data.angular_acceleration.x);
146 odo_acc[4] =
static_cast<double>(pose_data.angular_acceleration.y);
147 odo_acc[5] =
static_cast<double>(pose_data.angular_acceleration.z);
149 confidence = pose_data.tracker_confidence;
155 frame_origins.push_back(std::make_pair(confidence, frame_origin));
159 g.
open(config, callback);
165 #if defined(VISP_HAVE_X11)
169 #elif defined(VISP_HAVE_GDI)
175 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
178 display_left.
init(I_left, 10, 10,
"Left image");
179 display_right.
init(I_right,
static_cast<int>(I_left.
getWidth() / display_scale) + 80, 10,
"Right image");
180 display_pose.
init(I_pose, 10,
static_cast<int>(I_left.
getHeight() / display_scale) + 80,
187 frame_origins.push_back(std::make_pair(confidence, frame_origin));
191 std::this_thread::sleep_for(std::chrono::milliseconds(1));
199 frame_origins.push_back(std::make_pair(confidence, frame_origin));
210 std::list<std::pair<unsigned int, vpImagePoint> >::const_iterator it = frame_origins.begin();
211 std::pair<unsigned int, vpImagePoint> frame_origin_pair_prev = *(it++);
212 for (; it != frame_origins.end(); ++it) {
215 I_pose, frame_origin_pair_prev.second, (*it).second,
217 frame_origin_pair_prev = *it;
231 std::cerr <<
"RealSense error " << e.
what() << std::endl;
233 catch (
const std::exception &e) {
234 std::cerr << e.what() << std::endl;
242 #if !defined(VISP_HAVE_REALSENSE2)
243 std::cout <<
"You do not realsense2 SDK functionality enabled..." << std::endl;
244 std::cout <<
"Tip:" << std::endl;
245 std::cout <<
"- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
247 #elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
248 std::cout <<
"You don't have X11 or GDI display capabilities" << std::endl;
249 #elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
250 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="") vp_override
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)
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.