Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
tutorial-apriltag-detector.cpp
1 #include <visp3/detection/vpDetectorAprilTag.h>
5 #include <visp3/gui/vpDisplayGDI.h>
6 #include <visp3/gui/vpDisplayOpenCV.h>
7 #include <visp3/gui/vpDisplayX.h>
8 #include <visp3/io/vpImageIo.h>
9 #include <visp3/core/vpXmlParserCamera.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))
15 
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 {
88  vpImageIo::read(I, input_filename);
89 
90 #ifdef VISP_HAVE_X11
91  vpDisplayX d(I);
92 #elif defined(VISP_HAVE_GDI)
93  vpDisplayGDI d(I);
94 #elif defined(VISP_HAVE_OPENCV)
95  vpDisplayOpenCV d(I);
96 #endif
97 
99  vpDetectorAprilTag detector(tagFamily);
101 
103  detector.setAprilTagQuadDecimate(quad_decimate);
104  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
105  detector.setAprilTagNbThreads(nThreads);
106  detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
107  detector.setZAlignedWithCameraAxis(z_aligned);
109 
111 
112  double t = vpTime::measureTimeMs();
114  std::vector<vpHomogeneousMatrix> cMo_vec;
115  detector.detect(I, tagSize, cam, cMo_vec);
117  t = vpTime::measureTimeMs() - t;
118 
119  std::stringstream ss;
120  ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
121  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
122 
124  for (size_t i = 0; i < detector.getNbObjects(); i++) {
127  std::vector<vpImagePoint> p = detector.getPolygon(i);
128  vpRect bbox = detector.getBBox(i);
132  std::string message = detector.getMessage(i);
135  std::size_t tag_id_pos = message.find("id: ");
136  if (tag_id_pos != std::string::npos) {
137  int tag_id = atoi(message.substr(tag_id_pos + 4).c_str());
138  ss.str("");
139  ss << "Tag id: " << tag_id;
140  vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), ss.str(), vpColor::red);
141  }
143  for (size_t j = 0; j < p.size(); j++) {
144  vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
145  std::ostringstream number;
146  number << j;
147  vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), vpColor::blue);
148  }
149  }
150 
151  vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
152  vpDisplay::flush(I);
154 
156 
158  for (size_t i = 0; i < cMo_vec.size(); i++) {
159  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
160  }
162 
163  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
164  vpDisplay::flush(I);
166  } catch (const vpException &e) {
167  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
168  }
169 
170  return EXIT_SUCCESS;
171 #else
172  (void)argc;
173  (void)argv;
174  return 0;
175 #endif
176 }
vpRect getBBox(size_t i) const
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
double getTop() const
Definition: vpRect.h:193
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:134
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:71
size_t getNbObjects() const
const char * getMessage() const
Definition: vpException.cpp:90
XML parser to load and save intrinsic camera parameters.
static const vpColor green
Definition: vpColor.h:220
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static const vpColor red
Definition: vpColor.h:217
void setAprilTagQuadDecimate(float quadDecimate)
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
void setAprilTagNbThreads(int nThreads)
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)
double getLeft() const
Definition: vpRect.h:174
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)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
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))
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
Defines a rectangle in the plane.
Definition: vpRect.h:79
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
static vpColor getColor(const unsigned int &i)
Definition: vpColor.h:310
std::vector< std::string > & getMessage()
static const vpColor blue
Definition: vpColor.h:223
std::vector< std::vector< vpImagePoint > > & getPolygon()
bool detect(const vpImage< unsigned char > &I)