Visual Servoing Platform version 3.5.0
tutorial-apriltag-detector.cpp
1
3#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
11int 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 {
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);
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);
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}
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:310
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:90
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:149
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
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()