Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-apriltag-detector.cpp
1
3#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
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 {
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(HAVE_OPENCV_HIGHGUI)
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);
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);
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(HAVE_OPENCV_HIGHGUI)
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#else
195 (void)argc;
196 (void)argv;
197#endif
198 return EXIT_SUCCESS;
199}
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Class to define RGB colors available for display functionalities.
Definition vpColor.h:152
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).
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:132
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
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
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()