Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-apriltag-detector-live-T265-realsense.cpp
1
2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpRealSense2.h>
5#endif
7#include <visp3/detection/vpDetectorAprilTag.h>
9#include <visp3/core/vpImageConvert.h>
10#include <visp3/core/vpImageTools.h>
11#include <visp3/core/vpMeterPixelConversion.h>
12#include <visp3/core/vpPixelMeterConversion.h>
13#include <visp3/core/vpXmlParserCamera.h>
14#include <visp3/gui/vpDisplayGDI.h>
15#include <visp3/gui/vpDisplayOpenCV.h>
16#include <visp3/gui/vpDisplayX.h>
17#include <visp3/vision/vpPose.h>
18
19int main(int argc, const char **argv)
20{
22 // Realsense T265 is only supported if realsense API > 2.31.0
23#if defined(VISP_HAVE_APRILTAG) && defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
25
28 double tagSize = 0.053;
29 float quad_decimate = 1.0;
30 int nThreads = 1;
31 bool display_tag = false;
32 int color_id = -1;
33 unsigned int thickness = 2;
34 bool align_frame = false;
35
36#if !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
37 bool display_off = true;
38 std::cout << "Warning: There is no 3rd party (X11, GDI or openCV) to dislay images..." << std::endl;
39#else
40 bool display_off = false;
41#endif
42
43 for (int i = 1; i < argc; i++) {
44 if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
45 poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
46 } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
47 tagSize = atof(argv[i + 1]);
48 } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
49 quad_decimate = (float)atof(argv[i + 1]);
50 } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
51 nThreads = atoi(argv[i + 1]);
52 } else if (std::string(argv[i]) == "--display_tag") {
53 display_tag = true;
54 } else if (std::string(argv[i]) == "--display_off") {
55 display_off = true;
56 } else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
57 color_id = atoi(argv[i + 1]);
58 } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
59 thickness = (unsigned int)atoi(argv[i + 1]);
60 } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
61 tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
62 } else if (std::string(argv[i]) == "--z_aligned") {
63 align_frame = true;
64 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
65 std::cout << "Usage: " << argv[0]
66 << " [--tag_size <tag_size in m> (default: 0.053)]"
67 " [--quad_decimate <quad_decimate> (default: 1)]"
68 " [--nthreads <nb> (default: 1)]"
69 " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
70 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
71 " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
72 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
73 " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
74 " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
75 " [--display_tag] [--z_aligned]";
76#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
77 std::cout << " [--display_off] [--color <color id>] [--thickness <line thickness>]";
78#endif
79 std::cout << " [--help]" << std::endl;
80 return EXIT_SUCCESS;
81 }
82 }
83
84 try {
86 std::cout << "Use Realsense 2 grabber" << std::endl;
88 rs2::config config;
89 unsigned int width = 848, height = 800;
90 config.disable_stream(RS2_STREAM_FISHEYE, 1);
91 config.disable_stream(RS2_STREAM_FISHEYE, 2);
92 config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8);
93 config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8);
94
95 vpImage<unsigned char> I_left(height, width);
96 vpImage<unsigned char> I_undist(height, width);
97
98 g.open(config);
99 g.acquire(&I_left, NULL, NULL);
100
101 std::cout << "Read camera parameters from Realsense device" << std::endl;
102 vpCameraParameters cam_left, cam_undistort;
104 cam_undistort.initPersProjWithoutDistortion(cam_left.get_px(), cam_left.get_py(), cam_left.get_u0(),
105 cam_left.get_v0());
107
108 std::cout << cam_left << std::endl;
109 std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
110 std::cout << "tagFamily: " << tagFamily << std::endl;
111 std::cout << "nThreads : " << nThreads << std::endl;
112 std::cout << "Z aligned: " << align_frame << std::endl;
113
114 vpDisplay *display_left = NULL;
115 vpDisplay *display_undistort = NULL;
116 if (!display_off) {
117#ifdef VISP_HAVE_X11
118 display_left = new vpDisplayX(I_left, 100, 30, "Left image");
119 display_undistort = new vpDisplayX(I_undist, I_left.getWidth(), 30, "Undistorted image");
120#elif defined(VISP_HAVE_GDI)
121 display_left = new vpDisplayGDI(I_left, 100, 30, "Left image");
122#elif defined(HAVE_OPENCV_HIGHGUI)
123 display_left = new vpDisplayOpenCV(I_left, 100, 30, "Left image");
124#endif
125 }
126
128 vpArray2D<int> mapU, mapV;
129 vpArray2D<float> mapDu, mapDv;
130 vpImageTools::initUndistortMap(cam_left, I_left.getWidth(), I_left.getHeight(), mapU, mapV, mapDu, mapDv);
132
134 vpDetectorAprilTag detector(tagFamily);
136
138 detector.setAprilTagQuadDecimate(quad_decimate);
139 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
140 detector.setAprilTagNbThreads(nThreads);
141 detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
142 detector.setZAlignedWithCameraAxis(align_frame);
144
145 std::vector<double> time_vec;
146
147 std::vector<std::vector<vpImagePoint> > tag_corners;
148
149 for (;;) {
150 double t = vpTime::measureTimeMs();
151
153 g.acquire(&I_left, NULL, NULL);
154
156 vpImageTools::undistort(I_left, mapU, mapV, mapDu, mapDv, I_undist);
158
160 vpDisplay::display(I_left);
161 vpDisplay::display(I_undist);
163
165 std::vector<vpHomogeneousMatrix> cMo_vec, cMo_vec1;
166 detector.detect(I_undist, tagSize, cam_undistort, cMo_vec);
168
169 // Display tag corners, bounding box and pose
170 for (size_t i = 0; i < cMo_vec.size(); i++) {
171 tag_corners = detector.getTagsCorners();
172 for (size_t j = 0; j < 4; j++) {
173 vpDisplay::displayCross(I_undist, tag_corners[i][j], 20, vpColor::green, 2);
174 }
175
176 vpDisplay::displayRectangle(I_undist, detector.getBBox(i), vpColor::yellow, false, 3);
177 vpDisplay::displayFrame(I_undist, cMo_vec[i], cam_undistort, tagSize / 2, vpColor::red, 3);
178 }
179
180 t = vpTime::measureTimeMs() - t;
181 time_vec.push_back(t);
182
183 std::stringstream ss;
184 ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
185 vpDisplay::displayText(I_left, 50, 20, ss.str(), vpColor::red);
186 vpDisplay::displayText(I_undist, 50, 20, ss.str(), vpColor::red);
187
188 if (vpDisplay::getClick(I_left, false) || vpDisplay::getClick(I_undist, false))
189 break;
190
191 vpDisplay::flush(I_left);
192 vpDisplay::flush(I_undist);
193 }
194
195 std::cout << "Benchmark loop processing time" << std::endl;
196 std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
197 << " ; " << vpMath::getMedian(time_vec) << " ms"
198 << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
199
200 if (!display_off) {
201 delete display_left;
202 delete display_undistort;
203 }
204
205 } catch (const vpException &e) {
206 std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
207 }
208
209 return EXIT_SUCCESS;
210#else
211 (void)argc;
212 (void)argv;
213#ifndef VISP_HAVE_APRILTAG
214 std::cout << "Enable Apriltag support, configure and build ViSP to run this tutorial" << std::endl;
215#elif defined(VISP_HAVE_REALSENSE2) && !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
216 std::cout << "Realsense T265 device needs librealsense API > 2.31.0. ViSP is linked with librealsense API "
217 << RS2_API_VERSION_STR << ". You need to upgrade librealsense to use this example." << std::endl;
218#else
219 std::cout << "Install librealsense 3rd party, configure and build ViSP again to use this example." << std::endl;
220#endif
221#endif
222 return EXIT_SUCCESS;
223}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
static vpColor getColor(const unsigned int &i)
Definition vpColor.h:307
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
static const vpColor yellow
Definition vpColor.h:219
static const vpColor green
Definition vpColor.h:214
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
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
Class that defines generic functionalities for display.
Definition vpDisplay.h:173
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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
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
static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height, vpArray2D< int > &mapU, vpArray2D< int > &mapV, vpArray2D< float > &mapDu, vpArray2D< float > &mapDv)
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double getMedian(const std::vector< double > &v)
Definition vpMath.cpp:314
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition vpMath.cpp:345
static double getMean(const std::vector< double > &v)
Definition vpMath.cpp:294
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()