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;
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Implementation of an homogeneous matrix and operations on such kind of matrices.
Type * bitmap
points toward the bitmap
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
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...
static const vpColor none
error that can be emited by ViSP classes.
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
bool open(const rs2::config &cfg=rs2::config())
const char * what() const
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static void display(const vpImage< unsigned char > &I)
void setDownScalingFactor(unsigned int scale)
Generic class defining intrinsic camera parameters.
Implementation of a rotation vector as quaternion angle minimal representation.
rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index=-1) const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void resize(unsigned int i, bool flagNullify=true)
Implementation of column vector and the associated operations.
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))
vpHomogeneousMatrix inverse() const
unsigned int getHeight() const
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static const vpColor yellow
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Class that consider the case of a translation vector.