Visual Servoing Platform  version 3.3.1 under development (2020-08-12)
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 #ifdef VISP_HAVE_PUGIXML
10 #include <visp3/core/vpXmlParserCamera.h>
11 #endif
12 
13 int main(int argc, const char **argv)
14 {
16 #if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
17 
19  std::string input_filename = "AprilTag.pgm";
22  double tagSize = 0.053;
23  float quad_decimate = 1.0;
24  int nThreads = 1;
25  std::string intrinsic_file = "";
26  std::string camera_name = "";
27  bool display_tag = false;
28  int color_id = -1;
29  unsigned int thickness = 2;
30  bool z_aligned = false;
31 
32  for (int i = 1; i < argc; i++) {
33  if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
34  poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
35  } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
36  tagSize = atof(argv[i + 1]);
37  } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
38  input_filename = std::string(argv[i + 1]);
39  } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
40  quad_decimate = (float)atof(argv[i + 1]);
41  } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
42  nThreads = atoi(argv[i + 1]);
43  } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
44  intrinsic_file = std::string(argv[i + 1]);
45  } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
46  camera_name = std::string(argv[i + 1]);
47  } else if (std::string(argv[i]) == "--display_tag") {
48  display_tag = true;
49  } else if (std::string(argv[i]) == "--color" && i + 1 < argc) {
50  color_id = atoi(argv[i + 1]);
51  } else if (std::string(argv[i]) == "--thickness" && i + 1 < argc) {
52  thickness = (unsigned int)atoi(argv[i + 1]);
53  } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
54  tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
55  } else if (std::string(argv[i]) == "--z_aligned") {
56  z_aligned = true;
57  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
58  std::cout << "Usage: " << argv[0]
59  << " [--input <input file>] [--tag_size <tag_size in m>]"
60  " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
61  " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
62  " [--pose_method <method> (0: HOMOGRAPHY, 1: HOMOGRAPHY_VIRTUAL_VS, "
63  " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS, "
64  " 4: BEST_RESIDUAL_VIRTUAL_VS, 5: HOMOGRAPHY_ORTHOGONAL_ITERATION) (default: 0)]"
65  " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10 (DEPRECATED), 2: TAG_36ARTOOLKIT (DEPRECATED),"
66  " 3: TAG_25h9, 4: TAG_25h7 (DEPRECATED), 5: TAG_16h5, 6: TAG_CIRCLE21h7, 7: TAG_CIRCLE49h12,"
67  " 8: TAG_CUSTOM48h12, 9: TAG_STANDARD41h12, 10: TAG_STANDARD52h13) (default: 0)]"
68  " [--display_tag] [--color <color_id (0, 1, ...)>]"
69  " [--thickness <thickness>] [--z_aligned]"
70  " [--help]"
71  << std::endl;
72  return EXIT_SUCCESS;
73  }
74  }
75 
77  cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
78 #ifdef VISP_HAVE_PUGIXML
79  vpXmlParserCamera parser;
80  if (!intrinsic_file.empty() && !camera_name.empty())
81  parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
82 #endif
83  std::cout << cam << std::endl;
84  std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
85  std::cout << "tagFamily: " << tagFamily << std::endl;
86  std::cout << "nThreads : " << nThreads << std::endl;
87  std::cout << "Z aligned: " << z_aligned << std::endl;
88 
89  try {
91  vpImageIo::read(I, input_filename);
92 
93 #ifdef VISP_HAVE_X11
94  vpDisplayX d(I);
95 #elif defined(VISP_HAVE_GDI)
96  vpDisplayGDI d(I);
97 #elif defined(VISP_HAVE_OPENCV)
98  vpDisplayOpenCV d(I);
99 #endif
100 
102  vpDetectorAprilTag detector(tagFamily);
104 
106  detector.setAprilTagQuadDecimate(quad_decimate);
107  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
108  detector.setAprilTagNbThreads(nThreads);
109  detector.setDisplayTag(display_tag, color_id < 0 ? vpColor::none : vpColor::getColor(color_id), thickness);
110  detector.setZAlignedWithCameraAxis(z_aligned);
112 
114 
115  double t = vpTime::measureTimeMs();
117  std::vector<vpHomogeneousMatrix> cMo_vec;
118  detector.detect(I, tagSize, cam, cMo_vec);
120  t = vpTime::measureTimeMs() - t;
121 
122  std::stringstream ss;
123  ss << "Detection time: " << t << " ms for " << detector.getNbObjects() << " tags";
124  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
125 
127  for (size_t i = 0; i < detector.getNbObjects(); i++) {
130  std::vector<vpImagePoint> p = detector.getPolygon(i);
131  vpRect bbox = detector.getBBox(i);
135  std::string message = detector.getMessage(i);
138  std::size_t tag_id_pos = message.find("id: ");
139  if (tag_id_pos != std::string::npos) {
140  int tag_id = atoi(message.substr(tag_id_pos + 4).c_str());
141  ss.str("");
142  ss << "Tag id: " << tag_id;
143  vpDisplay::displayText(I, (int)(bbox.getTop() - 10), (int)bbox.getLeft(), ss.str(), vpColor::red);
144  }
146  for (size_t j = 0; j < p.size(); j++) {
147  vpDisplay::displayCross(I, p[j], 14, vpColor::red, 3);
148  std::ostringstream number;
149  number << j;
150  vpDisplay::displayText(I, p[j] + vpImagePoint(15, 5), number.str(), vpColor::blue);
151  }
152  }
153 
154  vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
155  vpDisplay::flush(I);
157 
159 
161  for (size_t i = 0; i < cMo_vec.size(); i++) {
162  vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
163  }
165 
166  vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
167  vpDisplay::flush(I);
169  } catch (const vpException &e) {
170  std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
171  }
172 
173  return EXIT_SUCCESS;
174 #else
175  (void)argc;
176  (void)argv;
177  return 0;
178 #endif
179 }
vpRect getBBox(size_t i) const
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:150
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:71
size_t getNbObjects() const
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)
const char * getMessage(void) const
Definition: vpException.cpp:90
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void read(vpImage< unsigned char > &I, const std::string &filename)
Definition: vpImageIo.cpp:243
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)