Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
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
64 class vpDetectorAprilTag::Impl
65 {
66 public:
67  Impl(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &method)
68  : m_poseEstimationMethod(method), m_tagsId(), m_tagFamily(tagFamily), m_td(nullptr), m_tf(nullptr), m_detections(nullptr),
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(nullptr),
138  m_tf(nullptr), m_detections(nullptr), 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 != nullptr) {
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 = nullptr;
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)
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 != nullptr);
310 
311  image_u8_t im = {/*.width =*/static_cast<int32_t>(I.getWidth()),
312  /*.height =*/static_cast<int32_t>(I.getHeight()),
313  /*.stride =*/static_cast<int32_t>(I.getWidth()),
314  /*.buf =*/I.bitmap };
315 
316  if (m_detections) {
317  apriltag_detections_destroy(m_detections);
318  m_detections = nullptr;
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  int zarray_size_m_detections = zarray_size(m_detections);
330  for (int i = 0; i < zarray_size_m_detections; ++i) {
331  apriltag_detection_t *det;
332  zarray_get(m_detections, i, &det);
333 
334  std::vector<vpImagePoint> polygon;
335  for (int j = 0; j < 4; ++j) {
336  polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0]));
337  }
338  polygons[static_cast<size_t>(i)] = polygon;
339  std::stringstream ss;
340  ss << m_tagFamily << " id: " << det->id;
341  messages[static_cast<size_t>(i)] = ss.str();
342  m_tagsId[static_cast<size_t>(i)] = det->id;
343 
344  if (displayTag) {
345  vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
346  vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
347  vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
348  vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
349 
350  vpDisplay::displayLine(I, static_cast<int>(det->p[0][1]), static_cast<int>(det->p[0][0]), static_cast<int>(det->p[1][1]), static_cast<int>(det->p[1][0]), Ox,
351  thickness);
352  vpDisplay::displayLine(I, static_cast<int>(det->p[0][1]), static_cast<int>(det->p[0][0]), static_cast<int>(det->p[3][1]), static_cast<int>(det->p[3][0]), Oy,
353  thickness);
354  vpDisplay::displayLine(I, static_cast<int>(det->p[1][1]), static_cast<int>(det->p[1][0]), static_cast<int>(det->p[2][1]), static_cast<int>(det->p[2][0]), Ox2,
355  thickness);
356  vpDisplay::displayLine(I, static_cast<int>(det->p[2][1]), static_cast<int>(det->p[2][0]), static_cast<int>(det->p[3][1]), static_cast<int>(det->p[3][0]), Oy2,
357  thickness);
358  }
359 
360  if (computePose) {
361  vpHomogeneousMatrix cMo, cMo2;
362  double err1, err2;
363  if (getPose(static_cast<size_t>(i), tagSize, cam, cMo, cMo_vec2 ? &cMo2 : nullptr, projErrors ? &err1 : nullptr,
364  projErrors2 ? &err2 : nullptr)) {
365  cMo_vec->push_back(cMo);
366  if (cMo_vec2) {
367  cMo_vec2->push_back(cMo2);
368  }
369  if (projErrors) {
370  projErrors->push_back(err1);
371  }
372  if (projErrors2) {
373  projErrors2->push_back(err2);
374  }
375  }
376  // else case should never happen
377  }
378  }
379 
380  return detected;
381  }
382 
383  void displayFrames(const vpImage<unsigned char> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
384  const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
385  {
386  size_t cmo_vec_size = cMo_vec.size();
387  for (size_t i = 0; i < cmo_vec_size; ++i) {
388  const vpHomogeneousMatrix &cMo = cMo_vec[i];
389  vpDisplay::displayFrame(I, cMo, cam, size, color, thickness);
390  }
391  }
392 
393  void displayFrames(const vpImage<vpRGBa> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
394  const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
395  {
396  size_t cmo_vec_size = cMo_vec.size();
397  for (size_t i = 0; i < cmo_vec_size; ++i) {
398  const vpHomogeneousMatrix &cMo = cMo_vec[i];
399  vpDisplay::displayFrame(I, cMo, cam, size, color, thickness);
400  }
401  }
402 
403  void displayTags(const vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
404  const vpColor &color, unsigned int thickness) const
405  {
406  size_t tagscorners_size = tagsCorners.size();
407  for (size_t i = 0; i < tagscorners_size; ++i) {
408  const vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
409  const vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
410  const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
411  const vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
412 
413  const std::vector<vpImagePoint> &corners = tagsCorners[i];
414  assert(corners.size() == 4);
415 
416  vpDisplay::displayLine(I, static_cast<int>(corners[0].get_i()), static_cast<int>(corners[0].get_j()), static_cast<int>(corners[1].get_i()), static_cast<int>(corners[1].get_j()),
417  Ox, thickness);
418  vpDisplay::displayLine(I, static_cast<int>(corners[0].get_i()), static_cast<int>(corners[0].get_j()), static_cast<int>(corners[3].get_i()), static_cast<int>(corners[3].get_j()),
419  Oy, thickness);
420  vpDisplay::displayLine(I, static_cast<int>(corners[1].get_i()), static_cast<int>(corners[1].get_j()), static_cast<int>(corners[2].get_i()), static_cast<int>(corners[2].get_j()),
421  Ox2, thickness);
422  vpDisplay::displayLine(I, static_cast<int>(corners[2].get_i()), static_cast<int>(corners[2].get_j()), static_cast<int>(corners[3].get_i()), static_cast<int>(corners[3].get_j()),
423  Oy2, thickness);
424  }
425  }
426 
427  void displayTags(const vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
428  const vpColor &color, unsigned int thickness) const
429  {
430  size_t tagscorners_size = tagsCorners.size();
431  for (size_t i = 0; i < tagscorners_size; ++i) {
432  const vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
433  const vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
434  const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
435  const vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
436 
437  const std::vector<vpImagePoint> &corners = tagsCorners[i];
438  assert(corners.size() == 4);
439 
440  vpDisplay::displayLine(I, static_cast<int>(corners[0].get_i()), static_cast<int>(corners[0].get_j()), static_cast<int>(corners[1].get_i()), static_cast<int>(corners[1].get_j()),
441  Ox, thickness);
442  vpDisplay::displayLine(I, static_cast<int>(corners[0].get_i()), static_cast<int>(corners[0].get_j()), static_cast<int>(corners[3].get_i()), static_cast<int>(corners[3].get_j()),
443  Oy, thickness);
444  vpDisplay::displayLine(I, static_cast<int>(corners[1].get_i()), static_cast<int>(corners[1].get_j()), static_cast<int>(corners[2].get_i()), static_cast<int>(corners[2].get_j()),
445  Ox2, thickness);
446  vpDisplay::displayLine(I, static_cast<int>(corners[2].get_i()), static_cast<int>(corners[2].get_j()), static_cast<int>(corners[3].get_i()), static_cast<int>(corners[3].get_j()),
447  Oy2, thickness);
448  }
449  }
450 
451  bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
452  vpHomogeneousMatrix *cMo2, double *projErrors, double *projErrors2)
453  {
454  if (m_detections == nullptr) {
455  throw(vpException(vpException::fatalError, "Cannot get tag index=%d pose: detection empty", tagIndex));
456  }
457  if (m_tagFamily == TAG_36ARTOOLKIT) {
458  // TAG_36ARTOOLKIT is not available anymore
459  std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
460  return false;
461  }
462 #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
465  std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
466  << std::endl;
467  return false;
468  }
469 #endif
470 
471  apriltag_detection_t *det;
472  zarray_get(m_detections, static_cast<int>(tagIndex), &det);
473 
474  int nb_detections = zarray_size(m_detections);
475  if (tagIndex >= static_cast<size_t>(nb_detections)) {
476  return false;
477  }
478 
479  // In AprilTag3, estimate_pose_for_tag_homography() and estimate_tag_pose() have been added.
480  // They use a tag frame aligned with the camera frame
481  // Before the release of AprilTag3, convention used was to define the z-axis of the tag going upward.
482  // To keep compatibility, we maintain the same convention than before and there is setZAlignedWithCameraAxis().
483  // Under the hood, we use aligned frames everywhere and transform the pose according to the option.
484 
485  vpHomogeneousMatrix cMo_homography_ortho_iter;
488  double fx = cam.get_px(), fy = cam.get_py();
489  double cx = cam.get_u0(), cy = cam.get_v0();
490 
491  apriltag_detection_info_t info;
492  info.det = det;
493  info.tagsize = tagSize;
494  info.fx = fx;
495  info.fy = fy;
496  info.cx = cx;
497  info.cy = cy;
498 
499  // projErrors and projErrors2 will be override later
500  getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
501  cMo_homography_ortho_iter = cMo;
502  }
503 
504  vpHomogeneousMatrix cMo_homography;
507  double fx = cam.get_px(), fy = cam.get_py();
508  double cx = cam.get_u0(), cy = cam.get_v0();
509 
510  apriltag_detection_info_t info;
511  info.det = det;
512  info.tagsize = tagSize;
513  info.fx = fx;
514  info.fy = fy;
515  info.cx = cx;
516  info.cy = cy;
517 
518  apriltag_pose_t pose;
519  estimate_pose_for_tag_homography(&info, &pose);
520  convertHomogeneousMatrix(pose, cMo);
521 
522  matd_destroy(pose.R);
523  matd_destroy(pose.t);
524 
525  cMo_homography = cMo;
526  }
527 
528  // Add marker object points
529  vpPose pose;
530  vpPoint pt;
531 
532  vpImagePoint imPt;
533  double x = 0.0, y = 0.0;
534  std::vector<vpPoint> pts(4);
535  pt.setWorldCoordinates(-tagSize / 2.0, tagSize / 2.0, 0.0);
536  imPt.set_uv(det->p[0][0], det->p[0][1]);
537  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
538  pt.set_x(x);
539  pt.set_y(y);
540  pts[0] = pt;
541 
542  pt.setWorldCoordinates(tagSize / 2.0, tagSize / 2.0, 0.0);
543  imPt.set_uv(det->p[1][0], det->p[1][1]);
544  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
545  pt.set_x(x);
546  pt.set_y(y);
547  pts[1] = pt;
548 
549  pt.setWorldCoordinates(tagSize / 2.0, -tagSize / 2.0, 0.0);
550  imPt.set_uv(det->p[2][0], det->p[2][1]);
551  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
552  pt.set_x(x);
553  pt.set_y(y);
554  pts[2] = pt;
555 
556  pt.setWorldCoordinates(-tagSize / 2.0, -tagSize / 2.0, 0.0);
557  imPt.set_uv(det->p[3][0], det->p[3][1]);
558  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
559  pt.set_x(x);
560  pt.set_y(y);
561  pts[3] = pt;
562 
563  pose.addPoints(pts);
564 
568  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
569 
570  double residual_dementhon = std::numeric_limits<double>::max(),
571  residual_lagrange = std::numeric_limits<double>::max();
572  double residual_homography = pose.computeResidual(cMo_homography);
573  double residual_homography_ortho_iter = pose.computeResidual(cMo_homography_ortho_iter);
574 
575  if (pose.computePose(vpPose::DEMENTHON, cMo_dementhon)) {
576  residual_dementhon = pose.computeResidual(cMo_dementhon);
577  }
578 
579  if (pose.computePose(vpPose::LAGRANGE, cMo_lagrange)) {
580  residual_lagrange = pose.computeResidual(cMo_lagrange);
581  }
582 
583  std::vector<double> residuals;
584  residuals.push_back(residual_dementhon);
585  residuals.push_back(residual_lagrange);
586  residuals.push_back(residual_homography);
587  residuals.push_back(residual_homography_ortho_iter);
588  std::vector<vpHomogeneousMatrix> poses;
589  poses.push_back(cMo_dementhon);
590  poses.push_back(cMo_lagrange);
591  poses.push_back(cMo_homography);
592  poses.push_back(cMo_homography_ortho_iter);
593 
594  std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
595  cMo = *(poses.begin() + minIndex);
596  }
597  else {
598  pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo);
599  }
600  }
601 
603  // Compute final pose using VVS
604  pose.computePose(vpPose::VIRTUAL_VS, cMo);
605  }
606 
607  // Only with HOMOGRAPHY_ORTHOGONAL_ITERATION we can directly get two solutions
609  if (cMo2) {
610  double scale = tagSize / 2.0;
611  double data_p0[] = { -scale, scale, 0 };
612  double data_p1[] = { scale, scale, 0 };
613  double data_p2[] = { scale, -scale, 0 };
614  double data_p3[] = { -scale, -scale, 0 };
615  matd_t *p[4] = { matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1),
616  matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3) };
617  matd_t *v[4];
618  for (int i = 0; i < 4; ++i) {
619  double data_v[] = { (det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(),
620  1 };
621  v[i] = matd_create_data(3, 1, data_v);
622  }
623 
624  apriltag_pose_t solution1, solution2;
625  const int nIters = 50;
626  solution1.R = matd_create_data(3, 3, cMo.getRotationMatrix().data);
627  solution1.t = matd_create_data(3, 1, cMo.getTranslationVector().data);
628 
629  double err2;
630  get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
631 
632  for (int i = 0; i < 4; ++i) {
633  matd_destroy(p[i]);
634  matd_destroy(v[i]);
635  }
636 
637  if (solution2.R) {
638  convertHomogeneousMatrix(solution2, *cMo2);
639 
640  matd_destroy(solution2.R);
641  matd_destroy(solution2.t);
642  }
643 
644  matd_destroy(solution1.R);
645  matd_destroy(solution1.t);
646  }
647  }
648 
649  // Compute projection error with vpPose::computeResidual() for consistency
650  if (projErrors) {
651  *projErrors = pose.computeResidual(cMo);
652  }
653  if (projErrors2 && cMo2) {
654  *projErrors2 = pose.computeResidual(*cMo2);
655  }
656 
657  if (!m_zAlignedWithCameraFrame) {
659  // Apply a rotation of 180deg around x axis
660  oMo[0][0] = 1;
661  oMo[0][1] = 0;
662  oMo[0][2] = 0;
663  oMo[1][0] = 0;
664  oMo[1][1] = -1;
665  oMo[1][2] = 0;
666  oMo[2][0] = 0;
667  oMo[2][1] = 0;
668  oMo[2][2] = -1;
669  cMo = cMo * oMo;
670  if (cMo2) {
671  *cMo2 = *cMo2 * oMo;
672  }
673  }
674 
675  return true;
676  }
677 
678  void getPoseWithOrthogonalMethod(apriltag_detection_info_t &info, vpHomogeneousMatrix &cMo1,
679  vpHomogeneousMatrix *cMo2, double *err1, double *err2)
680  {
681  apriltag_pose_t pose1, pose2;
682  double err_1, err_2;
683  estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50);
684  if (err_1 <= err_2) {
685  convertHomogeneousMatrix(pose1, cMo1);
686  if (cMo2) {
687  if (pose2.R) {
688  convertHomogeneousMatrix(pose2, *cMo2);
689  }
690  else {
691  *cMo2 = cMo1;
692  }
693  }
694  }
695  else {
696  convertHomogeneousMatrix(pose2, cMo1);
697  if (cMo2) {
698  convertHomogeneousMatrix(pose1, *cMo2);
699  }
700  }
701 
702  matd_destroy(pose1.R);
703  matd_destroy(pose1.t);
704  if (pose2.R) {
705  matd_destroy(pose2.t);
706  }
707  matd_destroy(pose2.R);
708 
709  if (err1) {
710  *err1 = err_1;
711  }
712  if (err2) {
713  *err2 = err_2;
714  }
715  }
716 
717  bool getZAlignedWithCameraAxis() { return m_zAlignedWithCameraFrame; }
718 
719  bool getAprilTagDecodeSharpening(double &decodeSharpening) const
720  {
721  if (m_td) {
722  decodeSharpening = m_td->decode_sharpening;
723  return true;
724  }
725  return false;
726  }
727 
728  bool getNbThreads(int &nThreads) const
729  {
730  if (m_td) {
731  nThreads = m_td->nthreads;
732  return true;
733  }
734  return false;
735  }
736 
737  bool getQuadDecimate(float &quadDecimate) const
738  {
739  if (m_td) {
740  quadDecimate = m_td->quad_decimate;
741  return true;
742  }
743  return false;
744  }
745 
746  bool getQuadSigma(float &quadSigma) const
747  {
748  if (m_td) {
749  quadSigma = m_td->quad_sigma;
750  return true;
751  }
752  return false;
753  }
754 
755  bool getRefineEdges(bool &refineEdges) const
756  {
757  if (m_td) {
758  refineEdges = (m_td->refine_edges ? true : false);
759  return true;
760  }
761  return false;
762  }
763 
764  bool getZAlignedWithCameraAxis() const { return m_zAlignedWithCameraFrame; }
765 
766  std::vector<int> getTagsId() const { return m_tagsId; }
767 
768  void setAprilTagDecodeSharpening(double decodeSharpening)
769  {
770  if (m_td) {
771  m_td->decode_sharpening = decodeSharpening;
772  }
773  }
774 
775  void setNbThreads(int nThreads)
776  {
777  if (m_td) {
778  m_td->nthreads = nThreads;
779  }
780  }
781 
782  void setQuadDecimate(float quadDecimate)
783  {
784  if (m_td) {
785  m_td->quad_decimate = quadDecimate;
786  }
787  }
788 
789  void setQuadSigma(float quadSigma)
790  {
791  if (m_td) {
792  m_td->quad_sigma = quadSigma;
793  }
794  }
795 
796  void setRefineDecode(bool) { }
797 
798  void setRefineEdges(bool refineEdges)
799  {
800  if (m_td) {
801  m_td->refine_edges = refineEdges ? 1 : 0;
802  }
803  }
804 
805  void setRefinePose(bool) { }
806 
807  void setPoseEstimationMethod(const vpPoseEstimationMethod &method) { m_poseEstimationMethod = method; }
808 
809  void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame) { m_zAlignedWithCameraFrame = zAlignedWithCameraFrame; }
810 
811 protected:
812  std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
814  std::vector<int> m_tagsId;
816  apriltag_detector_t *m_td;
817  apriltag_family_t *m_tf;
818  zarray_t *m_detections;
819  bool m_zAlignedWithCameraFrame;
820 };
821 #endif // DOXYGEN_SHOULD_SKIP_THIS
822 
824  const vpPoseEstimationMethod &poseEstimationMethod)
825  : m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2),
826  m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(),
827  m_impl(new Impl(tagFamily, poseEstimationMethod))
828 { }
829 
831  : vpDetectorBase(o), m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2),
832  m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(),
833  m_impl(new Impl(*o.m_impl))
834 { }
835 
837 {
838  swap(*this, o);
839  return *this;
840 }
841 
843 
852 {
853  m_message.clear();
854  m_polygon.clear();
855  m_nb_objects = 0;
856 
857  std::vector<vpHomogeneousMatrix> cMo_vec;
858  const double tagSize = 1.0;
859  bool detected = m_impl->detect(I, tagSize, m_defaultCam, m_polygon, m_message, m_displayTag, m_displayTagColor,
860  m_displayTagThickness, nullptr, nullptr, nullptr, nullptr);
861  m_nb_objects = m_message.size();
862 
863  return detected;
864 }
865 
883 bool vpDetectorAprilTag::detect(const vpImage<unsigned char> &I, double tagSize, const vpCameraParameters &cam,
884  std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
885  std::vector<double> *projErrors, std::vector<double> *projErrors2)
886 {
887  m_message.clear();
888  m_polygon.clear();
889  m_nb_objects = 0;
890 
891  cMo_vec.clear();
892  if (cMo_vec2) {
893  cMo_vec2->clear();
894  }
895  bool detected = m_impl->detect(I, tagSize, cam, m_polygon, m_message, m_displayTag, m_displayTagColor,
896  m_displayTagThickness, &cMo_vec, cMo_vec2, projErrors, projErrors2);
897  m_nb_objects = m_message.size();
898 
899  return detected;
900 }
901 
912 void vpDetectorAprilTag::displayFrames(const vpImage<unsigned char> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
913  const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
914 {
915  m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
916 }
917 
928 void vpDetectorAprilTag::displayFrames(const vpImage<vpRGBa> &I, const std::vector<vpHomogeneousMatrix> &cMo_vec,
929  const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const
930 {
931  m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
932 }
933 
942 void vpDetectorAprilTag::displayTags(const vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
943  const vpColor &color, unsigned int thickness) const
944 {
945  m_impl->displayTags(I, tagsCorners, color, thickness);
946 }
947 
956 void vpDetectorAprilTag::displayTags(const vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &tagsCorners,
957  const vpColor &color, unsigned int thickness) const
958 {
959  m_impl->displayTags(I, tagsCorners, color, thickness);
960 }
961 
993 bool vpDetectorAprilTag::getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam,
994  vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2, double *projError,
995  double *projError2)
996 {
997  return m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2);
998 }
999 
1017 std::vector<std::vector<vpPoint> > vpDetectorAprilTag::getTagsPoints3D(const std::vector<int> &tagsId,
1018  const std::map<int, double> &tagsSize) const
1019 {
1020  std::vector<std::vector<vpPoint> > tagsPoints3D;
1021 
1022  double default_size = -1;
1023  {
1024  std::map<int, double>::const_iterator it = tagsSize.find(-1);
1025  if (it != tagsSize.end()) {
1026  default_size = it->second; // Default size
1027  }
1028  }
1029  size_t tagsid_size = tagsId.size();
1030  for (size_t i = 0; i < tagsid_size; ++i) {
1031  std::map<int, double>::const_iterator it = tagsSize.find(tagsId[i]);
1032  double tagSize = default_size; // Default size
1033  if (it == tagsSize.end()) {
1034  if (default_size < 0) { // no default size found
1036  "Tag with id %d has no 3D size or there is no default 3D size defined", tagsId[i]));
1037  }
1038  }
1039  else {
1040  tagSize = it->second;
1041  }
1042  std::vector<vpPoint> points3D(4);
1043  if (m_impl->getZAlignedWithCameraAxis()) {
1044  points3D[0] = vpPoint(-tagSize / 2, tagSize / 2, 0);
1045  points3D[1] = vpPoint(tagSize / 2, tagSize / 2, 0);
1046  points3D[2] = vpPoint(tagSize / 2, -tagSize / 2, 0);
1047  points3D[3] = vpPoint(-tagSize / 2, -tagSize / 2, 0);
1048  }
1049  else {
1050  points3D[0] = vpPoint(-tagSize / 2, -tagSize / 2, 0);
1051  points3D[1] = vpPoint(tagSize / 2, -tagSize / 2, 0);
1052  points3D[2] = vpPoint(tagSize / 2, tagSize / 2, 0);
1053  points3D[3] = vpPoint(-tagSize / 2, tagSize / 2, 0);
1054  }
1055  tagsPoints3D.push_back(points3D);
1056  }
1057 
1058  return tagsPoints3D;
1059 }
1060 
1066 std::vector<std::vector<vpImagePoint> > vpDetectorAprilTag::getTagsCorners() const { return m_polygon; }
1067 
1073 std::vector<int> vpDetectorAprilTag::getTagsId() const { return m_impl->getTagsId(); }
1074 
1076 {
1077  return m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1078 }
1079 
1081 {
1082  // back-up settings
1083  double decodeSharpening = 0.25;
1084  m_impl->getAprilTagDecodeSharpening(decodeSharpening);
1085  int nThreads = 1;
1086  m_impl->getNbThreads(nThreads);
1087  float quadDecimate = 1;
1088  m_impl->getQuadDecimate(quadDecimate);
1089  float quadSigma = 0;
1090  m_impl->getQuadSigma(quadSigma);
1091  bool refineEdges = true;
1092  m_impl->getRefineEdges(refineEdges);
1093  bool zAxis = m_impl->getZAlignedWithCameraAxis();
1094 
1095  delete m_impl;
1096  m_impl = new Impl(tagFamily, m_poseEstimationMethod);
1097  m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1098  m_impl->setNbThreads(nThreads);
1099  m_impl->setQuadDecimate(quadDecimate);
1100  m_impl->setQuadSigma(quadSigma);
1101  m_impl->setRefineEdges(refineEdges);
1102  m_impl->setZAlignedWithCameraAxis(zAxis);
1103 }
1104 
1111 {
1112  if (nThreads > 0) {
1113  m_impl->setNbThreads(nThreads);
1114  }
1115 }
1116 
1123 {
1124  m_poseEstimationMethod = poseEstimationMethod;
1125  m_impl->setPoseEstimationMethod(poseEstimationMethod);
1126 }
1127 
1140 void vpDetectorAprilTag::setAprilTagQuadDecimate(float quadDecimate) { m_impl->setQuadDecimate(quadDecimate); }
1141 
1154 void vpDetectorAprilTag::setAprilTagQuadSigma(float quadSigma) { m_impl->setQuadSigma(quadSigma); }
1155 
1156 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1161 {
1162  m_impl->setRefineDecode(refineDecode);
1163 }
1164 #endif
1165 
1180 void vpDetectorAprilTag::setAprilTagRefineEdges(bool refineEdges) { m_impl->setRefineEdges(refineEdges); }
1181 
1182 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1186 void vpDetectorAprilTag::setAprilTagRefinePose(bool refinePose) { m_impl->setRefinePose(refinePose); }
1187 #endif
1188 
1190 {
1191  using std::swap;
1192 
1193  swap(o1.m_impl, o2.m_impl);
1194 }
1195 
1201 void vpDetectorAprilTag::setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
1202 {
1203  m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
1204 }
1205 
1206 #elif !defined(VISP_BUILD_SHARED_LIBS)
1207 // Work around to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has
1208 // no symbols
1209 void dummy_vpDetectorAprilTag() { }
1210 #endif
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
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
unsigned int m_displayTagThickness
bool detect(const vpImage< unsigned char > &I) vp_override
vpAprilTagFamily m_tagFamily
void setAprilTagRefineEdges(bool refineEdges)
vpPoseEstimationMethod m_poseEstimationMethod
vp_deprecated void setAprilTagRefinePose(bool refinePose)
@ TAG_CIRCLE21h7
AprilTag Circle21h7 pattern.
@ TAG_25h7
DEPRECATED AND POOR DETECTION PERFORMANCE.
@ TAG_36ARTOOLKIT
DEPRECATED AND WILL NOT DETECT ARTOOLKIT TAGS.
@ TAG_25h9
AprilTag 25h9 pattern.
@ TAG_CUSTOM48h12
AprilTag Custom48h12 pattern.
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
@ TAG_STANDARD52h13
AprilTag Standard52h13 pattern.
@ TAG_16h5
AprilTag 16h5 pattern.
@ TAG_STANDARD41h12
AprilTag Standard41h12 pattern.
@ TAG_CIRCLE49h12
AprilTag Circle49h12 pattern.
void setAprilTagQuadSigma(float quadSigma)
void setAprilTagNbThreads(int nThreads)
vp_deprecated void setAprilTagRefineDecode(bool refineDecode)
vpDetectorAprilTag(const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
std::vector< int > getTagsId() const
bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=nullptr, double *projError=nullptr, double *projError2=nullptr)
virtual ~vpDetectorAprilTag() vp_override
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 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 ...
Definition: vpImagePoint.h:82
void set_uv(double u, double v)
Definition: vpImagePoint.h:353
unsigned int getWidth() const
Definition: vpImage.h:245
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:504
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:506
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
@ DEMENTHON
Definition: vpPose.h:84
@ VIRTUAL_VS
Definition: vpPose.h:93
@ LAGRANGE
Definition: vpPose.h:83
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:100
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:290
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=nullptr)
Definition: vpPose.cpp:340