Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpDetectorAprilTag.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Base class for AprilTag detection.
32 */
33#include <visp3/core/vpConfig.h>
34
35#ifdef VISP_HAVE_APRILTAG
36#include <map>
37
38#ifdef __cplusplus
39extern "C" {
40#endif
41#include <apriltag.h>
42#include <apriltag_pose.h>
43#include <common/homography.h>
44#include <tag16h5.h>
45#include <tag25h7.h>
46#include <tag25h9.h>
47#include <tag36h10.h>
48#include <tag36h11.h>
49#include <tagCircle21h7.h>
50#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
51#include <tagCircle49h12.h>
52#include <tagCustom48h12.h>
53#include <tagStandard41h12.h>
54#include <tagStandard52h13.h>
55#endif
56#include <tagAruco4x4_50.h>
57#include <tagAruco4x4_100.h>
58#include <tagAruco4x4_250.h>
59#include <tagAruco4x4_1000.h>
60#include <tagAruco5x5_50.h>
61#include <tagAruco5x5_100.h>
62#include <tagAruco5x5_250.h>
63#include <tagAruco5x5_1000.h>
64#include <tagAruco6x6_50.h>
65#include <tagAruco6x6_100.h>
66#include <tagAruco6x6_250.h>
67#include <tagAruco6x6_1000.h>
68#include <tagAruco7x7_50.h>
69#include <tagAruco7x7_100.h>
70#include <tagAruco7x7_250.h>
71#include <tagAruco7x7_1000.h>
72#include <tagAruco_MIP_36h12.h>
73#ifdef __cplusplus
74}
75#endif
76
77#include <visp3/core/vpDisplay.h>
78#include <visp3/core/vpPixelMeterConversion.h>
79#include <visp3/core/vpPoint.h>
80#include <visp3/detection/vpDetectorAprilTag.h>
81#include <visp3/vision/vpPose.h>
82
83BEGIN_VISP_NAMESPACE
84
85#ifndef DOXYGEN_SHOULD_SKIP_THIS
86class vpDetectorAprilTag::Impl
87{
88public:
89 Impl(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &method)
90 : m_poseEstimationMethod(method), m_tagsId(), m_tagFamily(tagFamily), m_tagsDecisionMargin(),
91 m_tagsHammingDistance(), m_td(nullptr), m_tf(nullptr),
92 m_detections(nullptr), m_decisionMarginThreshold(-1), m_hammingDistanceThreshold(2), m_zAlignedWithCameraFrame(false)
93 {
94 switch (m_tagFamily) {
95 case TAG_36h11:
96 m_tf = tag36h11_create();
97 break;
98
99 case TAG_36h10:
100 m_tf = tag36h10_create();
101 break;
102
103 case TAG_36ARTOOLKIT:
104 break;
105
106 case TAG_25h9:
107 m_tf = tag25h9_create();
108 break;
109
110 case TAG_25h7:
111 m_tf = tag25h7_create();
112 break;
113
114 case TAG_16h5:
115 m_tf = tag16h5_create();
116 break;
117
118 case TAG_CIRCLE21h7:
119 m_tf = tagCircle21h7_create();
120 break;
121
122 case TAG_CIRCLE49h12:
123#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
124 m_tf = tagCircle49h12_create();
125#endif
126 break;
127
128 case TAG_CUSTOM48h12:
129#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
130 m_tf = tagCustom48h12_create();
131#endif
132 break;
133
134 case TAG_STANDARD52h13:
135#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
136 m_tf = tagStandard52h13_create();
137#endif
138 break;
139
140 case TAG_STANDARD41h12:
141#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
142 m_tf = tagStandard41h12_create();
143#endif
144 break;
145
146 case TAG_ARUCO_4x4_50:
147 m_tf = tagAruco4x4_50_create();
148 break;
149
150 case TAG_ARUCO_4x4_100:
151 m_tf = tagAruco4x4_100_create();
152 break;
153
154 case TAG_ARUCO_4x4_250:
155 m_tf = tagAruco4x4_250_create();
156 break;
157
158 case TAG_ARUCO_4x4_1000:
159 m_tf = tagAruco4x4_1000_create();
160 break;
161
162 case TAG_ARUCO_5x5_50:
163 m_tf = tagAruco5x5_50_create();
164 break;
165
166 case TAG_ARUCO_5x5_100:
167 m_tf = tagAruco5x5_100_create();
168 break;
169
170 case TAG_ARUCO_5x5_250:
171 m_tf = tagAruco5x5_250_create();
172 break;
173
174 case TAG_ARUCO_5x5_1000:
175 m_tf = tagAruco5x5_1000_create();
176 break;
177
178 case TAG_ARUCO_6x6_50:
179 m_tf = tagAruco6x6_50_create();
180 break;
181
182 case TAG_ARUCO_6x6_100:
183 m_tf = tagAruco6x6_100_create();
184 break;
185
186 case TAG_ARUCO_6x6_250:
187 m_tf = tagAruco6x6_250_create();
188 break;
189
190 case TAG_ARUCO_6x6_1000:
191 m_tf = tagAruco6x6_1000_create();
192 break;
193
194 case TAG_ARUCO_7x7_50:
195 m_tf = tagAruco7x7_50_create();
196 break;
197
198 case TAG_ARUCO_7x7_100:
199 m_tf = tagAruco7x7_100_create();
200 break;
201
202 case TAG_ARUCO_7x7_250:
203 m_tf = tagAruco7x7_250_create();
204 break;
205
206 case TAG_ARUCO_7x7_1000:
207 m_tf = tagAruco7x7_1000_create();
208 break;
209
210 case TAG_ARUCO_MIP_36h12:
211 m_tf = tagArucoMIP_36h12_create();
212 break;
213
214 default:
215 throw vpException(vpException::fatalError, "Unknown Tag family!");
216 }
217
218 if ((m_tagFamily != TAG_36ARTOOLKIT) && m_tf) {
219 m_td = apriltag_detector_create();
220 int bits_corrected = (m_tagFamily == TAG_ARUCO_4x4_1000) ? 1 : 2;
221 apriltag_detector_add_family(m_td, m_tf, bits_corrected);
222 }
223
224 m_mapOfCorrespondingPoseMethods[DEMENTHON_VIRTUAL_VS] = vpPose::DEMENTHON;
225 m_mapOfCorrespondingPoseMethods[LAGRANGE_VIRTUAL_VS] = vpPose::LAGRANGE;
226 }
227
228 Impl(const Impl &o)
229 : m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagsId(o.m_tagsId), m_tagFamily(o.m_tagFamily),
230 m_tagsDecisionMargin(o.m_tagsDecisionMargin), m_tagsHammingDistance(o.m_tagsHammingDistance), m_td(nullptr),
231 m_tf(nullptr), m_detections(nullptr), m_decisionMarginThreshold(o.m_decisionMarginThreshold),
232 m_hammingDistanceThreshold(o.m_hammingDistanceThreshold), m_zAlignedWithCameraFrame(o.m_zAlignedWithCameraFrame)
233 {
234 switch (m_tagFamily) {
235 case TAG_36h11:
236 m_tf = tag36h11_create();
237 break;
238
239 case TAG_36h10:
240 m_tf = tag36h10_create();
241 break;
242
243 case TAG_36ARTOOLKIT:
244 break;
245
246 case TAG_25h9:
247 m_tf = tag25h9_create();
248 break;
249
250 case TAG_25h7:
251 m_tf = tag25h7_create();
252 break;
253
254 case TAG_16h5:
255 m_tf = tag16h5_create();
256 break;
257
258 case TAG_CIRCLE21h7:
259 m_tf = tagCircle21h7_create();
260 break;
261
262 case TAG_CIRCLE49h12:
263#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
264 m_tf = tagCircle49h12_create();
265#endif
266 break;
267
268 case TAG_CUSTOM48h12:
269#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
270 m_tf = tagCustom48h12_create();
271#endif
272 break;
273
274 case TAG_STANDARD52h13:
275#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
276 m_tf = tagStandard52h13_create();
277#endif
278 break;
279
280 case TAG_STANDARD41h12:
281#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
282 m_tf = tagStandard41h12_create();
283#endif
284 break;
285
286 case TAG_ARUCO_4x4_50:
287 m_tf = tagAruco4x4_50_create();
288 break;
289
290 case TAG_ARUCO_4x4_100:
291 m_tf = tagAruco4x4_100_create();
292 break;
293
294 case TAG_ARUCO_4x4_250:
295 m_tf = tagAruco4x4_250_create();
296 break;
297
298 case TAG_ARUCO_4x4_1000:
299 m_tf = tagAruco4x4_1000_create();
300 break;
301
302 case TAG_ARUCO_5x5_50:
303 m_tf = tagAruco5x5_50_create();
304 break;
305
306 case TAG_ARUCO_5x5_100:
307 m_tf = tagAruco5x5_100_create();
308 break;
309
310 case TAG_ARUCO_5x5_250:
311 m_tf = tagAruco5x5_250_create();
312 break;
313
314 case TAG_ARUCO_5x5_1000:
315 m_tf = tagAruco5x5_1000_create();
316 break;
317
318 case TAG_ARUCO_6x6_50:
319 m_tf = tagAruco6x6_50_create();
320 break;
321
322 case TAG_ARUCO_6x6_100:
323 m_tf = tagAruco6x6_100_create();
324 break;
325
326 case TAG_ARUCO_6x6_250:
327 m_tf = tagAruco6x6_250_create();
328 break;
329
330 case TAG_ARUCO_6x6_1000:
331 m_tf = tagAruco6x6_1000_create();
332 break;
333
334 case TAG_ARUCO_7x7_50:
335 m_tf = tagAruco7x7_50_create();
336 break;
337
338 case TAG_ARUCO_7x7_100:
339 m_tf = tagAruco7x7_100_create();
340 break;
341
342 case TAG_ARUCO_7x7_250:
343 m_tf = tagAruco7x7_250_create();
344 break;
345
346 case TAG_ARUCO_7x7_1000:
347 m_tf = tagAruco7x7_1000_create();
348 break;
349
350 case TAG_ARUCO_MIP_36h12:
351 m_tf = tagArucoMIP_36h12_create();
352 break;
353
354 default:
355 throw vpException(vpException::fatalError, "Unknown Tag family!");
356 }
357
358 if ((m_tagFamily != TAG_36ARTOOLKIT) && m_tf) {
359 m_td = apriltag_detector_copy(o.m_td);
360 int bits_corrected = (m_tagFamily == TAG_ARUCO_4x4_1000) ? 1 : 2;
361 apriltag_detector_add_family(m_td, m_tf, bits_corrected);
362 }
363
364 m_mapOfCorrespondingPoseMethods[DEMENTHON_VIRTUAL_VS] = vpPose::DEMENTHON;
365 m_mapOfCorrespondingPoseMethods[LAGRANGE_VIRTUAL_VS] = vpPose::LAGRANGE;
366
367 if (o.m_detections != nullptr) {
368 m_detections = apriltag_detections_copy(o.m_detections);
369 }
370 }
371
372 ~Impl()
373 {
374 if (m_td) {
375 apriltag_detector_destroy(m_td);
376 }
377
378 if (m_tf) {
379 switch (m_tagFamily) {
380 case TAG_36h11:
381 tag36h11_destroy(m_tf);
382 break;
383
384 case TAG_36h10:
385 tag36h10_destroy(m_tf);
386 break;
387
388 case TAG_36ARTOOLKIT:
389 break;
390
391 case TAG_25h9:
392 tag25h9_destroy(m_tf);
393 break;
394
395 case TAG_25h7:
396 tag25h7_destroy(m_tf);
397 break;
398
399 case TAG_16h5:
400 tag16h5_destroy(m_tf);
401 break;
402
403 case TAG_CIRCLE21h7:
404 tagCircle21h7_destroy(m_tf);
405 break;
406
407 case TAG_CIRCLE49h12:
408#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
409 tagCustom48h12_destroy(m_tf);
410#endif
411 break;
412
413 case TAG_CUSTOM48h12:
414#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
415 tagCustom48h12_destroy(m_tf);
416#endif
417 break;
418
419 case TAG_STANDARD52h13:
420#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
421 tagStandard52h13_destroy(m_tf);
422#endif
423 break;
424
425 case TAG_STANDARD41h12:
426#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
427 tagStandard41h12_destroy(m_tf);
428#endif
429 break;
430
431 case TAG_ARUCO_4x4_50:
432 tagAruco4x4_50_destroy(m_tf);
433 break;
434
435 case TAG_ARUCO_4x4_100:
436 tagAruco4x4_100_destroy(m_tf);
437 break;
438
439 case TAG_ARUCO_4x4_250:
440 tagAruco4x4_250_destroy(m_tf);
441 break;
442
443 case TAG_ARUCO_4x4_1000:
444 tagAruco4x4_1000_destroy(m_tf);
445 break;
446
447 case TAG_ARUCO_5x5_50:
448 tagAruco5x5_50_destroy(m_tf);
449 break;
450
451 case TAG_ARUCO_5x5_100:
452 tagAruco5x5_100_destroy(m_tf);
453 break;
454
455 case TAG_ARUCO_5x5_250:
456 tagAruco5x5_250_destroy(m_tf);
457 break;
458
459 case TAG_ARUCO_5x5_1000:
460 tagAruco5x5_1000_destroy(m_tf);
461 break;
462
463 case TAG_ARUCO_6x6_50:
464 tagAruco6x6_50_destroy(m_tf);
465 break;
466
467 case TAG_ARUCO_6x6_100:
468 tagAruco6x6_100_destroy(m_tf);
469 break;
470
471 case TAG_ARUCO_6x6_250:
472 tagAruco6x6_250_destroy(m_tf);
473 break;
474
475 case TAG_ARUCO_6x6_1000:
476 tagAruco6x6_1000_destroy(m_tf);
477 break;
478
479 case TAG_ARUCO_7x7_50:
480 tagAruco7x7_50_destroy(m_tf);
481 break;
482
483 case TAG_ARUCO_7x7_100:
484 tagAruco7x7_100_destroy(m_tf);
485 break;
486
487 case TAG_ARUCO_7x7_250:
488 tagAruco7x7_250_destroy(m_tf);
489 break;
490
491 case TAG_ARUCO_7x7_1000:
492 tagAruco7x7_1000_destroy(m_tf);
493 break;
494
495 case TAG_ARUCO_MIP_36h12:
496 tagArucoMIP_36h12_destroy(m_tf);
497 break;
498
499 default:
500 break;
501 }
502 }
503
504 if (m_detections) {
505 apriltag_detections_destroy(m_detections);
506 m_detections = nullptr;
507 }
508 }
509
510 void convertHomogeneousMatrix(const apriltag_pose_t &pose, vpHomogeneousMatrix &cMo)
511 {
512 const unsigned int val_3 = 3;
513 for (unsigned int i = 0; i < val_3; ++i) {
514 for (unsigned int j = 0; j < val_3; ++j) {
515 cMo[i][j] = MATD_EL(pose.R, i, j);
516 }
517 cMo[i][val_3] = MATD_EL(pose.t, i, 0);
518 }
519 }
520
521 bool detect(const vpImage<unsigned char> &I, double tagSize, const vpCameraParameters &cam,
522 std::vector<std::vector<vpImagePoint> > &polygons, std::vector<std::string> &messages, bool displayTag,
523 const vpColor color, unsigned int thickness, std::vector<vpHomogeneousMatrix> *cMo_vec,
524 std::vector<vpHomogeneousMatrix> *cMo_vec2, std::vector<double> *projErrors,
525 std::vector<double> *projErrors2)
526 {
527 if (m_tagFamily == TAG_36ARTOOLKIT) {
528 // TAG_36ARTOOLKIT is not available anymore
529 std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
530 return false;
531 }
532#if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
533 if ((m_tagFamily == TAG_CIRCLE49h12) || (m_tagFamily == TAG_CUSTOM48h12) || (m_tagFamily == TAG_STANDARD41h12) ||
534 (m_tagFamily == TAG_STANDARD52h13)) {
535 std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
536 << std::endl;
537 return false;
538 }
539#endif
540
541 const bool computePose = (cMo_vec != nullptr);
542
543 image_u8_t im = {/*.width =*/static_cast<int32_t>(I.getWidth()),
544 /*.height =*/static_cast<int32_t>(I.getHeight()),
545 /*.stride =*/static_cast<int32_t>(I.getWidth()),
546 /*.buf =*/I.bitmap };
547
548 if (m_detections) {
549 apriltag_detections_destroy(m_detections);
550 m_detections = nullptr;
551 }
552
553 m_detections = apriltag_detector_detect(m_td, &im);
554 int nb_detections = zarray_size(m_detections);
555 bool detected = nb_detections > 0;
556
557 polygons.clear(); messages.clear(); m_tagsId.clear(); m_tagsDecisionMargin.clear(); m_tagsHammingDistance.clear();
558 polygons.reserve(static_cast<size_t>(nb_detections));
559 messages.reserve(static_cast<size_t>(nb_detections));
560 m_tagsId.reserve(static_cast<size_t>(nb_detections));
561 m_tagsDecisionMargin.reserve(static_cast<size_t>(nb_detections));
562 m_tagsHammingDistance.reserve(static_cast<size_t>(nb_detections));
563
564 int zarray_size_m_detections = zarray_size(m_detections);
565 for (int i = 0; i < zarray_size_m_detections; ++i) {
566 apriltag_detection_t *det;
567 zarray_get(m_detections, i, &det);
568
569 if (m_decisionMarginThreshold > 0) {
570 if (det->decision_margin < m_decisionMarginThreshold) {
571 continue;
572 }
573 }
574 if (det->hamming > m_hammingDistanceThreshold) {
575 continue;
576 }
577
578 std::vector<vpImagePoint> polygon;
579 const int polygonSize = 4;
580 for (int j = 0; j < polygonSize; ++j) {
581 polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0]));
582 }
583 polygons.push_back(polygon);
584 std::stringstream ss;
585 ss << m_tagFamily << " id: " << det->id;
586 messages.push_back(ss.str());
587 m_tagsId.push_back(det->id);
588 m_tagsDecisionMargin.push_back(det->decision_margin);
589 m_tagsHammingDistance.push_back(det->hamming);
590
591 if (displayTag) {
592 vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
593 vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
594 vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
595 vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
596
597 const unsigned int polyId0 = 0, polyId1 = 1, polyId2 = 2, polyId3 = 3;
598 vpDisplay::displayLine(I, static_cast<int>(det->p[polyId0][1]), static_cast<int>(det->p[polyId0][0]),
599 static_cast<int>(det->p[polyId1][1]), static_cast<int>(det->p[polyId1][0]), Ox, thickness);
600 vpDisplay::displayLine(I, static_cast<int>(det->p[polyId0][1]), static_cast<int>(det->p[polyId0][0]),
601 static_cast<int>(det->p[polyId3][1]), static_cast<int>(det->p[polyId3][0]), Oy, thickness);
602 vpDisplay::displayLine(I, static_cast<int>(det->p[polyId1][1]), static_cast<int>(det->p[polyId1][0]),
603 static_cast<int>(det->p[polyId2][1]), static_cast<int>(det->p[polyId2][0]), Ox2, thickness);
604 vpDisplay::displayLine(I, static_cast<int>(det->p[polyId2][1]), static_cast<int>(det->p[polyId2][0]),
605 static_cast<int>(det->p[polyId3][1]), static_cast<int>(det->p[polyId3][0]), Oy2, thickness);
606 }
607
608 if (computePose) {
609 vpHomogeneousMatrix cMo, cMo2;
610 double err1, err2;
611 if (getPose(static_cast<size_t>(i), tagSize, cam, cMo, cMo_vec2 ? &cMo2 : nullptr, projErrors ? &err1 : nullptr,
612 projErrors2 ? &err2 : nullptr)) {
613 cMo_vec->push_back(cMo);
614 if (cMo_vec2) {
615 cMo_vec2->push_back(cMo2);
616 }
617 if (projErrors) {
618 projErrors->push_back(err1);
619 }
620 if (projErrors2) {
621 projErrors2->push_back(err2);
622 }
623 }
624 // else case should never happen
625 }
626 }
627
628 return detected;
629 }
630
631 void displayFrames(const vpImage<unsigned char> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
632 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
633 {
634 size_t cmo_vec_size = cMo_vec.size();
635 for (size_t i = 0; i < cmo_vec_size; ++i) {
636 const vpHomogeneousMatrix &cMo = cMo_vec[i];
637 vpDisplay::displayFrame(I, cMo, cam, size, color, thickness);
638 }
639 }
640
641 void displayFrames(const vpImage<vpRGBa> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
642 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
643 {
644 size_t cmo_vec_size = cMo_vec.size();
645 for (size_t i = 0; i < cmo_vec_size; ++i) {
646 const vpHomogeneousMatrix &cMo = cMo_vec[i];
647 vpDisplay::displayFrame(I, cMo, cam, size, color, thickness);
648 }
649 }
650
651 void displayTags(const vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
652 const vpColor &color, unsigned int thickness) const
653 {
654 size_t tagscorners_size = tagsCorners.size();
655 for (size_t i = 0; i < tagscorners_size; ++i) {
656 const vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
657 const vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
658 const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
659 const vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
660
661 const std::vector<vpImagePoint> &corners = tagsCorners[i];
662 assert(corners.size() == 4);
663 const unsigned int cornerId0 = 0, cornerId1 = 1, cornerId2 = 2, cornerId3 = 3;
664 const double line0_length_8 = vpImagePoint::distance(corners[cornerId0], corners[cornerId1]) / 8;
665 unsigned int rect_size = static_cast<unsigned int>(line0_length_8);
666 if (line0_length_8 < 2) {
667 rect_size = 2;
668 }
669 vpDisplay::displayRectangle(I, corners[cornerId0], 0, rect_size, rect_size, vpColor::red, thickness);
670
671 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId0].get_i()), static_cast<int>(corners[cornerId0].get_j()),
672 static_cast<int>(corners[cornerId1].get_i()), static_cast<int>(corners[cornerId1].get_j()), Ox, thickness);
673 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId0].get_i()), static_cast<int>(corners[cornerId0].get_j()),
674 static_cast<int>(corners[cornerId3].get_i()), static_cast<int>(corners[cornerId3].get_j()), Oy, thickness);
675 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId1].get_i()), static_cast<int>(corners[cornerId1].get_j()),
676 static_cast<int>(corners[cornerId2].get_i()), static_cast<int>(corners[cornerId2].get_j()), Ox2, thickness);
677 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId2].get_i()), static_cast<int>(corners[cornerId2].get_j()),
678 static_cast<int>(corners[cornerId3].get_i()), static_cast<int>(corners[cornerId3].get_j()), Oy2, thickness);
679 }
680 }
681
682 void displayTags(const vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
683 const vpColor &color, unsigned int thickness) const
684 {
685 size_t tagscorners_size = tagsCorners.size();
686 for (size_t i = 0; i < tagscorners_size; ++i) {
687 const vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
688 const vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
689 const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
690 const vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
691
692 const std::vector<vpImagePoint> &corners = tagsCorners[i];
693 assert(corners.size() == 4);
694 const unsigned int cornerId0 = 0, cornerId1 = 1, cornerId2 = 2, cornerId3 = 3;
695 const double line0_length_8 = vpImagePoint::distance(corners[cornerId0], corners[cornerId1]) / 8;
696 unsigned int rect_size = static_cast<unsigned int>(line0_length_8);
697 if (line0_length_8 < 2) {
698 rect_size = 2;
699 }
700 vpDisplay::displayRectangle(I, corners[cornerId0], 0, rect_size, rect_size, vpColor::red, thickness);
701
702 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId0].get_i()), static_cast<int>(corners[cornerId0].get_j()),
703 static_cast<int>(corners[cornerId1].get_i()), static_cast<int>(corners[cornerId1].get_j()), Ox, thickness);
704 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId0].get_i()), static_cast<int>(corners[cornerId0].get_j()),
705 static_cast<int>(corners[cornerId3].get_i()), static_cast<int>(corners[cornerId3].get_j()), Oy, thickness);
706 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId1].get_i()), static_cast<int>(corners[cornerId1].get_j()),
707 static_cast<int>(corners[cornerId2].get_i()), static_cast<int>(corners[cornerId2].get_j()), Ox2, thickness);
708 vpDisplay::displayLine(I, static_cast<int>(corners[cornerId2].get_i()), static_cast<int>(corners[cornerId2].get_j()),
709 static_cast<int>(corners[cornerId3].get_i()), static_cast<int>(corners[cornerId3].get_j()), Oy2, thickness);
710 }
711 }
712
720 bool getTagImage(vpImage<unsigned char> &I, int id)
721 {
722 if (id >= static_cast<int>(m_tf->ncodes)) {
723 std::cerr << "Cannot get tag image with id " << id << " for family " << m_tagFamily << std::endl;
724 return false;
725 }
726 image_u8_t *img_8u = apriltag_to_image(m_tf, id);
727
728 I.init(img_8u->height, img_8u->width);
729 for (int i = 0; i < img_8u->height; i++) {
730 for (int j = 0; j < img_8u->width; j++) {
731 I[i][j] = img_8u->buf[i*img_8u->stride + j];
732 }
733 }
734
735 image_u8_destroy(img_8u);
736 return true;
737 }
738
739 bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
740 vpHomogeneousMatrix *cMo2, double *projErrors, double *projErrors2)
741 {
742 if (m_detections == nullptr) {
743 throw(vpException(vpException::fatalError, "Cannot get tag index=%d pose: detection empty", tagIndex));
744 }
745 if (m_tagFamily == TAG_36ARTOOLKIT) {
746 // TAG_36ARTOOLKIT is not available anymore
747 std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
748 return false;
749 }
750#if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
751 if ((m_tagFamily == TAG_CIRCLE49h12) || (m_tagFamily == TAG_CUSTOM48h12) || (m_tagFamily == TAG_STANDARD41h12) ||
752 (m_tagFamily == TAG_STANDARD52h13)) {
753 std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
754 << std::endl;
755 return false;
756 }
757#endif
758
759 apriltag_detection_t *det;
760 zarray_get(m_detections, static_cast<int>(tagIndex), &det);
761
762 int nb_detections = zarray_size(m_detections);
763 if (tagIndex >= static_cast<size_t>(nb_detections)) {
764 return false;
765 }
766
767 // In AprilTag3, estimate_pose_for_tag_homography() and estimate_tag_pose() have been added.
768 // They use a tag frame aligned with the camera frame
769 // Before the release of AprilTag3, convention used was to define the z-axis of the tag going upward.
770 // To keep compatibility, we maintain the same convention than before and there is setZAlignedWithCameraAxis().
771 // Under the hood, we use aligned frames everywhere and transform the pose according to the option.
772
773 vpHomogeneousMatrix cMo_homography_ortho_iter;
774 if ((m_poseEstimationMethod == HOMOGRAPHY_ORTHOGONAL_ITERATION) ||
775 (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS)) {
776 double fx = cam.get_px(), fy = cam.get_py();
777 double cx = cam.get_u0(), cy = cam.get_v0();
778
779 apriltag_detection_info_t info;
780 info.det = det;
781 info.tagsize = tagSize;
782 info.fx = fx;
783 info.fy = fy;
784 info.cx = cx;
785 info.cy = cy;
786
787 // projErrors and projErrors2 will be override later
788 getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
789 cMo_homography_ortho_iter = cMo;
790 }
791
792 vpHomogeneousMatrix cMo_homography;
793 if ((m_poseEstimationMethod == HOMOGRAPHY) || (m_poseEstimationMethod == HOMOGRAPHY_VIRTUAL_VS) ||
794 (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS)) {
795 double fx = cam.get_px(), fy = cam.get_py();
796 double cx = cam.get_u0(), cy = cam.get_v0();
797
798 apriltag_detection_info_t info;
799 info.det = det;
800 info.tagsize = tagSize;
801 info.fx = fx;
802 info.fy = fy;
803 info.cx = cx;
804 info.cy = cy;
805
806 apriltag_pose_t pose;
807 estimate_pose_for_tag_homography(&info, &pose);
808 convertHomogeneousMatrix(pose, cMo);
809
810 matd_destroy(pose.R);
811 matd_destroy(pose.t);
812
813 cMo_homography = cMo;
814 }
815
816 // Add marker object points
817 vpPose pose;
818 vpPoint pt;
819
820 vpImagePoint imPt;
821 double x = 0.0, y = 0.0;
822 std::vector<vpPoint> pts(4);
823 pt.setWorldCoordinates(-tagSize / 2.0, tagSize / 2.0, 0.0);
824 imPt.set_uv(det->p[0][0], det->p[0][1]);
825 vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
826 pt.set_x(x);
827 pt.set_y(y);
828 pts[0] = pt;
829
830 pt.setWorldCoordinates(tagSize / 2.0, tagSize / 2.0, 0.0);
831 imPt.set_uv(det->p[1][0], det->p[1][1]);
832 vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
833 pt.set_x(x);
834 pt.set_y(y);
835 pts[1] = pt;
836
837 const int idCorner2 = 2;
838 pt.setWorldCoordinates(tagSize / 2.0, -tagSize / 2.0, 0.0);
839 imPt.set_uv(det->p[idCorner2][0], det->p[idCorner2][1]);
840 vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
841 pt.set_x(x);
842 pt.set_y(y);
843 pts[idCorner2] = pt;
844
845 const int idCorner3 = 3;
846 pt.setWorldCoordinates(-tagSize / 2.0, -tagSize / 2.0, 0.0);
847 imPt.set_uv(det->p[idCorner3][0], det->p[idCorner3][1]);
848 vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
849 pt.set_x(x);
850 pt.set_y(y);
851 pts[idCorner3] = pt;
852
853 pose.addPoints(pts);
854
855 if ((m_poseEstimationMethod != HOMOGRAPHY) && (m_poseEstimationMethod != HOMOGRAPHY_VIRTUAL_VS) &&
856 (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION)) {
857 if (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) {
858 vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
859
860 double residual_dementhon = std::numeric_limits<double>::max(),
861 residual_lagrange = std::numeric_limits<double>::max();
862 double residual_homography = pose.computeResidual(cMo_homography);
863 double residual_homography_ortho_iter = pose.computeResidual(cMo_homography_ortho_iter);
864
865 if (pose.computePose(vpPose::DEMENTHON, cMo_dementhon)) {
866 residual_dementhon = pose.computeResidual(cMo_dementhon);
867 }
868
869 if (pose.computePose(vpPose::LAGRANGE, cMo_lagrange)) {
870 residual_lagrange = pose.computeResidual(cMo_lagrange);
871 }
872
873 std::vector<double> residuals;
874 residuals.push_back(residual_dementhon);
875 residuals.push_back(residual_lagrange);
876 residuals.push_back(residual_homography);
877 residuals.push_back(residual_homography_ortho_iter);
878 std::vector<vpHomogeneousMatrix> poses;
879 poses.push_back(cMo_dementhon);
880 poses.push_back(cMo_lagrange);
881 poses.push_back(cMo_homography);
882 poses.push_back(cMo_homography_ortho_iter);
883
884 std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
885 cMo = *(poses.begin() + minIndex);
886 }
887 else {
888 pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo);
889 }
890 }
891
892 if ((m_poseEstimationMethod != HOMOGRAPHY) && (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION)) {
893 // Compute final pose using VVS
894 pose.computePose(vpPose::VIRTUAL_VS, cMo);
895 }
896
897 // Only with HOMOGRAPHY_ORTHOGONAL_ITERATION we can directly get two solutions
898 if (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) {
899 if (cMo2) {
900 double scale = tagSize / 2.0;
901 double data_p0[] = { -scale, scale, 0 };
902 double data_p1[] = { scale, scale, 0 };
903 double data_p2[] = { scale, -scale, 0 };
904 double data_p3[] = { -scale, -scale, 0 };
905 const unsigned int nbPoints = 4;
906 const int nbRows = 3;
907 matd_t *p[nbPoints] = { matd_create_data(nbRows, 1, data_p0), matd_create_data(nbRows, 1, data_p1),
908 matd_create_data(nbRows, 1, data_p2), matd_create_data(nbRows, 1, data_p3) };
909 matd_t *v[nbPoints];
910 for (unsigned int i = 0; i < nbPoints; ++i) {
911 double data_v[] = { (det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(),
912 1 };
913 v[i] = matd_create_data(nbRows, 1, data_v);
914 }
915
916 apriltag_pose_t solution1, solution2;
917 const int nIters = 50;
918 const int nbCols = 3;
919 solution1.R = matd_create_data(nbRows, nbCols, cMo.getRotationMatrix().data);
920 solution1.t = matd_create_data(nbRows, 1, cMo.getTranslationVector().data);
921
922 double err2;
923 get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
924
925 for (unsigned int i = 0; i < nbPoints; ++i) {
926 matd_destroy(p[i]);
927 matd_destroy(v[i]);
928 }
929
930 if (solution2.R) {
931 convertHomogeneousMatrix(solution2, *cMo2);
932
933 matd_destroy(solution2.R);
934 matd_destroy(solution2.t);
935 }
936
937 matd_destroy(solution1.R);
938 matd_destroy(solution1.t);
939 }
940 }
941
942 // Compute projection error with vpPose::computeResidual() for consistency
943 if (projErrors) {
944 *projErrors = pose.computeResidual(cMo);
945 }
946 if (projErrors2 && cMo2) {
947 *projErrors2 = pose.computeResidual(*cMo2);
948 }
949
950 if (!m_zAlignedWithCameraFrame) {
951 const unsigned int idX = 0, idY = 1, idZ = 2;
952 vpHomogeneousMatrix oMo;
953 // Apply a rotation of 180deg around x axis
954 oMo[idX][idX] = 1;
955 oMo[idX][idY] = 0;
956 oMo[idX][idZ] = 0;
957 oMo[idY][idX] = 0;
958 oMo[idY][idY] = -1;
959 oMo[idY][idZ] = 0;
960 oMo[idZ][idX] = 0;
961 oMo[idZ][idY] = 0;
962 oMo[idZ][idZ] = -1;
963 cMo = cMo * oMo;
964 if (cMo2) {
965 *cMo2 = *cMo2 * oMo;
966 }
967 }
968
969 return true;
970 }
971
972 void getPoseWithOrthogonalMethod(apriltag_detection_info_t &info, vpHomogeneousMatrix &cMo1,
973 vpHomogeneousMatrix *cMo2, double *err1, double *err2)
974 {
975 apriltag_pose_t pose1, pose2;
976 double err_1, err_2;
977 const unsigned int nbIters = 50;
978 estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, nbIters);
979 if (err_1 <= err_2) {
980 convertHomogeneousMatrix(pose1, cMo1);
981 if (cMo2) {
982 if (pose2.R) {
983 convertHomogeneousMatrix(pose2, *cMo2);
984 }
985 else {
986 *cMo2 = cMo1;
987 }
988 }
989 }
990 else {
991 convertHomogeneousMatrix(pose2, cMo1);
992 if (cMo2) {
993 convertHomogeneousMatrix(pose1, *cMo2);
994 }
995 }
996
997 matd_destroy(pose1.R);
998 matd_destroy(pose1.t);
999 if (pose2.R) {
1000 matd_destroy(pose2.t);
1001 }
1002 matd_destroy(pose2.R);
1003
1004 if (err1) {
1005 *err1 = err_1;
1006 }
1007 if (err2) {
1008 *err2 = err_2;
1009 }
1010 }
1011
1012 bool getZAlignedWithCameraAxis() { return m_zAlignedWithCameraFrame; }
1013
1014 float getAprilTagDecisionMarginThreshold() const
1015 {
1016 return m_decisionMarginThreshold;
1017 }
1018
1019 int getAprilTagHammingDistanceThreshold() const
1020 {
1021 return m_hammingDistanceThreshold;
1022 }
1023
1024 bool getAprilTagDecodeSharpening(double &decodeSharpening) const
1025 {
1026 if (m_td) {
1027 decodeSharpening = m_td->decode_sharpening;
1028 return true;
1029 }
1030 return false;
1031 }
1032
1033 bool getNbThreads(int &nThreads) const
1034 {
1035 if (m_td) {
1036 nThreads = m_td->nthreads;
1037 return true;
1038 }
1039 return false;
1040 }
1041
1042 bool getQuadDecimate(float &quadDecimate) const
1043 {
1044 if (m_td) {
1045 quadDecimate = m_td->quad_decimate;
1046 return true;
1047 }
1048 return false;
1049 }
1050
1051 bool getQuadSigma(float &quadSigma) const
1052 {
1053 if (m_td) {
1054 quadSigma = m_td->quad_sigma;
1055 return true;
1056 }
1057 return false;
1058 }
1059
1060 bool getRefineEdges(bool &refineEdges) const
1061 {
1062 if (m_td) {
1063 refineEdges = (m_td->refine_edges ? true : false);
1064 return true;
1065 }
1066 return false;
1067 }
1068
1069 bool getZAlignedWithCameraAxis() const { return m_zAlignedWithCameraFrame; }
1070
1071 std::vector<float> getTagsDecisionMargin() const { return m_tagsDecisionMargin; }
1072
1073 std::vector<int> getTagsHammingDistance() const { return m_tagsHammingDistance; }
1074
1075 std::vector<int> getTagsId() const { return m_tagsId; }
1076
1077 void setAprilTagDecisionMarginThreshold(float decisionMarginThreshold)
1078 {
1079 m_decisionMarginThreshold = decisionMarginThreshold;
1080 }
1081
1082 void setAprilTagHammingDistanceThreshold(int hammingDistanceThreshold)
1083 {
1084 m_hammingDistanceThreshold = hammingDistanceThreshold;
1085 }
1086
1087 void setAprilTagDecodeSharpening(double decodeSharpening)
1088 {
1089 if (m_td) {
1090 m_td->decode_sharpening = decodeSharpening;
1091 }
1092 }
1093
1094 void setDebugFlag(bool flag)
1095 {
1096 if (m_td) {
1097 m_td->debug = flag;
1098 }
1099 }
1100
1101 void setNbThreads(int nThreads)
1102 {
1103 if (m_td) {
1104 m_td->nthreads = nThreads;
1105 }
1106 }
1107
1108 void setQuadDecimate(float quadDecimate)
1109 {
1110 if (m_td) {
1111 m_td->quad_decimate = quadDecimate;
1112 }
1113 }
1114
1115 void setQuadSigma(float quadSigma)
1116 {
1117 if (m_td) {
1118 m_td->quad_sigma = quadSigma;
1119 }
1120 }
1121
1122 void setRefineDecode(bool) { }
1123
1124 void setRefineEdges(bool refineEdges)
1125 {
1126 if (m_td) {
1127 m_td->refine_edges = (refineEdges ? true : false);
1128 }
1129 }
1130
1131 void setRefinePose(bool) { }
1132
1133 void setPoseEstimationMethod(const vpPoseEstimationMethod &method)
1134 {
1135 m_poseEstimationMethod = method;
1136 }
1137
1138 void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
1139 {
1140 m_zAlignedWithCameraFrame = zAlignedWithCameraFrame;
1141 }
1142
1143 bool isZAlignedWithCameraAxis() const
1144 {
1145 return m_zAlignedWithCameraFrame;
1146 }
1147
1148protected:
1149 std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
1150 vpPoseEstimationMethod m_poseEstimationMethod;
1151 std::vector<int> m_tagsId;
1152 vpAprilTagFamily m_tagFamily;
1153 std::vector<float> m_tagsDecisionMargin;
1154 std::vector<int> m_tagsHammingDistance;
1155 apriltag_detector_t *m_td;
1156 apriltag_family_t *m_tf;
1157 zarray_t *m_detections;
1158 float m_decisionMarginThreshold;
1159 int m_hammingDistanceThreshold;
1160 bool m_zAlignedWithCameraFrame;
1161};
1162
1163namespace
1164{
1165const unsigned int def_tagThickness = 2;
1166}
1167#endif // DOXYGEN_SHOULD_SKIP_THIS
1168
1169vpDetectorAprilTag::vpDetectorAprilTag(const vpAprilTagFamily &tagFamily,
1170 const vpPoseEstimationMethod &poseEstimationMethod)
1171 : m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(def_tagThickness),
1172 m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(),
1173 m_impl(new Impl(tagFamily, poseEstimationMethod))
1174{ }
1175
1176vpDetectorAprilTag::vpDetectorAprilTag(const vpDetectorAprilTag &o)
1177 : vpDetectorBase(o), m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(def_tagThickness),
1178 m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(),
1179 m_impl(new Impl(*o.m_impl))
1180{ }
1181
1182vpDetectorAprilTag &vpDetectorAprilTag::operator=(vpDetectorAprilTag o)
1183{
1184 swap(*this, o);
1185 return *this;
1186}
1187
1188vpDetectorAprilTag::~vpDetectorAprilTag() { delete m_impl; }
1189
1197bool vpDetectorAprilTag::detect(const vpImage<unsigned char> &I)
1198{
1199 m_message.clear();
1200 m_polygon.clear();
1201 m_nb_objects = 0;
1202
1203 std::vector<vpHomogeneousMatrix> cMo_vec;
1204 const double tagSize = 1.0;
1205 bool detected = m_impl->detect(I, tagSize, m_defaultCam, m_polygon, m_message, m_displayTag, m_displayTagColor,
1206 m_displayTagThickness, nullptr, nullptr, nullptr, nullptr);
1207 m_nb_objects = m_message.size();
1208
1209 return detected;
1210}
1211
1229bool vpDetectorAprilTag::detect(const vpImage<unsigned char> &I, double tagSize, const vpCameraParameters &cam,
1230 std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
1231 std::vector<double> *projErrors, std::vector<double> *projErrors2)
1232{
1233 m_message.clear();
1234 m_polygon.clear();
1235 m_nb_objects = 0;
1236
1237 cMo_vec.clear();
1238 if (cMo_vec2) {
1239 cMo_vec2->clear();
1240 }
1241 bool detected = m_impl->detect(I, tagSize, cam, m_polygon, m_message, m_displayTag, m_displayTagColor,
1242 m_displayTagThickness, &cMo_vec, cMo_vec2, projErrors, projErrors2);
1243 m_nb_objects = m_message.size();
1244
1245 return detected;
1246}
1247
1258void vpDetectorAprilTag::displayFrames(const vpImage<unsigned char> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
1259 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
1260{
1261 m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
1262}
1263
1274void vpDetectorAprilTag::displayFrames(const vpImage<vpRGBa> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
1275 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
1276{
1277 m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
1278}
1279
1288void vpDetectorAprilTag::displayTags(const vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
1289 const vpColor &color, unsigned int thickness) const
1290{
1291 m_impl->displayTags(I, tagsCorners, color, thickness);
1292}
1293
1302void vpDetectorAprilTag::displayTags(const vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
1303 const vpColor &color, unsigned int thickness) const
1304{
1305 m_impl->displayTags(I, tagsCorners, color, thickness);
1306}
1307
1339bool vpDetectorAprilTag::getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam,
1340 vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2, double *projError,
1341 double *projError2)
1342{
1343 return m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2);
1344}
1345
1363std::vector<std::vector<vpPoint> > vpDetectorAprilTag::getTagsPoints3D(const std::vector<int> &tagsId,
1364 const std::map<int, double> &tagsSize) const
1365{
1366 std::vector<std::vector<vpPoint> > tagsPoints3D;
1367
1368 double default_size = -1;
1369
1370 std::map<int, double>::const_iterator it = tagsSize.find(-1);
1371 if (it != tagsSize.end()) {
1372 default_size = it->second; // Default size
1373 }
1374
1375 size_t tagsid_size = tagsId.size();
1376 for (size_t i = 0; i < tagsid_size; ++i) {
1377 it = tagsSize.find(tagsId[i]);
1378 double tagSize = default_size; // Default size
1379 if (it == tagsSize.end()) {
1380 if (default_size < 0) { // no default size found
1381 throw(vpException(vpException::fatalError,
1382 "Tag with id %d has no 3D size or there is no default 3D size defined", tagsId[i]));
1383 }
1384 }
1385 else {
1386 tagSize = it->second;
1387 }
1388 std::vector<vpPoint> points3D(4);
1389 const unsigned int idX = 0, idY = 1, idZ = 2, idHomogen = 3;
1390 const double middleFactor = 2.0;
1391 if (m_impl->getZAlignedWithCameraAxis()) {
1392 points3D[idX] = vpPoint(-tagSize / middleFactor, tagSize / middleFactor, 0);
1393 points3D[idY] = vpPoint(tagSize / middleFactor, tagSize / middleFactor, 0);
1394 points3D[idZ] = vpPoint(tagSize / middleFactor, -tagSize / middleFactor, 0);
1395 points3D[idHomogen] = vpPoint(-tagSize / middleFactor, -tagSize / middleFactor, 0);
1396 }
1397 else {
1398 points3D[idX] = vpPoint(-tagSize / middleFactor, -tagSize / middleFactor, 0);
1399 points3D[idY] = vpPoint(tagSize / middleFactor, -tagSize / middleFactor, 0);
1400 points3D[idZ] = vpPoint(tagSize / middleFactor, tagSize / middleFactor, 0);
1401 points3D[idHomogen] = vpPoint(-tagSize / middleFactor, tagSize / middleFactor, 0);
1402 }
1403 tagsPoints3D.push_back(points3D);
1404 }
1405
1406 return tagsPoints3D;
1407}
1408
1417float vpDetectorAprilTag::getAprilTagDecisionMarginThreshold() const
1418{
1419 return m_impl->getAprilTagDecisionMarginThreshold();
1420}
1421
1435int vpDetectorAprilTag::getAprilTagHammingDistanceThreshold() const
1436{
1437 return m_impl->getAprilTagHammingDistanceThreshold();
1438}
1439
1445std::vector<std::vector<vpImagePoint> > vpDetectorAprilTag::getTagsCorners() const { return m_polygon; }
1446
1462void vpDetectorAprilTag::setAprilTagDebugOption(bool flag)
1463{
1464 m_impl->setDebugFlag(flag);
1465}
1466
1474bool vpDetectorAprilTag::getTagImage(vpImage<unsigned char> &I, int id)
1475{
1476 return m_impl->getTagImage(I, id);
1477}
1478
1488std::vector<float> vpDetectorAprilTag::getTagsDecisionMargin() const { return m_impl->getTagsDecisionMargin(); }
1489
1500std::vector<int> vpDetectorAprilTag::getTagsHammingDistance() const { return m_impl->getTagsHammingDistance(); }
1501
1507std::vector<int> vpDetectorAprilTag::getTagsId() const { return m_impl->getTagsId(); }
1508
1509void vpDetectorAprilTag::setAprilTagDecodeSharpening(double decodeSharpening)
1510{
1511 return m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1512}
1513
1537void vpDetectorAprilTag::setAprilTagDecisionMarginThreshold(float decisionMarginThreshold)
1538{
1539 m_impl->setAprilTagDecisionMarginThreshold(decisionMarginThreshold);
1540}
1541
1563void vpDetectorAprilTag::setAprilTagHammingDistanceThreshold(int hammingDistanceThreshold)
1564{
1565 assert((hammingDistanceThreshold > -1) && (hammingDistanceThreshold < 256));
1566 m_impl->setAprilTagHammingDistanceThreshold(hammingDistanceThreshold);
1567}
1568
1569void vpDetectorAprilTag::setAprilTagFamily(const vpAprilTagFamily &tagFamily)
1570{
1571 // back-up settings
1572 double decodeSharpening = 0.25;
1573 m_impl->getAprilTagDecodeSharpening(decodeSharpening);
1574 int nThreads = 1;
1575 m_impl->getNbThreads(nThreads);
1576 float quadDecimate = 1;
1577 m_impl->getQuadDecimate(quadDecimate);
1578 float quadSigma = 0;
1579 m_impl->getQuadSigma(quadSigma);
1580 bool refineEdges = true;
1581 m_impl->getRefineEdges(refineEdges);
1582 bool zAxis = m_impl->getZAlignedWithCameraAxis();
1583
1584 delete m_impl;
1585 m_impl = new Impl(tagFamily, m_poseEstimationMethod);
1586 m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1587 m_impl->setNbThreads(nThreads);
1588 m_impl->setQuadDecimate(quadDecimate);
1589 m_impl->setQuadSigma(quadSigma);
1590 m_impl->setRefineEdges(refineEdges);
1591 m_impl->setZAlignedWithCameraAxis(zAxis);
1592}
1593
1599void vpDetectorAprilTag::setAprilTagNbThreads(int nThreads)
1600{
1601 if (nThreads > 0) {
1602 m_impl->setNbThreads(nThreads);
1603 }
1604}
1605
1611void vpDetectorAprilTag::setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
1612{
1613 m_poseEstimationMethod = poseEstimationMethod;
1614 m_impl->setPoseEstimationMethod(poseEstimationMethod);
1615}
1616
1629void vpDetectorAprilTag::setAprilTagQuadDecimate(float quadDecimate) { m_impl->setQuadDecimate(quadDecimate); }
1630
1643void vpDetectorAprilTag::setAprilTagQuadSigma(float quadSigma) { m_impl->setQuadSigma(quadSigma); }
1644
1645#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1649void vpDetectorAprilTag::setAprilTagRefineDecode(bool refineDecode)
1650{
1651 m_impl->setRefineDecode(refineDecode);
1652}
1653#endif
1654
1669void vpDetectorAprilTag::setAprilTagRefineEdges(bool refineEdges) { m_impl->setRefineEdges(refineEdges); }
1670
1671#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1675void vpDetectorAprilTag::setAprilTagRefinePose(bool refinePose) { m_impl->setRefinePose(refinePose); }
1676#endif
1677
1687void vpDetectorAprilTag::setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
1688{
1689 m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
1690}
1691
1696bool vpDetectorAprilTag::isZAlignedWithCameraAxis() const
1697{
1698 return m_impl->isZAlignedWithCameraAxis();
1699}
1700END_VISP_NAMESPACE
1701#elif !defined(VISP_BUILD_SHARED_LIBS)
1702// Work around to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has
1703// no symbols
1704void dummy_vpDetectorAprilTag() { }
1705#endif
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition vpCameraParameters.h:311
vpColor
Class to define RGB colors available for display functionalities.
Definition vpColor.h:157
vpColor::red
static const vpColor red
Definition vpColor.h:198
vpColor::none
static const vpColor none
Definition vpColor.h:210
vpColor::blue
static const vpColor blue
Definition vpColor.h:204
vpColor::yellow
static const vpColor yellow
Definition vpColor.h:206
vpColor::green
static const vpColor green
Definition vpColor.h:201
vpDetectorAprilTag::getTagsDecisionMargin
std::vector< float > getTagsDecisionMargin() const
Definition vpDetectorAprilTag.cpp:1488
vpDetectorAprilTag::setZAlignedWithCameraAxis
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
Definition vpDetectorAprilTag.cpp:1687
vpDetectorAprilTag::getTagsCorners
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
Definition vpDetectorAprilTag.cpp:1445
vpDetectorAprilTag::setAprilTagQuadDecimate
void setAprilTagQuadDecimate(float quadDecimate)
Definition vpDetectorAprilTag.cpp:1629
vpDetectorAprilTag::swap
friend void swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2)
Definition vpDetectorAprilTag.h:423
vpDetectorAprilTag::setAprilTagHammingDistanceThreshold
void setAprilTagHammingDistanceThreshold(int hammingDistanceThreshold)
Definition vpDetectorAprilTag.cpp:1563
vpDetectorAprilTag::displayFrames
void displayFrames(const vpImage< unsigned char > &I, const std::vector< vpHomogeneousMatrix > &cMo_vec, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1) const
Definition vpDetectorAprilTag.cpp:1258
vpDetectorAprilTag::operator=
vpDetectorAprilTag & operator=(vpDetectorAprilTag o)
Definition vpDetectorAprilTag.cpp:1182
vpDetectorAprilTag::getTagsPoints3D
std::vector< std::vector< vpPoint > > getTagsPoints3D(const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const
Definition vpDetectorAprilTag.cpp:1363
vpDetectorAprilTag::vpDetectorAprilTag
vpDetectorAprilTag(const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS)
Definition vpDetectorAprilTag.cpp:1169
vpDetectorAprilTag::detect
bool detect(const vpImage< unsigned char > &I) VP_OVERRIDE
Definition vpDetectorAprilTag.cpp:1197
vpDetectorAprilTag::m_displayTagThickness
unsigned int m_displayTagThickness
Definition vpDetectorAprilTag.h:444
vpDetectorAprilTag::getTagsHammingDistance
std::vector< int > getTagsHammingDistance() const
Definition vpDetectorAprilTag.cpp:1500
vpDetectorAprilTag::m_tagFamily
vpAprilTagFamily m_tagFamily
Definition vpDetectorAprilTag.h:446
vpDetectorAprilTag::setAprilTagRefineEdges
void setAprilTagRefineEdges(bool refineEdges)
Definition vpDetectorAprilTag.cpp:1669
vpDetectorAprilTag::isZAlignedWithCameraAxis
bool isZAlignedWithCameraAxis() const
Definition vpDetectorAprilTag.cpp:1696
vpDetectorAprilTag::m_poseEstimationMethod
vpPoseEstimationMethod m_poseEstimationMethod
Definition vpDetectorAprilTag.h:445
vpDetectorAprilTag::setAprilTagRefinePose
VP_DEPRECATED void setAprilTagRefinePose(bool refinePose)
Definition vpDetectorAprilTag.cpp:1675
vpDetectorAprilTag::setAprilTagQuadSigma
void setAprilTagQuadSigma(float quadSigma)
Definition vpDetectorAprilTag.cpp:1643
vpDetectorAprilTag::setAprilTagDebugOption
void setAprilTagDebugOption(bool flag)
Definition vpDetectorAprilTag.cpp:1462
vpDetectorAprilTag::setAprilTagNbThreads
void setAprilTagNbThreads(int nThreads)
Definition vpDetectorAprilTag.cpp:1599
vpDetectorAprilTag::setAprilTagRefineDecode
VP_DEPRECATED void setAprilTagRefineDecode(bool refineDecode)
Definition vpDetectorAprilTag.cpp:1649
vpDetectorAprilTag::setAprilTagPoseEstimationMethod
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
Definition vpDetectorAprilTag.cpp:1611
vpDetectorAprilTag::setAprilTagDecisionMarginThreshold
void setAprilTagDecisionMarginThreshold(float decisionMarginThreshold)
Definition vpDetectorAprilTag.cpp:1537
vpDetectorAprilTag::getTagsId
std::vector< int > getTagsId() const
Definition vpDetectorAprilTag.cpp:1507
vpDetectorAprilTag::getTagImage
bool getTagImage(vpImage< unsigned char > &I, int id)
Definition vpDetectorAprilTag.cpp:1474
vpDetectorAprilTag::getPose
bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=nullptr, double *projError=nullptr, double *projError2=nullptr)
Definition vpDetectorAprilTag.cpp:1339
vpDetectorAprilTag::getAprilTagDecisionMarginThreshold
float getAprilTagDecisionMarginThreshold() const
Definition vpDetectorAprilTag.cpp:1417
vpDetectorAprilTag::displayTags
void displayTags(const vpImage< unsigned char > &I, const std::vector< std::vector< vpImagePoint > > &tagsCorners, const vpColor &color=vpColor::none, unsigned int thickness=1) const
Definition vpDetectorAprilTag.cpp:1288
vpDetectorAprilTag::setAprilTagFamily
void setAprilTagFamily(const vpAprilTagFamily &tagFamily)
Definition vpDetectorAprilTag.cpp:1569
vpDetectorAprilTag::~vpDetectorAprilTag
virtual ~vpDetectorAprilTag() VP_OVERRIDE
Definition vpDetectorAprilTag.cpp:1188
vpDetectorAprilTag::getAprilTagHammingDistanceThreshold
int getAprilTagHammingDistanceThreshold() const
Definition vpDetectorAprilTag.cpp:1435
vpDetectorAprilTag::setAprilTagDecodeSharpening
void setAprilTagDecodeSharpening(double decodeSharpening)
Definition vpDetectorAprilTag.cpp:1509
vpDetectorBase::m_message
std::vector< std::string > m_message
Message attached to each object.
Definition vpDetectorBase.h:129
vpDetectorBase::m_polygon
std::vector< std::vector< vpImagePoint > > m_polygon
For each object, defines the polygon that contains the object.
Definition vpDetectorBase.h:128
vpDetectorBase::m_nb_objects
size_t m_nb_objects
Number of detected objects.
Definition vpDetectorBase.h:130
vpDisplay::displayLine
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Definition vpDisplay_uchar.cpp:458
vpDisplay::displayFrame
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))
Definition vpDisplay_uchar.cpp:441
vpDisplay::displayRectangle
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)
Definition vpDisplay_uchar.cpp:665
vpException
error that can be emitted by ViSP classes.
Definition vpException.h:60
vpException::fatalError
@ fatalError
Fatal error.
Definition vpException.h:72
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition vpHomogeneousMatrix.h:221
vpImagePoint::distance
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition vpImagePoint.cpp:481
vpImagePoint::set_uv
void set_uv(double u, double v)
Definition vpImagePoint.h:357
vpImage
Definition of the vpImage class member functions.
Definition vpImage.h:131
vpImage::init
void init(unsigned int height, unsigned int width)
Set the size of the image.
Definition vpImage.h:387
vpImage::getWidth
unsigned int getWidth() const
Definition vpImage.h:242
vpImage::bitmap
Type * bitmap
points toward the bitmap
Definition vpImage.h:135
vpImage::getHeight
unsigned int getHeight() const
Definition vpImage.h:181
vpPixelMeterConversion::convertPoint
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Definition vpPixelMeterConversion.h:108
vpPoint
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:79
vpPoint::set_x
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:471
vpPoint::setWorldCoordinates
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:116
vpPoint::set_y
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:473
vpPose::DEMENTHON
@ DEMENTHON
Definition vpPose.h:88
vpPose::VIRTUAL_VS
@ VIRTUAL_VS
Definition vpPose.h:97
vpPose::LAGRANGE
@ LAGRANGE
Definition vpPose.h:87
vpPose::computePose
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func=nullptr)
Definition vpPose.cpp:385
vpPose::addPoints
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:103
vpPose::computeResidual
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition vpPose.cpp:298
BEGIN_VISP_NAMESPACE
Definition vpMbtDistanceCircle.cpp:55