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