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>
10 #include <visp3/core/vpXmlParserCamera.h>
13 int main(
int argc,
const char **argv)
16 #if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
19 std::string input_filename =
"AprilTag.pgm";
22 double tagSize = 0.053;
23 float quad_decimate = 1.0;
25 std::string intrinsic_file =
"";
26 std::string camera_name =
"";
27 bool display_tag =
false;
29 unsigned int thickness = 2;
30 bool z_aligned =
false;
32 for (
int i = 1; i < argc; i++) {
33 if (std::string(argv[i]) ==
"--pose_method" && i + 1 < argc) {
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") {
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) {
55 }
else if (std::string(argv[i]) ==
"--z_aligned") {
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: "
63 "HOMOGRAPHY_VIRTUAL_VS,"
64 " 2: DEMENTHON_VIRTUAL_VS, 3: LAGRANGE_VIRTUAL_VS,"
65 " 4: BEST_RESIDUAL_VIRTUAL_VS)]"
66 " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: "
68 " 3: TAG_25h9, 4: TAG_25h7)]"
69 " [--display_tag] [--color <color_id (0, 1, ...)>]"
70 " [--thickness <thickness>] [--z_aligned]"
81 if (!intrinsic_file.empty() && !camera_name.empty())
84 std::cout <<
"cam:\n" << cam << std::endl;
85 std::cout <<
"poseEstimationMethod: " << poseEstimationMethod << std::endl;
86 std::cout <<
"tagFamily: " << tagFamily << std::endl;
87 std::cout <<
"Z aligned: " << z_aligned << std::endl;
95 #elif defined(VISP_HAVE_GDI)
97 #elif defined(VISP_HAVE_OPENCV)
106 detector.setAprilTagQuadDecimate(quad_decimate);
107 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
108 detector.setAprilTagNbThreads(nThreads);
110 detector.setZAlignedWithCameraAxis(z_aligned);
117 std::vector<vpHomogeneousMatrix> cMo_vec;
118 detector.detect(I, tagSize, cam, cMo_vec);
122 std::stringstream ss;
123 ss <<
"Detection time: " << t <<
" ms for " << detector.getNbObjects() <<
" tags";
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());
142 ss <<
"Tag id: " << tag_id;
147 for (
size_t j = 0; j < p.size(); j++) {
149 std::ostringstream number;
162 for (
size_t i = 0; i < cMo_vec.size(); i++) {
171 std::cerr <<
"Catch an exception: " << e.
getMessage() << std::endl;