Visual Servoing Platform  version 3.6.1 under development (2024-05-27)
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  }
34  else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
35  tagSize = atof(argv[i + 1]);
36  }
37  else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
38  input_filename = std::string(argv[i + 1]);
39  }
40  else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
41  quad_decimate = (float)atof(argv[i + 1]);
42  }
43  else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
44  nThreads = atoi(argv[i + 1]);
45  }
46  else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
47  intrinsic_file = std::string(argv[i + 1]);
48  }
49  else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
50  camera_name = std::string(argv[i + 1]);
51  }
52  else if (std::string(argv[i]) == "--display_tag") {
53  display_tag = true;
54  }
55  else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
56  color_id = atoi(argv[i + 1]);
57  }
58  else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
59  thickness = (unsigned int)atoi(argv[i + 1]);
60  }
61  else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
62  tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
63  }
64  else if (std::string(argv[i]) == "--z_aligned") {
65  z_aligned = true;
66  }
67  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
68  std::cout << "Usage: " << argv[0]
69  << " [--input <input file>] [--tag_size <tag_size in m>]"
70  " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
71  " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
72  " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
73  " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
74  " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
75  " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
76  " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
77  " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
78  " [--display_tag] [--color <color_id (0, 1, ...)>]"
79  " [--thickness <thickness>] [--z_aligned]"
80  " [--help]"
81  << std::endl;
82  return EXIT_SUCCESS;
83  }
84  }
85 
87  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
88 #if defined(VISP_HAVE_PUGIXML)
89  vpXmlParserCamera parser;
90  if (!intrinsic_file.empty() && !camera_name.empty()) {
91  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
92  }
93 #endif
94 
95  std::cout << cam << std::endl;
96  std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
97  std::cout << "tagFamily: " << tagFamily << std::endl;
98  std::cout << "nThreads : " << nThreads << std::endl;
99  std::cout << "Z aligned: " << z_aligned << std::endl;
100 
101  try {
102  vpImage<vpRGBa> I_color;
103  vpImageIo::read(I_color, input_filename);
105  vpImageConvert::convert(I_color, I);
106 
107 #ifdef VISP_HAVE_X11
108  vpDisplayX d(I);
109 #elif defined(VISP_HAVE_GDI)
110  vpDisplayGDI d(I);
111 #elif defined(HAVE_OPENCV_HIGHGUI)
112  vpDisplayOpenCV d(I);
113 #endif
114 
116  vpDetectorAprilTag detector(tagFamily);
118 
120  detector.setAprilTagQuadDecimate(quad_decimate);
121  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
122  detector.setAprilTagNbThreads(nThreads);
123  detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
124  detector.setZAlignedWithCameraAxis(z_aligned);
126 
128 
129  double t = vpTime::measureTimeMs();
131  std::vector<vpHomogeneousMatrix> cMo_vec;
132  detector.detect(I, tagSize, cam, cMo_vec);
134  t = vpTime::measureTimeMs() - t;
135 
136  std::stringstream ss;
137  ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
138  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
139 
141  for (size_t i = 0; i < detector.getNbObjects(); i++) {
144  std::vector<vpImagePoint> p = detector.getPolygon(i);
145  vpRect bbox = detector.getBBox(i);
149  std::string message = detector.getMessage(i);
152  std::size_t tag_id_pos = message.find("id: ");
153  if (tag_id_pos != std::string::npos) {
154  int tag_id = atoi(message.substr(tag_id_pos + 4).c_str());
155  ss.str("");
156  ss << "Tag id: " << tag_id;
157  vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), ss.str(), vpColor::red);
158  }
160  for (size_t j = 0; j < p.size(); j++) {
161  vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
162  std::ostringstream number;
163  number << j;
164  vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), vpColor::blue);
165  }
166  }
167 
168  vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
169  vpDisplay::flush(I);
171 
173 
175  for (size_t i = 0; i < cMo_vec.size(); i++) {
176  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
177  }
179 
180  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
181  vpDisplay::flush(I);
183 
184 #ifdef VISP_HAVE_X11
185  vpDisplayX d2(I_color, 50, 50);
186 #elif defined(VISP_HAVE_GDI)
187  vpDisplayGDI d2(I_color, 50, 50);
188 #elif defined(HAVE_OPENCV_HIGHGUI)
189  vpDisplayOpenCV d2(I_color, 50, 50);
190 #endif
191  // To test the displays on a vpRGBa image
192  vpDisplay::display(I_color);
193 
194  // Display frames and tags but remove the display of the last element
195  std::vector<std::vector<vpImagePoint> > tagsCorners = detector.getTagsCorners();
196  tagsCorners.pop_back();
197  detector.displayTags(I_color, tagsCorners, vpColor::none, 3);
198 
199  cMo_vec.pop_back();
200  detector.displayFrames(I_color, cMo_vec, cam, tagSize / 2, vpColor::none, 3);
201 
202  vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
203  vpDisplay::flush(I_color);
204  vpDisplay::getClick(I_color);
205  }
206  catch (const vpException &e) {
207  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
208  }
209 
210 #else
211  (void)argc;
212  (void)argv;
213 #endif
214  return EXIT_SUCCESS;
215 }
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
Perspective projection without 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 blue
Definition: vpColor.h:217
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
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 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:143
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
Defines a rectangle in the plane.
Definition: vpRect.h:76
double getLeft() const
Definition: vpRect.h:170
double getTop() const
Definition: vpRect.h:189
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, bool verbose=true)
VISP_EXPORT double measureTimeMs()