Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-pose-from-points-realsense-T265.cpp
1
2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpRealSense2.h>
5#endif
6#include <visp3/gui/vpDisplayGDI.h>
7#include <visp3/gui/vpDisplayOpenCV.h>
8#include <visp3/gui/vpDisplayX.h>
9
10#include "pose_helper.h"
11
12int main(int argc, char **argv)
13{
14#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
15 defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
16 try {
17 double opt_square_width = 0.12;
18 int opt_camera_index = 1; // camera index: 1. Left, 2.Right
19
20 for (int i = 0; i < argc; i++) {
21 if (std::string(argv[i]) == "--camera_index" && i + 1 < argc) {
22 opt_camera_index = atoi(argv[i + 1]);
23 } else if (std::string(argv[i]) == "--square_width" && i + 1 < argc) {
24 opt_square_width = atoi(argv[i + 1]);
25 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
26 std::cout << "\nUsage: " << argv[0] << " [--camera_index <1.Left | 2.Right> (default: 1)]"
27 << " [--square_width <square width in meter (default: 0.12)] [--help] [-h]\n"
28 << "\nExample using right camera and square size 0.1:\n"
29 << " " << argv[0] << "--camera_index 2 --square_width 0.1\n"
30 << std::endl;
31 return EXIT_SUCCESS;
32 }
33 }
34
36
38 std::cout << "Use Realsense 2 grabber" << std::endl;
40 rs2::config config;
41 config.enable_stream(RS2_STREAM_FISHEYE, 1);
42 config.enable_stream(RS2_STREAM_FISHEYE, 2);
43
44 g.open(config);
45 if (opt_camera_index == 1) // Left camera
46 g.acquire(&I, NULL, NULL);
47 else
48 g.acquire(NULL, &I, NULL);
49
50 std::cout << "Read camera parameters from Realsense device" << std::endl;
51 // Parameters of our camera
53 RS2_STREAM_FISHEYE, vpCameraParameters::ProjWithKannalaBrandtDistortion, opt_camera_index);
55
56 std::cout << "Square width : " << opt_square_width << std::endl;
57 std::cout << cam << std::endl;
58
59 // The pose container
61
62 std::vector<vpDot2> dot(4);
63 std::vector<vpPoint> point; // 3D coordinates of the points
64 std::vector<vpImagePoint> ip; // 2D coordinates of the points in pixels
65 double L = opt_square_width / 2.;
66 point.push_back(vpPoint(-L, -L, 0));
67 point.push_back(vpPoint(L, -L, 0));
68 point.push_back(vpPoint(L, L, 0));
69 point.push_back(vpPoint(-L, L, 0));
70
71#if defined(VISP_HAVE_X11)
72 vpDisplayX d(I);
73#elif defined(VISP_HAVE_GDI)
74 vpDisplayGDI d(I);
75#elif defined(HAVE_OPENCV_HIGHGUI)
76 vpDisplayOpenCV d(I);
77#endif
78
79 bool quit = false;
80 bool apply_cv = false; // apply computer vision
81 bool init_cv = true; // initialize tracking and pose computation
82
83 while (!quit) {
84 double t_begin = vpTime::measureTimeMs();
85
86 if (opt_camera_index == 1)
87 g.acquire(&I, NULL, NULL);
88 else
89 g.acquire(NULL, &I, NULL);
90
92 if (apply_cv) {
93 try {
94 ip = track(I, dot, init_cv);
95 computePose(point, ip, cam, init_cv, cMo);
96 vpDisplay::displayFrame(I, cMo, cam, opt_square_width, vpColor::none, 3);
97 if (init_cv)
98 init_cv = false; // turn off the computer vision initialisation specific stuff
99
100 { // Display estimated pose in [m] and [deg]
101 vpPoseVector pose(cMo);
102 std::stringstream ss;
103 ss << "Translation: " << std::setprecision(5) << pose[0] << " " << pose[1] << " " << pose[2] << " [m]";
104 vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
105 ss.str(""); // erase ss
106 ss << "Rotation tu: " << std::setprecision(4) << vpMath::deg(pose[3]) << " " << vpMath::deg(pose[4]) << " "
107 << vpMath::deg(pose[5]) << " [deg]";
108 vpDisplay::displayText(I, 80, 20, ss.str(), vpColor::red);
109 }
110 } catch (...) {
111 std::cout << "Computer vision failure." << std::endl;
112 apply_cv = false;
113 init_cv = true;
114 }
115 }
116 vpDisplay::displayText(I, 20, 20, "Right click: quit", vpColor::red);
117 if (apply_cv) {
118 vpDisplay::displayText(I, 40, 20, "Computer vision in progress...", vpColor::red);
119 } else {
120 vpDisplay::displayText(I, 40, 20, "Left click : start", vpColor::red);
121 }
123 if (vpDisplay::getClick(I, button, false)) {
124 if (button == vpMouseButton::button3) {
125 quit = true;
126 } else if (button == vpMouseButton::button1) {
127 apply_cv = true;
128 }
129 }
130 {
131 std::stringstream ss;
132 ss << "Time: " << vpTime::measureTimeMs() - t_begin << " ms";
133 vpDisplay::displayText(I, 20, I.getWidth() - 100, ss.str(), vpColor::red);
134 }
136 }
137 } catch (const vpException &e) {
138 std::cout << "Catch an exception: " << e.getMessage() << std::endl;
139 }
140#elif !defined(VISP_HAVE_REALSENSE2)
141 (void)argc;
142 (void)argv;
143 std::cout << "You do not realsense2 SDK functionality enabled..." << std::endl;
144 std::cout << "Tip:" << std::endl;
145 std::cout << "- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
146 return EXIT_SUCCESS;
147#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
148 (void)argc;
149 (void)argv;
150 std::cout << "You do not build ViSP with c++11 or higher compiler flag" << std::endl;
151 std::cout << "Tip:" << std::endl;
152 std::cout << "- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
153#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
154 (void)argc;
155 (void)argv;
156 std::cout << "Install a 3rd party dedicated to image display (X11, GDI, OpenCV), configure and build ViSP again to "
157 "use this example"
158 << std::endl;
159#elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
160 (void)argc;
161 (void)argv;
162 std::cout << "Install librealsense version > 2.31.0" << std::endl;
163#endif
164}
Generic class defining intrinsic camera parameters.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(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), 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.
Definition vpException.h:59
const char * getMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
static double deg(double rad)
Definition vpMath.h:106
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
Implementation of a pose vector and operations on poses.
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()