Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpDetectorAprilTag.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Base class for AprilTag detection.
33 *
34*****************************************************************************/
35#include <visp3/core/vpConfig.h>
36
37#ifdef VISP_HAVE_APRILTAG
38#include <map>
39
40#include <apriltag.h>
41#include <apriltag_pose.h>
42#include <common/homography.h>
43#include <tag16h5.h>
44#include <tag25h7.h>
45#include <tag25h9.h>
46#include <tag36h10.h>
47#include <tag36h11.h>
48#include <tagCircle21h7.h>
49#include <tagStandard41h12.h>
50#include <visp3/detection/vpDetectorAprilTag.h>
51#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
52#include <tagCircle49h12.h>
53#include <tagCustom48h12.h>
54#include <tagStandard41h12.h>
55#include <tagStandard52h13.h>
56#endif
57
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpPixelMeterConversion.h>
60#include <visp3/core/vpPoint.h>
61#include <visp3/vision/vpPose.h>
62
63#ifndef DOXYGEN_SHOULD_SKIP_THIS
64class vpDetectorAprilTag::Impl
65{
66public:
67 Impl(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &method)
68 : m_poseEstimationMethod(method), m_tagsId(), m_tagFamily(tagFamily), m_td(NULL), m_tf(NULL), m_detections(NULL),
69 m_zAlignedWithCameraFrame(false)
70 {
71 switch (m_tagFamily) {
72 case TAG_36h11:
73 m_tf = tag36h11_create();
74 break;
75
76 case TAG_36h10:
77 m_tf = tag36h10_create();
78 break;
79
80 case TAG_36ARTOOLKIT:
81 break;
82
83 case TAG_25h9:
84 m_tf = tag25h9_create();
85 break;
86
87 case TAG_25h7:
88 m_tf = tag25h7_create();
89 break;
90
91 case TAG_16h5:
92 m_tf = tag16h5_create();
93 break;
94
95 case TAG_CIRCLE21h7:
96 m_tf = tagCircle21h7_create();
97 break;
98
99 case TAG_CIRCLE49h12:
100#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
101 m_tf = tagCircle49h12_create();
102#endif
103 break;
104
105 case TAG_CUSTOM48h12:
106#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
107 m_tf = tagCustom48h12_create();
108#endif
109 break;
110
111 case TAG_STANDARD52h13:
112#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
113 m_tf = tagStandard52h13_create();
114#endif
115 break;
116
117 case TAG_STANDARD41h12:
118#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
119 m_tf = tagStandard41h12_create();
120#endif
121 break;
122
123 default:
124 throw vpException(vpException::fatalError, "Unknown Tag family!");
125 }
126
127 if (m_tagFamily != TAG_36ARTOOLKIT && m_tf) {
128 m_td = apriltag_detector_create();
129 apriltag_detector_add_family(m_td, m_tf);
130 }
131
132 m_mapOfCorrespondingPoseMethods[DEMENTHON_VIRTUAL_VS] = vpPose::DEMENTHON;
133 m_mapOfCorrespondingPoseMethods[LAGRANGE_VIRTUAL_VS] = vpPose::LAGRANGE;
134 }
135
136 Impl(const Impl &o)
137 : m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagsId(o.m_tagsId), m_tagFamily(o.m_tagFamily), m_td(NULL),
138 m_tf(NULL), m_detections(NULL), m_zAlignedWithCameraFrame(o.m_zAlignedWithCameraFrame)
139 {
140 switch (m_tagFamily) {
141 case TAG_36h11:
142 m_tf = tag36h11_create();
143 break;
144
145 case TAG_36h10:
146 m_tf = tag36h10_create();
147 break;
148
149 case TAG_36ARTOOLKIT:
150 break;
151
152 case TAG_25h9:
153 m_tf = tag25h9_create();
154 break;
155
156 case TAG_25h7:
157 m_tf = tag25h7_create();
158 break;
159
160 case TAG_16h5:
161 m_tf = tag16h5_create();
162 break;
163
164 case TAG_CIRCLE21h7:
165 m_tf = tagCircle21h7_create();
166 break;
167
168 case TAG_CIRCLE49h12:
169#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
170 m_tf = tagCircle49h12_create();
171#endif
172 break;
173
174 case TAG_CUSTOM48h12:
175#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
176 m_tf = tagCustom48h12_create();
177#endif
178 break;
179
180 case TAG_STANDARD52h13:
181#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
182 m_tf = tagStandard52h13_create();
183#endif
184 break;
185
186 case TAG_STANDARD41h12:
187#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
188 m_tf = tagStandard41h12_create();
189#endif
190 break;
191
192 default:
193 throw vpException(vpException::fatalError, "Unknown Tag family!");
194 }
195
196 if (m_tagFamily != TAG_36ARTOOLKIT && m_tf) {
197 m_td = apriltag_detector_create();
198 apriltag_detector_add_family(m_td, m_tf);
199 }
200
201 m_mapOfCorrespondingPoseMethods[DEMENTHON_VIRTUAL_VS] = vpPose::DEMENTHON;
202 m_mapOfCorrespondingPoseMethods[LAGRANGE_VIRTUAL_VS] = vpPose::LAGRANGE;
203
204 if (o.m_detections != NULL) {
205 m_detections = apriltag_detections_copy(o.m_detections);
206 }
207 }
208
209 ~Impl()
210 {
211 if (m_td) {
212 apriltag_detector_destroy(m_td);
213 }
214
215 if (m_tf) {
216 switch (m_tagFamily) {
217 case TAG_36h11:
218 tag36h11_destroy(m_tf);
219 break;
220
221 case TAG_36h10:
222 tag36h10_destroy(m_tf);
223 break;
224
225 case TAG_36ARTOOLKIT:
226 break;
227
228 case TAG_25h9:
229 tag25h9_destroy(m_tf);
230 break;
231
232 case TAG_25h7:
233 tag25h7_destroy(m_tf);
234 break;
235
236 case TAG_16h5:
237 tag16h5_destroy(m_tf);
238 break;
239
240 case TAG_CIRCLE21h7:
241 tagCircle21h7_destroy(m_tf);
242 break;
243
244 case TAG_CIRCLE49h12:
245#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
246 tagCustom48h12_destroy(m_tf);
247#endif
248 break;
249
250 case TAG_CUSTOM48h12:
251#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
252 tagCustom48h12_destroy(m_tf);
253#endif
254 break;
255
256 case TAG_STANDARD52h13:
257#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
258 tagStandard52h13_destroy(m_tf);
259#endif
260 break;
261
262 case TAG_STANDARD41h12:
263#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
264 tagStandard41h12_destroy(m_tf);
265#endif
266 break;
267
268 default:
269 break;
270 }
271 }
272
273 if (m_detections) {
274 apriltag_detections_destroy(m_detections);
275 m_detections = NULL;
276 }
277 }
278
279 void convertHomogeneousMatrix(const apriltag_pose_t &pose, vpHomogeneousMatrix &cMo)
280 {
281 for (unsigned int i = 0; i < 3; i++) {
282 for (unsigned int j = 0; j < 3; j++) {
283 cMo[i][j] = MATD_EL(pose.R, i, j);
284 }
285 cMo[i][3] = MATD_EL(pose.t, i, 0);
286 }
287 }
288
289 bool detect(const vpImage<unsigned char> &I, double tagSize, const vpCameraParameters &cam,
290 std::vector<std::vector<vpImagePoint> > &polygons, std::vector<std::string> &messages, bool displayTag,
291 const vpColor color, unsigned int thickness, std::vector<vpHomogeneousMatrix> *cMo_vec,
292 std::vector<vpHomogeneousMatrix> *cMo_vec2, std::vector<double> *projErrors,
293 std::vector<double> *projErrors2)
294 {
295 if (m_tagFamily == TAG_36ARTOOLKIT) {
296 // TAG_36ARTOOLKIT is not available anymore
297 std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
298 return false;
299 }
300#if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
301 if (m_tagFamily == TAG_CIRCLE49h12 || m_tagFamily == TAG_CUSTOM48h12 || m_tagFamily == TAG_STANDARD41h12 ||
302 m_tagFamily == TAG_STANDARD52h13) {
303 std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
304 << std::endl;
305 return false;
306 }
307#endif
308
309 const bool computePose = (cMo_vec != NULL);
310
311 image_u8_t im = {/*.width =*/(int32_t)I.getWidth(),
312 /*.height =*/(int32_t)I.getHeight(),
313 /*.stride =*/(int32_t)I.getWidth(),
314 /*.buf =*/I.bitmap};
315
316 if (m_detections) {
317 apriltag_detections_destroy(m_detections);
318 m_detections = NULL;
319 }
320
321 m_detections = apriltag_detector_detect(m_td, &im);
322 int nb_detections = zarray_size(m_detections);
323 bool detected = nb_detections > 0;
324
325 polygons.resize(static_cast<size_t>(nb_detections));
326 messages.resize(static_cast<size_t>(nb_detections));
327 m_tagsId.resize(static_cast<size_t>(nb_detections));
328
329 for (int i = 0; i < zarray_size(m_detections); i++) {
330 apriltag_detection_t *det;
331 zarray_get(m_detections, i, &det);
332
333 std::vector<vpImagePoint> polygon;
334 for (int j = 0; j < 4; j++) {
335 polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0]));
336 }
337 polygons[static_cast<size_t>(i)] = polygon;
338 std::stringstream ss;
339 ss << m_tagFamily << " id: " << det->id;
340 messages[static_cast<size_t>(i)] = ss.str();
341 m_tagsId[static_cast<size_t>(i)] = det->id;
342
343 if (displayTag) {
344 vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
345 vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
346 vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
347 vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
348
349 vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[1][1], (int)det->p[1][0], Ox,
350 thickness);
351 vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[3][1], (int)det->p[3][0], Oy,
352 thickness);
353 vpDisplay::displayLine(I, (int)det->p[1][1], (int)det->p[1][0], (int)det->p[2][1], (int)det->p[2][0], Ox2,
354 thickness);
355 vpDisplay::displayLine(I, (int)det->p[2][1], (int)det->p[2][0], (int)det->p[3][1], (int)det->p[3][0], Oy2,
356 thickness);
357 }
358
359 if (computePose) {
360 vpHomogeneousMatrix cMo, cMo2;
361 double err1, err2;
362 if (getPose(static_cast<size_t>(i), tagSize, cam, cMo, cMo_vec2 ? &cMo2 : NULL, projErrors ? &err1 : NULL,
363 projErrors2 ? &err2 : NULL)) {
364 cMo_vec->push_back(cMo);
365 if (cMo_vec2) {
366 cMo_vec2->push_back(cMo2);
367 }
368 if (projErrors) {
369 projErrors->push_back(err1);
370 }
371 if (projErrors2) {
372 projErrors2->push_back(err2);
373 }
374 }
375 // else case should never happen
376 }
377 }
378
379 return detected;
380 }
381
382 void displayFrames(const vpImage<unsigned char> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
383 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
384 {
385 for (size_t i = 0; i < cMo_vec.size(); i++) {
386 const vpHomogeneousMatrix &cMo = cMo_vec[i];
387 vpDisplay::displayFrame(I, cMo, cam, size, color, thickness);
388 }
389 }
390
391 void displayFrames(const vpImage<vpRGBa> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
392 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
393 {
394 for (size_t i = 0; i < cMo_vec.size(); i++) {
395 const vpHomogeneousMatrix &cMo = cMo_vec[i];
396 vpDisplay::displayFrame(I, cMo, cam, size, color, thickness);
397 }
398 }
399
400 void displayTags(const vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
401 const vpColor &color, unsigned int thickness) const
402 {
403 for (size_t i = 0; i < tagsCorners.size(); i++) {
404 const vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
405 const vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
406 const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
407 const vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
408
409 const std::vector<vpImagePoint> &corners = tagsCorners[i];
410 assert(corners.size() == 4);
411
412 vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[1].get_i(), (int)corners[1].get_j(),
413 Ox, thickness);
414 vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(),
415 Oy, thickness);
416 vpDisplay::displayLine(I, (int)corners[1].get_i(), (int)corners[1].get_j(), (int)corners[2].get_i(), (int)corners[2].get_j(),
417 Ox2, thickness);
418 vpDisplay::displayLine(I, (int)corners[2].get_i(), (int)corners[2].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(),
419 Oy2, thickness);
420 }
421 }
422
423 void displayTags(const vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
424 const vpColor &color, unsigned int thickness) const
425 {
426 for (size_t i = 0; i < tagsCorners.size(); i++) {
427 const vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
428 const vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
429 const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
430 const vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
431
432 const std::vector<vpImagePoint> &corners = tagsCorners[i];
433 assert(corners.size() == 4);
434
435 vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[1].get_i(), (int)corners[1].get_j(),
436 Ox, thickness);
437 vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(),
438 Oy, thickness);
439 vpDisplay::displayLine(I, (int)corners[1].get_i(), (int)corners[1].get_j(), (int)corners[2].get_i(), (int)corners[2].get_j(),
440 Ox2, thickness);
441 vpDisplay::displayLine(I, (int)corners[2].get_i(), (int)corners[2].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(),
442 Oy2, thickness);
443 }
444 }
445
446 bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
447 vpHomogeneousMatrix *cMo2, double *projErrors, double *projErrors2)
448 {
449 if (m_detections == NULL) {
450 throw(vpException(vpException::fatalError, "Cannot get tag index=%d pose: detection empty", tagIndex));
451 }
452 if (m_tagFamily == TAG_36ARTOOLKIT) {
453 // TAG_36ARTOOLKIT is not available anymore
454 std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
455 return false;
456 }
457#if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
458 if (m_tagFamily == TAG_CIRCLE49h12 || m_tagFamily == TAG_CUSTOM48h12 || m_tagFamily == TAG_STANDARD41h12 ||
459 m_tagFamily == TAG_STANDARD52h13) {
460 std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
461 << std::endl;
462 return false;
463 }
464#endif
465
466 apriltag_detection_t *det;
467 zarray_get(m_detections, static_cast<int>(tagIndex), &det);
468
469 int nb_detections = zarray_size(m_detections);
470 if (tagIndex >= (size_t)nb_detections) {
471 return false;
472 }
473
474 // In AprilTag3, estimate_pose_for_tag_homography() and estimate_tag_pose() have been added.
475 // They use a tag frame aligned with the camera frame
476 // Before the release of AprilTag3, convention used was to define the z-axis of the tag going upward.
477 // To keep compatibility, we maintain the same convention than before and there is setZAlignedWithCameraAxis().
478 // Under the hood, we use aligned frames everywhere and transform the pose according to the option.
479
480 vpHomogeneousMatrix cMo_homography_ortho_iter;
481 if (m_poseEstimationMethod == HOMOGRAPHY_ORTHOGONAL_ITERATION ||
482 m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) {
483 double fx = cam.get_px(), fy = cam.get_py();
484 double cx = cam.get_u0(), cy = cam.get_v0();
485
486 apriltag_detection_info_t info;
487 info.det = det;
488 info.tagsize = tagSize;
489 info.fx = fx;
490 info.fy = fy;
491 info.cx = cx;
492 info.cy = cy;
493
494 // projErrors and projErrors2 will be override later
495 getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
496 cMo_homography_ortho_iter = cMo;
497 }
498
499 vpHomogeneousMatrix cMo_homography;
500 if (m_poseEstimationMethod == HOMOGRAPHY || m_poseEstimationMethod == HOMOGRAPHY_VIRTUAL_VS ||
501 m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) {
502 double fx = cam.get_px(), fy = cam.get_py();
503 double cx = cam.get_u0(), cy = cam.get_v0();
504
505 apriltag_detection_info_t info;
506 info.det = det;
507 info.tagsize = tagSize;
508 info.fx = fx;
509 info.fy = fy;
510 info.cx = cx;
511 info.cy = cy;
512
513 apriltag_pose_t pose;
514 estimate_pose_for_tag_homography(&info, &pose);
515 convertHomogeneousMatrix(pose, cMo);
516
517 matd_destroy(pose.R);
518 matd_destroy(pose.t);
519
520 cMo_homography = cMo;
521 }
522
523 // Add marker object points
524 vpPose pose;
525 vpPoint pt;
526
527 vpImagePoint imPt;
528 double x = 0.0, y = 0.0;
529 std::vector<vpPoint> pts(4);
530 pt.setWorldCoordinates(-tagSize / 2.0, tagSize / 2.0, 0.0);
531 imPt.set_uv(det->p[0][0], det->p[0][1]);
533 pt.set_x(x);
534 pt.set_y(y);
535 pts[0] = pt;
536
537 pt.setWorldCoordinates(tagSize / 2.0, tagSize / 2.0, 0.0);
538 imPt.set_uv(det->p[1][0], det->p[1][1]);
540 pt.set_x(x);
541 pt.set_y(y);
542 pts[1] = pt;
543
544 pt.setWorldCoordinates(tagSize / 2.0, -tagSize / 2.0, 0.0);
545 imPt.set_uv(det->p[2][0], det->p[2][1]);
547 pt.set_x(x);
548 pt.set_y(y);
549 pts[2] = pt;
550
551 pt.setWorldCoordinates(-tagSize / 2.0, -tagSize / 2.0, 0.0);
552 imPt.set_uv(det->p[3][0], det->p[3][1]);
554 pt.set_x(x);
555 pt.set_y(y);
556 pts[3] = pt;
557
558 pose.addPoints(pts);
559
560 if (m_poseEstimationMethod != HOMOGRAPHY && m_poseEstimationMethod != HOMOGRAPHY_VIRTUAL_VS &&
561 m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) {
562 if (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) {
563 vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
564
565 double residual_dementhon = std::numeric_limits<double>::max(),
566 residual_lagrange = std::numeric_limits<double>::max();
567 double residual_homography = pose.computeResidual(cMo_homography);
568 double residual_homography_ortho_iter = pose.computeResidual(cMo_homography_ortho_iter);
569
570 if (pose.computePose(vpPose::DEMENTHON, cMo_dementhon)) {
571 residual_dementhon = pose.computeResidual(cMo_dementhon);
572 }
573
574 if (pose.computePose(vpPose::LAGRANGE, cMo_lagrange)) {
575 residual_lagrange = pose.computeResidual(cMo_lagrange);
576 }
577
578 std::vector<double> residuals;
579 residuals.push_back(residual_dementhon);
580 residuals.push_back(residual_lagrange);
581 residuals.push_back(residual_homography);
582 residuals.push_back(residual_homography_ortho_iter);
583 std::vector<vpHomogeneousMatrix> poses;
584 poses.push_back(cMo_dementhon);
585 poses.push_back(cMo_lagrange);
586 poses.push_back(cMo_homography);
587 poses.push_back(cMo_homography_ortho_iter);
588
589 std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
590 cMo = *(poses.begin() + minIndex);
591 } else {
592 pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo);
593 }
594 }
595
596 if (m_poseEstimationMethod != HOMOGRAPHY && m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) {
597 // Compute final pose using VVS
599 }
600
601 // Only with HOMOGRAPHY_ORTHOGONAL_ITERATION we can directly get two solutions
602 if (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) {
603 if (cMo2) {
604 double scale = tagSize / 2.0;
605 double data_p0[] = {-scale, scale, 0};
606 double data_p1[] = {scale, scale, 0};
607 double data_p2[] = {scale, -scale, 0};
608 double data_p3[] = {-scale, -scale, 0};
609 matd_t *p[4] = {matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1),
610 matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3)};
611 matd_t *v[4];
612 for (int i = 0; i < 4; i++) {
613 double data_v[] = {(det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(),
614 1};
615 v[i] = matd_create_data(3, 1, data_v);
616 }
617
618 apriltag_pose_t solution1, solution2;
619 const int nIters = 50;
620 solution1.R = matd_create_data(3, 3, cMo.getRotationMatrix().data);
621 solution1.t = matd_create_data(3, 1, cMo.getTranslationVector().data);
622
623 double err2;
624 get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
625
626 for (int i = 0; i < 4; i++) {
627 matd_destroy(p[i]);
628 matd_destroy(v[i]);
629 }
630
631 if (solution2.R) {
632 convertHomogeneousMatrix(solution2, *cMo2);
633
634 matd_destroy(solution2.R);
635 matd_destroy(solution2.t);
636 }
637
638 matd_destroy(solution1.R);
639 matd_destroy(solution1.t);
640 }
641 }
642
643 // Compute projection error with vpPose::computeResidual() for consistency
644 if (projErrors) {
645 *projErrors = pose.computeResidual(cMo);
646 }
647 if (projErrors2 && cMo2) {
648 *projErrors2 = pose.computeResidual(*cMo2);
649 }
650
651 if (!m_zAlignedWithCameraFrame) {
653 // Apply a rotation of 180deg around x axis
654 oMo[0][0] = 1;
655 oMo[0][1] = 0;
656 oMo[0][2] = 0;
657 oMo[1][0] = 0;
658 oMo[1][1] = -1;
659 oMo[1][2] = 0;
660 oMo[2][0] = 0;
661 oMo[2][1] = 0;
662 oMo[2][2] = -1;
663 cMo = cMo * oMo;
664 if (cMo2) {
665 *cMo2 = *cMo2 * oMo;
666 }
667 }
668
669 return true;
670 }
671
672 void getPoseWithOrthogonalMethod(apriltag_detection_info_t &info, vpHomogeneousMatrix &cMo1,
673 vpHomogeneousMatrix *cMo2, double *err1, double *err2)
674 {
675 apriltag_pose_t pose1, pose2;
676 double err_1, err_2;
677 estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50);
678 if (err_1 <= err_2) {
679 convertHomogeneousMatrix(pose1, cMo1);
680 if (cMo2) {
681 if (pose2.R) {
682 convertHomogeneousMatrix(pose2, *cMo2);
683 } else {
684 *cMo2 = cMo1;
685 }
686 }
687 } else {
688 convertHomogeneousMatrix(pose2, cMo1);
689 if (cMo2) {
690 convertHomogeneousMatrix(pose1, *cMo2);
691 }
692 }
693
694 matd_destroy(pose1.R);
695 matd_destroy(pose1.t);
696 if (pose2.R) {
697 matd_destroy(pose2.t);
698 }
699 matd_destroy(pose2.R);
700
701 if (err1)
702 *err1 = err_1;
703 if (err2)
704 *err2 = err_2;
705 }
706
707 bool getZAlignedWithCameraAxis() { return m_zAlignedWithCameraFrame; }
708
709 bool getAprilTagDecodeSharpening(double &decodeSharpening) const
710 {
711 if (m_td) {
712 decodeSharpening = m_td->decode_sharpening;
713 return true;
714 }
715 return false;
716 }
717
718 bool getNbThreads(int &nThreads) const
719 {
720 if (m_td) {
721 nThreads = m_td->nthreads;
722 return true;
723 }
724 return false;
725 }
726
727 bool getQuadDecimate(float &quadDecimate) const
728 {
729 if (m_td) {
730 quadDecimate = m_td->quad_decimate;
731 return true;
732 }
733 return false;
734 }
735
736 bool getQuadSigma(float &quadSigma) const
737 {
738 if (m_td) {
739 quadSigma = m_td->quad_sigma;
740 return true;
741 }
742 return false;
743 }
744
745 bool getRefineEdges(bool &refineEdges) const
746 {
747 if (m_td) {
748 refineEdges = (m_td->refine_edges ? true : false);
749 return true;
750 }
751 return false;
752 }
753
754 bool getZAlignedWithCameraAxis() const { return m_zAlignedWithCameraFrame; }
755
756 std::vector<int> getTagsId() const { return m_tagsId; }
757
758 void setAprilTagDecodeSharpening(double decodeSharpening)
759 {
760 if (m_td) {
761 m_td->decode_sharpening = decodeSharpening;
762 }
763 }
764
765 void setNbThreads(int nThreads)
766 {
767 if (m_td) {
768 m_td->nthreads = nThreads;
769 }
770 }
771
772 void setQuadDecimate(float quadDecimate)
773 {
774 if (m_td) {
775 m_td->quad_decimate = quadDecimate;
776 }
777 }
778
779 void setQuadSigma(float quadSigma)
780 {
781 if (m_td) {
782 m_td->quad_sigma = quadSigma;
783 }
784 }
785
786 void setRefineDecode(bool) {}
787
788 void setRefineEdges(bool refineEdges)
789 {
790 if (m_td) {
791 m_td->refine_edges = refineEdges ? 1 : 0;
792 }
793 }
794
795 void setRefinePose(bool) {}
796
797 void setPoseEstimationMethod(const vpPoseEstimationMethod &method) { m_poseEstimationMethod = method; }
798
799 void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame) { m_zAlignedWithCameraFrame = zAlignedWithCameraFrame; }
800
801protected:
802 std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
803 vpPoseEstimationMethod m_poseEstimationMethod;
804 std::vector<int> m_tagsId;
805 vpAprilTagFamily m_tagFamily;
806 apriltag_detector_t *m_td;
807 apriltag_family_t *m_tf;
808 zarray_t *m_detections;
809 bool m_zAlignedWithCameraFrame;
810};
811#endif // DOXYGEN_SHOULD_SKIP_THIS
812
814 const vpPoseEstimationMethod &poseEstimationMethod)
815 : m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2),
816 m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(),
817 m_impl(new Impl(tagFamily, poseEstimationMethod))
818{
819}
820
822 : vpDetectorBase(o), m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2),
823 m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(),
824 m_impl(new Impl(*o.m_impl))
825{
826}
827
829{
830 swap(*this, o);
831 return *this;
832}
833
835
844{
845 m_message.clear();
846 m_polygon.clear();
847 m_nb_objects = 0;
848
849 std::vector<vpHomogeneousMatrix> cMo_vec;
850 const double tagSize = 1.0;
851 bool detected = m_impl->detect(I, tagSize, m_defaultCam, m_polygon, m_message, m_displayTag, m_displayTagColor,
852 m_displayTagThickness, NULL, NULL, NULL, NULL);
853 m_nb_objects = m_message.size();
854
855 return detected;
856}
857
876 std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
877 std::vector<double> *projErrors, std::vector<double> *projErrors2)
878{
879 m_message.clear();
880 m_polygon.clear();
881 m_nb_objects = 0;
882
883 cMo_vec.clear();
884 if (cMo_vec2) {
885 cMo_vec2->clear();
886 }
887 bool detected = m_impl->detect(I, tagSize, cam, m_polygon, m_message, m_displayTag, m_displayTagColor,
888 m_displayTagThickness, &cMo_vec, cMo_vec2, projErrors, projErrors2);
889 m_nb_objects = m_message.size();
890
891 return detected;
892}
893
904void vpDetectorAprilTag::displayFrames(const vpImage<unsigned char> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
905 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
906{
907 m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
908}
909
920void vpDetectorAprilTag::displayFrames(const vpImage<vpRGBa> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
921 const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
922{
923 m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
924}
925
934void vpDetectorAprilTag::displayTags(const vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
935 const vpColor &color, unsigned int thickness) const
936{
937 m_impl->displayTags(I, tagsCorners, color, thickness);
938}
939
948void vpDetectorAprilTag::displayTags(const vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
949 const vpColor &color, unsigned int thickness) const
950{
951 m_impl->displayTags(I, tagsCorners, color, thickness);
952}
953
985bool vpDetectorAprilTag::getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam,
986 vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2, double *projError,
987 double *projError2)
988{
989 return m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2);
990}
991
1009std::vector<std::vector<vpPoint> > vpDetectorAprilTag::getTagsPoints3D(const std::vector<int> &tagsId,
1010 const std::map<int, double> &tagsSize) const
1011{
1012 std::vector<std::vector<vpPoint> > tagsPoints3D;
1013
1014 double default_size = -1;
1015 {
1016 std::map<int, double>::const_iterator it = tagsSize.find(-1);
1017 if (it != tagsSize.end()) {
1018 default_size = it->second; // Default size
1019 }
1020 }
1021 for (size_t i = 0; i < tagsId.size(); i++) {
1022 std::map<int, double>::const_iterator it = tagsSize.find(tagsId[i]);
1023 double tagSize = default_size; // Default size
1024 if (it == tagsSize.end()) {
1025 if (default_size < 0) { // no default size found
1027 "Tag with id %d has no 3D size or there is no default 3D size defined", tagsId[i]));
1028 }
1029 } else {
1030 tagSize = it->second;
1031 }
1032 std::vector<vpPoint> points3D(4);
1033 if (m_impl->getZAlignedWithCameraAxis()) {
1034 points3D[0] = vpPoint(-tagSize / 2, tagSize / 2, 0);
1035 points3D[1] = vpPoint(tagSize / 2, tagSize / 2, 0);
1036 points3D[2] = vpPoint(tagSize / 2, -tagSize / 2, 0);
1037 points3D[3] = vpPoint(-tagSize / 2, -tagSize / 2, 0);
1038 } else {
1039 points3D[0] = vpPoint(-tagSize / 2, -tagSize / 2, 0);
1040 points3D[1] = vpPoint(tagSize / 2, -tagSize / 2, 0);
1041 points3D[2] = vpPoint(tagSize / 2, tagSize / 2, 0);
1042 points3D[3] = vpPoint(-tagSize / 2, tagSize / 2, 0);
1043 }
1044 tagsPoints3D.push_back(points3D);
1045 }
1046
1047 return tagsPoints3D;
1048}
1049
1055std::vector<std::vector<vpImagePoint> > vpDetectorAprilTag::getTagsCorners() const { return m_polygon; }
1056
1062std::vector<int> vpDetectorAprilTag::getTagsId() const { return m_impl->getTagsId(); }
1063
1065{
1066 return m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1067}
1068
1070{
1071 // back-up settings
1072 double decodeSharpening = 0.25;
1073 m_impl->getAprilTagDecodeSharpening(decodeSharpening);
1074 int nThreads = 1;
1075 m_impl->getNbThreads(nThreads);
1076 float quadDecimate = 1;
1077 m_impl->getQuadDecimate(quadDecimate);
1078 float quadSigma = 0;
1079 m_impl->getQuadSigma(quadSigma);
1080 bool refineEdges = true;
1081 m_impl->getRefineEdges(refineEdges);
1082 bool zAxis = m_impl->getZAlignedWithCameraAxis();
1083
1084 delete m_impl;
1085 m_impl = new Impl(tagFamily, m_poseEstimationMethod);
1086 m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1087 m_impl->setNbThreads(nThreads);
1088 m_impl->setQuadDecimate(quadDecimate);
1089 m_impl->setQuadSigma(quadSigma);
1090 m_impl->setRefineEdges(refineEdges);
1091 m_impl->setZAlignedWithCameraAxis(zAxis);
1092}
1093
1100{
1101 if (nThreads > 0) {
1102 m_impl->setNbThreads(nThreads);
1103 }
1104}
1105
1112{
1113 m_poseEstimationMethod = poseEstimationMethod;
1114 m_impl->setPoseEstimationMethod(poseEstimationMethod);
1115}
1116
1129void vpDetectorAprilTag::setAprilTagQuadDecimate(float quadDecimate) { m_impl->setQuadDecimate(quadDecimate); }
1130
1143void vpDetectorAprilTag::setAprilTagQuadSigma(float quadSigma) { m_impl->setQuadSigma(quadSigma); }
1144
1145#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1149vp_deprecated void vpDetectorAprilTag::setAprilTagRefineDecode(bool refineDecode)
1150{
1151 m_impl->setRefineDecode(refineDecode);
1152}
1153#endif
1154
1169void vpDetectorAprilTag::setAprilTagRefineEdges(bool refineEdges) { m_impl->setRefineEdges(refineEdges); }
1170
1171#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1175vp_deprecated void vpDetectorAprilTag::setAprilTagRefinePose(bool refinePose) { m_impl->setRefinePose(refinePose); }
1176#endif
1177
1179{
1180 using std::swap;
1181
1182 swap(o1.m_impl, o2.m_impl);
1183}
1184
1190void vpDetectorAprilTag::setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
1191{
1192 m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
1193}
1194
1195#elif !defined(VISP_BUILD_SHARED_LIBS)
1196// Work around to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has
1197// no symbols
1198void dummy_vpDetectorAprilTag() {}
1199#endif
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
Generic class defining intrinsic camera parameters.
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 yellow
Definition vpColor.h:219
static const vpColor green
Definition vpColor.h:214
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
void setAprilTagQuadDecimate(float quadDecimate)
friend void swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2)
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
vpDetectorAprilTag & operator=(vpDetectorAprilTag o)
std::vector< std::vector< vpPoint > > getTagsPoints3D(const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const
bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=NULL, double *projError=NULL, double *projError2=NULL)
unsigned int m_displayTagThickness
void setAprilTagRefineDecode(bool refineDecode)
void setAprilTagRefineEdges(bool refineEdges)
vpPoseEstimationMethod m_poseEstimationMethod
void setAprilTagQuadSigma(float quadSigma)
void setAprilTagNbThreads(int nThreads)
bool detect(const vpImage< unsigned char > &I)
vpDetectorAprilTag(const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
std::vector< int > getTagsId() const
void displayTags(const vpImage< unsigned char > &I, const std::vector< std::vector< vpImagePoint > > &tagsCorners, const vpColor &color=vpColor::none, unsigned int thickness=1) const
void setAprilTagFamily(const vpAprilTagFamily &tagFamily)
void setAprilTagRefinePose(bool refinePose)
void setAprilTagDecodeSharpening(double decodeSharpening)
std::vector< std::string > m_message
Message attached to each object.
std::vector< std::vector< vpImagePoint > > m_polygon
For each object, defines the polygon that contains the object.
size_t m_nb_objects
Number of detected objects.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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))
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_uv(double u, double v)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
unsigned int getHeight() const
Definition vpImage.h:184
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
@ DEMENTHON
Definition vpPose.h:87
@ VIRTUAL_VS
Definition vpPose.h:96
@ LAGRANGE
Definition vpPose.h:86
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:155
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:369
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469