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>
11 int main(
int argc,
const char **argv)
14 #if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
17 std::string input_filename =
"AprilTag.pgm";
20 double tagSize = 0.053;
21 float quad_decimate = 1.0;
23 std::string intrinsic_file =
"";
24 std::string camera_name =
"";
25 bool display_tag =
false;
27 unsigned int thickness = 2;
28 bool z_aligned =
false;
30 for (
int i = 1; i < argc; i++) {
31 if (std::string(argv[i]) ==
"--pose_method" && i + 1 < argc) {
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") {
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) {
53 }
else if (std::string(argv[i]) ==
"--z_aligned") {
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]"
77 if (!intrinsic_file.empty() && !camera_name.empty())
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;
92 #elif defined(VISP_HAVE_GDI)
94 #elif defined(VISP_HAVE_OPENCV)
103 detector.setAprilTagQuadDecimate(quad_decimate);
104 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
105 detector.setAprilTagNbThreads(nThreads);
107 detector.setZAlignedWithCameraAxis(z_aligned);
114 std::vector<vpHomogeneousMatrix> cMo_vec;
115 detector.detect(I, tagSize, cam, cMo_vec);
119 std::stringstream ss;
120 ss <<
"Detection time: " << t <<
" ms for " << detector.getNbObjects() <<
" tags";
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());
139 ss <<
"Tag id: " << tag_id;
143 for (
size_t j = 0; j < p.size(); j++) {
145 std::ostringstream number;
158 for (
size_t i = 0; i < cMo_vec.size(); i++) {
167 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithoutDistortion
static vpColor getColor(const unsigned int &i)
static const vpColor none
static const vpColor blue
static const vpColor green
@ 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...
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.
const char * getMessage() const
static void read(vpImage< unsigned char > &I, const std::string &filename)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Defines a rectangle in the plane.
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()