Visual Servoing Platform  version 3.5.1 under development (2023-02-03)
tutorial-apriltag-detector.cpp
1 #include <visp3/detection/vpDetectorAprilTag.h>
5 #include <visp3/core/vpXmlParserCamera.h>
6 #include <visp3/gui/vpDisplayGDI.h>
7 #include <visp3/gui/vpDisplayOpenCV.h>
8 #include <visp3/gui/vpDisplayX.h>
9 #include <visp3/io/vpImageIo.h>
10 
11 int main(int argc, const char **argv)
12 {
14 #if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
16 
17  std::string input_filename = "AprilTag.pgm";
20  double tagSize = 0.053;
21  float quad_decimate = 1.0;
22  int nThreads = 1;
23  std::string intrinsic_file = "";
24  std::string camera_name = "";
25  bool display_tag = false;
26  int color_id = -1;
27  unsigned int thickness = 2;
28  bool z_aligned = false;
29 
30  for (int i = 1; i < argc; i++) {
31  if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
32  poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
33  } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
34  tagSize = atof(argv[i + 1]);
35  } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
36  input_filename = std::string(argv[i + 1]);
37  } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
38  quad_decimate = (float)atof(argv[i + 1]);
39  } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
40  nThreads = atoi(argv[i + 1]);
41  } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
42  intrinsic_file = std::string(argv[i + 1]);
43  } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
44  camera_name = std::string(argv[i + 1]);
45  } else if (std::string(argv[i]) == "--display_tag") {
46  display_tag = true;
47  } else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
48  color_id = atoi(argv[i + 1]);
49  } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
50  thickness = (unsigned int)atoi(argv[i + 1]);
51  } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
52  tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
53  } else if (std::string(argv[i]) == "--z_aligned") {
54  z_aligned = true;
55  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
56  std::cout << "Usage: " << argv[0]
57  << " [--input <input file>] [--tag_size <tag_size in m>]"
58  " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
59  " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
60  " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
61  " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
62  " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
63  " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
64  " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
65  " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
66  " [--display_tag] [--color <color_id (0, 1, ...)>]"
67  " [--thickness <thickness>] [--z_aligned]"
68  " [--help]"
69  << std::endl;
70  return EXIT_SUCCESS;
71  }
72  }
73 
75  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
76  vpXmlParserCamera parser;
77  if (!intrinsic_file.empty() && !camera_name.empty())
78  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
79 
80  std::cout << cam << std::endl;
81  std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
82  std::cout << "tagFamily: " << tagFamily << std::endl;
83  std::cout << "nThreads : " << nThreads << std::endl;
84  std::cout << "Z aligned: " << z_aligned << std::endl;
85 
86  try {
87  vpImage<vpRGBa> I_color;
88  vpImageIo::read(I_color, input_filename);
90  vpImageConvert::convert(I_color, I);
91 
92 #ifdef VISP_HAVE_X11
93  vpDisplayX d(I);
94 #elif defined(VISP_HAVE_GDI)
95  vpDisplayGDI d(I);
96 #elif defined(VISP_HAVE_OPENCV)
97  vpDisplayOpenCV d(I);
98 #endif
99 
101  vpDetectorAprilTag detector(tagFamily);
103 
105  detector.setAprilTagQuadDecimate(quad_decimate);
106  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
107  detector.setAprilTagNbThreads(nThreads);
108  detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
109  detector.setZAlignedWithCameraAxis(z_aligned);
111 
113 
114  double t = vpTime::measureTimeMs();
116  std::vector<vpHomogeneousMatrix> cMo_vec;
117  detector.detect(I, tagSize, cam, cMo_vec);
119  t = vpTime::measureTimeMs() - t;
120 
121  std::stringstream ss;
122  ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
123  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
124 
126  for (size_t i = 0; i < detector.getNbObjects(); i++) {
129  std::vector<vpImagePoint> p = detector.getPolygon(i);
130  vpRect bbox = detector.getBBox(i);
134  std::string message = detector.getMessage(i);
137  std::size_t tag_id_pos = message.find("id: ");
138  if (tag_id_pos != std::string::npos) {
139  int tag_id = atoi(message.substr(tag_id_pos + 4).c_str());
140  ss.str("");
141  ss << "Tag id: " << tag_id;
142  vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), ss.str(), vpColor::red);
143  }
145  for (size_t j = 0; j < p.size(); j++) {
146  vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
147  std::ostringstream number;
148  number << j;
149  vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), vpColor::blue);
150  }
151  }
152 
153  vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
154  vpDisplay::flush(I);
156 
158 
160  for (size_t i = 0; i < cMo_vec.size(); i++) {
161  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
162  }
164 
165  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
166  vpDisplay::flush(I);
168 
169 #ifdef VISP_HAVE_X11
170  vpDisplayX d2(I_color, 50, 50);
171 #elif defined(VISP_HAVE_GDI)
172  vpDisplayGDI d2(I_color, 50, 50);
173 #elif defined(VISP_HAVE_OPENCV)
174  vpDisplayOpenCV d2(I_color, 50, 50);
175 #endif
176  // To test the displays on a vpRGBa image
177  vpDisplay::display(I_color);
178 
179  // Display frames and tags but remove the display of the last element
180  std::vector<std::vector<vpImagePoint> > tagsCorners = detector.getTagsCorners();
181  tagsCorners.pop_back();
182  detector.displayTags(I_color, tagsCorners, vpColor::none, 3);
183 
184  cMo_vec.pop_back();
185  detector.displayFrames(I_color, cMo_vec, cam, tagSize / 2, vpColor::none, 3);
186 
187  vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
188  vpDisplay::flush(I_color);
189  vpDisplay::getClick(I_color);
190  } catch (const vpException &e) {
191  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
192  }
193 
194  return EXIT_SUCCESS;
195 #else
196  (void)argc;
197  (void)argv;
198  return 0;
199 #endif
200 }
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static vpColor getColor(const unsigned int &i)
Definition: vpColor.h:313
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
static const vpColor blue
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:220
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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:135
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
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 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))
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 emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:87
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:148
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getLeft() const
Definition: vpRect.h:174
double getTop() const
Definition: vpRect.h:193
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
VISP_EXPORT double measureTimeMs()