Visual Servoing Platform  version 3.2.1 under development (2019-09-17) under development (2019-09-17)
vpDetectorAprilTag.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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 April Tag 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 <common/homography.h>
42 #include <tag16h5.h>
43 #include <tag25h7.h>
44 #include <tag25h9.h>
45 #include <tag36h10.h>
46 #include <tag36h11.h>
47 #include <tagCircle21h7.h>
48 #include <tagStandard41h12.h>
49 #include <apriltag_pose.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_cam(), m_poseEstimationMethod(method), m_tagFamily(tagFamily), m_tagSize(1.0), m_td(NULL),
69  m_tf(NULL), m_detections(NULL), 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, "Unknow 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()
137  {
138  if (m_td) {
139  apriltag_detector_destroy(m_td);
140  }
141 
142  if (m_tf) {
143  switch (m_tagFamily) {
144  case TAG_36h11:
145  tag36h11_destroy(m_tf);
146  break;
147 
148  case TAG_36h10:
149  tag36h10_destroy(m_tf);
150  break;
151 
152  case TAG_36ARTOOLKIT:
153  break;
154 
155  case TAG_25h9:
156  tag25h9_destroy(m_tf);
157  break;
158 
159  case TAG_25h7:
160  tag25h7_destroy(m_tf);
161  break;
162 
163  case TAG_16h5:
164  tag16h5_destroy(m_tf);
165  break;
166 
167  case TAG_CIRCLE21h7:
168  tagCircle21h7_destroy(m_tf);
169  break;
170 
171  case TAG_CIRCLE49h12:
172  #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
173  tagCustom48h12_destroy(m_tf);
174  #endif
175  break;
176 
177  case TAG_CUSTOM48h12:
178  #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
179  tagCustom48h12_destroy(m_tf);
180  #endif
181  break;
182 
183  case TAG_STANDARD52h13:
184  #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
185  tagStandard52h13_destroy(m_tf);
186  #endif
187  break;
188 
189  case TAG_STANDARD41h12:
190  #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
191  tagStandard41h12_destroy(m_tf);
192  #endif
193  break;
194 
195  default:
196  break;
197  }
198  }
199 
200  if (m_detections) {
201  apriltag_detections_destroy(m_detections);
202  m_detections = NULL;
203  }
204  }
205 
206  void convertHomogeneousMatrix(const apriltag_pose_t &pose, vpHomogeneousMatrix &cMo) {
207  for (unsigned int i = 0; i < 3; i++) {
208  for (unsigned int j = 0; j < 3; j++) {
209  cMo[i][j] = MATD_EL(pose.R, i, j);
210  }
211  cMo[i][3] = MATD_EL(pose.t, i, 0);
212  }
213  }
214 
215  bool detect(const vpImage<unsigned char> &I, std::vector<std::vector<vpImagePoint> > &polygons,
216  std::vector<std::string> &messages, const bool displayTag, const vpColor color,
217  const unsigned int thickness, std::vector<vpHomogeneousMatrix> *cMo_vec,
218  std::vector<vpHomogeneousMatrix> *cMo_vec2, std::vector<double> *projErrors,
219  std::vector<double> *projErrors2)
220  {
221  if (m_tagFamily == TAG_36ARTOOLKIT) {
222  //TAG_36ARTOOLKIT is not available anymore
223  std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
224  return false;
225  }
226 #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
228  std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." << std::endl;
229  return false;
230  }
231 #endif
232 
233  const bool computePose = (cMo_vec != NULL);
234 
235  image_u8_t im = {/*.width =*/(int32_t)I.getWidth(),
236  /*.height =*/(int32_t)I.getHeight(),
237  /*.stride =*/(int32_t)I.getWidth(),
238  /*.buf =*/I.bitmap};
239 
240  if (m_detections) {
241  apriltag_detections_destroy(m_detections);
242  m_detections = NULL;
243  }
244 
245  m_detections = apriltag_detector_detect(m_td, &im);
246  int nb_detections = zarray_size(m_detections);
247  bool detected = nb_detections > 0;
248 
249  polygons.resize((size_t)nb_detections);
250  messages.resize((size_t)nb_detections);
251 
252  for (int i = 0; i < zarray_size(m_detections); i++) {
253  apriltag_detection_t *det;
254  zarray_get(m_detections, i, &det);
255 
256  std::vector<vpImagePoint> polygon;
257  for (int j = 0; j < 4; j++) {
258  polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0]));
259  }
260  polygons[static_cast<size_t>(i)] = polygon;
261  std::stringstream ss;
262  ss << m_tagFamily << " id: " << det->id;
263  messages[static_cast<size_t>(i)] = ss.str();
264 
265  if (displayTag) {
266  vpColor Ox = (color == vpColor::none) ? vpColor::red : color;
267  vpColor Oy = (color == vpColor::none) ? vpColor::green : color;
268  vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color;
269  vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color;
270 
271  vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[1][1], (int)det->p[1][0],
272  Ox, thickness);
273  vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[3][1], (int)det->p[3][0],
274  Oy, thickness);
275  vpDisplay::displayLine(I, (int)det->p[1][1], (int)det->p[1][0], (int)det->p[2][1], (int)det->p[2][0],
276  Ox2, thickness);
277  vpDisplay::displayLine(I, (int)det->p[2][1], (int)det->p[2][0], (int)det->p[3][1], (int)det->p[3][0],
278  Oy2, thickness);
279  }
280 
281  if (computePose) {
282  vpHomogeneousMatrix cMo, cMo2;
283  double err1, err2;
284  if (getPose(static_cast<size_t>(i), m_tagSize, m_cam, cMo, cMo_vec2 ? &cMo2 : NULL,
285  projErrors ? &err1 : NULL, projErrors2 ? &err2 : NULL)) {
286  cMo_vec->push_back(cMo);
287  if (cMo_vec2) {
288  cMo_vec2->push_back(cMo2);
289  }
290  if (projErrors) {
291  projErrors->push_back(err1);
292  }
293  if (projErrors2) {
294  projErrors2->push_back(err2);
295  }
296  }
297  // else case should never happen
298  }
299  }
300 
301  return detected;
302  }
303 
304  bool getPose(size_t tagIndex, const double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2,
305  double *projErrors, double *projErrors2) {
306  if (m_detections == NULL) {
307  throw(vpException(vpException::fatalError, "Cannot get tag index=%d pose: detection empty", tagIndex));
308  }
309  if (m_tagFamily == TAG_36ARTOOLKIT) {
310  //TAG_36ARTOOLKIT is not available anymore
311  std::cerr << "TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
312  return false;
313  }
314 #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
316  std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." << std::endl;
317  return false;
318  }
319 #endif
320 
321  apriltag_detection_t *det;
322  zarray_get(m_detections, static_cast<int>(tagIndex), &det);
323 
324  int nb_detections = zarray_size(m_detections);
325  if (tagIndex >= (size_t)nb_detections) {
326  return false;
327  }
328 
329  //In AprilTag3, estimate_pose_for_tag_homography() and estimate_tag_pose() have been added.
330  //They use a tag frame aligned with the camera frame
331  //Before the release of AprilTag3, convention used was to define the z-axis of the tag going upward.
332  //To keep compatibility, we maintain the same convention than before and there is setZAlignedWithCameraAxis().
333  //Under the hood, we use aligned frames everywhere and transform the pose according to the option.
334 
335  vpHomogeneousMatrix cMo_homography_ortho_iter;
338  double fx = cam.get_px(), fy = cam.get_py();
339  double cx = cam.get_u0(), cy = cam.get_v0();
340 
341  apriltag_detection_info_t info;
342  info.det = det;
343  info.tagsize = tagSize;
344  info.fx = fx;
345  info.fy = fy;
346  info.cx = cx;
347  info.cy = cy;
348 
349  //projErrors and projErrors2 will be override later
350  getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
351  cMo_homography_ortho_iter = cMo;
352  }
353 
354  vpHomogeneousMatrix cMo_homography;
357  double fx = cam.get_px(), fy = cam.get_py();
358  double cx = cam.get_u0(), cy = cam.get_v0();
359 
360  apriltag_detection_info_t info;
361  info.det = det;
362  info.tagsize = tagSize;
363  info.fx = fx;
364  info.fy = fy;
365  info.cx = cx;
366  info.cy = cy;
367 
368  apriltag_pose_t pose;
369  estimate_pose_for_tag_homography(&info, &pose);
370  convertHomogeneousMatrix(pose, cMo);
371 
372  matd_destroy(pose.R);
373  matd_destroy(pose.t);
374 
375  cMo_homography = cMo;
376  }
377 
378  // Add marker object points
379  vpPose pose;
380  vpPoint pt;
381 
382  vpImagePoint imPt;
383  double x = 0.0, y = 0.0;
384  std::vector<vpPoint> pts(4);
385  pt.setWorldCoordinates(-tagSize / 2.0, tagSize / 2.0, 0.0);
386  imPt.set_uv(det->p[0][0], det->p[0][1]);
387  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
388  pt.set_x(x);
389  pt.set_y(y);
390  pts[0] = pt;
391 
392  pt.setWorldCoordinates(tagSize / 2.0, tagSize / 2.0, 0.0);
393  imPt.set_uv(det->p[1][0], det->p[1][1]);
394  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
395  pt.set_x(x);
396  pt.set_y(y);
397  pts[1] = pt;
398 
399  pt.setWorldCoordinates(tagSize / 2.0, -tagSize / 2.0, 0.0);
400  imPt.set_uv(det->p[2][0], det->p[2][1]);
401  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
402  pt.set_x(x);
403  pt.set_y(y);
404  pts[2] = pt;
405 
406  pt.setWorldCoordinates(-tagSize / 2.0, -tagSize / 2.0, 0.0);
407  imPt.set_uv(det->p[3][0], det->p[3][1]);
408  vpPixelMeterConversion::convertPoint(cam, imPt, x, y);
409  pt.set_x(x);
410  pt.set_y(y);
411  pts[3] = pt;
412 
413  pose.addPoints(pts);
414 
418  vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
419 
420  double residual_dementhon = std::numeric_limits<double>::max(),
421  residual_lagrange = std::numeric_limits<double>::max();
422  double residual_homography = pose.computeResidual(cMo_homography);
423  double residual_homography_ortho_iter = pose.computeResidual(cMo_homography_ortho_iter);
424 
425  if (pose.computePose(vpPose::DEMENTHON, cMo_dementhon)) {
426  residual_dementhon = pose.computeResidual(cMo_dementhon);
427  }
428 
429  if (pose.computePose(vpPose::LAGRANGE, cMo_lagrange)) {
430  residual_lagrange = pose.computeResidual(cMo_lagrange);
431  }
432 
433  std::vector<double> residuals;
434  residuals.push_back(residual_dementhon);
435  residuals.push_back(residual_lagrange);
436  residuals.push_back(residual_homography);
437  residuals.push_back(residual_homography_ortho_iter);
438  std::vector<vpHomogeneousMatrix> poses;
439  poses.push_back(cMo_dementhon);
440  poses.push_back(cMo_lagrange);
441  poses.push_back(cMo_homography);
442  poses.push_back(cMo_homography_ortho_iter);
443 
444  std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
445  cMo = *(poses.begin() + minIndex);
446  } else {
447  pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo);
448  }
449  }
450 
453  // Compute final pose using VVS
454  pose.computePose(vpPose::VIRTUAL_VS, cMo);
455  }
456 
457  //Only with HOMOGRAPHY_ORTHOGONAL_ITERATION we can directly get two solutions
459  if (cMo2) {
460  double scale = tagSize/2.0;
461  double data_p0[] = {-scale, scale, 0};
462  double data_p1[] = {scale, scale, 0};
463  double data_p2[] = {scale, -scale, 0};
464  double data_p3[] = {-scale, -scale, 0};
465  matd_t* p[4] = {matd_create_data(3, 1, data_p0),
466  matd_create_data(3, 1, data_p1),
467  matd_create_data(3, 1, data_p2),
468  matd_create_data(3, 1, data_p3)};
469  matd_t* v[4];
470  for (int i = 0; i < 4; i++) {
471  double data_v[] = {(det->p[i][0] - cam.get_u0())/cam.get_px(), (det->p[i][1] - cam.get_v0())/cam.get_py(), 1};
472  v[i] = matd_create_data(3, 1, data_v);
473  }
474 
475  apriltag_pose_t solution1, solution2;
476  const int nIters = 50;
477  solution1.R = matd_create_data(3, 3, cMo.getRotationMatrix().data);
478  solution1.t = matd_create_data(3, 1, cMo.getTranslationVector().data);
479 
480  double err2;
481  get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
482 
483  for (int i = 0; i < 4; i++) {
484  matd_destroy(p[i]);
485  matd_destroy(v[i]);
486  }
487 
488  if (solution2.R) {
489  convertHomogeneousMatrix(solution2, *cMo2);
490 
491  matd_destroy(solution2.R);
492  matd_destroy(solution2.t);
493  }
494 
495  matd_destroy(solution1.R);
496  matd_destroy(solution1.t);
497  }
498  }
499 
500  //Compute projection error with vpPose::computeResidual() for consistency
501  if (projErrors) {
502  *projErrors = pose.computeResidual(cMo);
503  }
504  if (projErrors2 && cMo2) {
505  *projErrors2 = pose.computeResidual(*cMo2);
506  }
507 
508  if (!m_zAlignedWithCameraFrame) {
510  // Apply a rotation of 180deg around x axis
511  oMo[0][0] = 1; oMo[0][1] = 0; oMo[0][2] = 0;
512  oMo[1][0] = 0; oMo[1][1] = -1; oMo[1][2] = 0;
513  oMo[2][0] = 0; oMo[2][1] = 0; oMo[2][2] = -1;
514  cMo = cMo*oMo;
515  if (cMo2) {
516  *cMo2 = *cMo2*oMo;
517  }
518  }
519 
520  return true;
521  }
522 
523  void getPoseWithOrthogonalMethod(apriltag_detection_info_t &info, vpHomogeneousMatrix &cMo1, vpHomogeneousMatrix *cMo2,
524  double *err1, double *err2) {
525  apriltag_pose_t pose1, pose2;
526  double err_1, err_2;
527  estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50);
528  if (err_1 <= err_2) {
529  convertHomogeneousMatrix(pose1, cMo1);
530  if (cMo2) {
531  if (pose2.R) {
532  convertHomogeneousMatrix(pose2, *cMo2);
533  } else {
534  *cMo2 = cMo1;
535  }
536  }
537  } else {
538  convertHomogeneousMatrix(pose2, cMo1);
539  if (cMo2) {
540  convertHomogeneousMatrix(pose1, *cMo2);
541  }
542  }
543 
544  matd_destroy(pose1.R);
545  matd_destroy(pose1.t);
546  if (pose2.R) {
547  matd_destroy(pose2.t);
548  }
549  matd_destroy(pose2.R);
550 
551  if (err1)
552  *err1 = err_1;
553  if (err2)
554  *err2 = err_2;
555  }
556 
557  void setCameraParameters(const vpCameraParameters &cam) { m_cam = cam; }
558 
559  void setAprilTagDecodeSharpening(const double decodeSharpening) {
560  if (m_td) {
561  m_td->decode_sharpening = decodeSharpening;
562  }
563  }
564 
565  void setNbThreads(const int nThreads) {
566  if (m_td) {
567  m_td->nthreads = nThreads;
568  }
569  }
570 
571  void setQuadDecimate(const float quadDecimate) {
572  if (m_td) {
573  m_td->quad_decimate = quadDecimate;
574  }
575  }
576 
577  void setQuadSigma(const float quadSigma) {
578  if (m_td) {
579  m_td->quad_sigma = quadSigma;
580  }
581  }
582 
583  void setRefineDecode(const bool) { }
584 
585  void setRefineEdges(const bool refineEdges) {
586  if (m_td) {
587  m_td->refine_edges = refineEdges ? 1 : 0;
588  }
589  }
590 
591  void setRefinePose(const bool) { }
592 
593  void setTagSize(const double tagSize) { m_tagSize = tagSize; }
594 
595  void setPoseEstimationMethod(const vpPoseEstimationMethod &method) { m_poseEstimationMethod = method; }
596 
597  void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame) { m_zAlignedWithCameraFrame = zAlignedWithCameraFrame; }
598 
599 protected:
600  vpCameraParameters m_cam;
601  std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
604  double m_tagSize;
605  apriltag_detector_t *m_td;
606  apriltag_family_t *m_tf;
607  zarray_t *m_detections;
608  bool m_zAlignedWithCameraFrame;
609 };
610 #endif // DOXYGEN_SHOULD_SKIP_THIS
611 
613  const vpPoseEstimationMethod &poseEstimationMethod)
615  m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily),
616  m_impl(new Impl(tagFamily, poseEstimationMethod))
617 {
618 }
619 
621 
630 {
631  m_message.clear();
632  m_polygon.clear();
633  m_nb_objects = 0;
634 
635  std::vector<vpHomogeneousMatrix> cMo_vec;
636  bool detected = m_impl->detect(I, m_polygon, m_message, m_displayTag,
638  NULL, NULL, NULL, NULL);
639  m_nb_objects = m_message.size();
640 
641  return detected;
642 }
643 
661 bool vpDetectorAprilTag::detect(const vpImage<unsigned char> &I, const double tagSize, const vpCameraParameters &cam,
662  std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
663  std::vector<double> *projErrors, std::vector<double> *projErrors2)
664 {
665  m_message.clear();
666  m_polygon.clear();
667  m_nb_objects = 0;
668 
669  m_impl->setTagSize(tagSize);
670  m_impl->setCameraParameters(cam);
671  bool detected = m_impl->detect(I, m_polygon, m_message, m_displayTag,
673  &cMo_vec, cMo_vec2, projErrors, projErrors2);
674  m_nb_objects = m_message.size();
675 
676  return detected;
677 }
678 
708 bool vpDetectorAprilTag::getPose(size_t tagIndex, const double tagSize, const vpCameraParameters &cam,
710  double *projError, double *projError2)
711 {
712  return (m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2));
713 }
714 
715 void vpDetectorAprilTag::setAprilTagDecodeSharpening(const double decodeSharpening)
716 {
717  return (m_impl->setAprilTagDecodeSharpening(decodeSharpening));
718 }
719 
726 {
727  if (nThreads > 0)
728  m_impl->setNbThreads(nThreads);
729 }
730 
737 {
738  m_poseEstimationMethod = poseEstimationMethod;
739  m_impl->setPoseEstimationMethod(poseEstimationMethod);
740 }
741 
754 void vpDetectorAprilTag::setAprilTagQuadDecimate(const float quadDecimate)
755 {
756  m_impl->setQuadDecimate(quadDecimate);
757 }
758 
771 void vpDetectorAprilTag::setAprilTagQuadSigma(const float quadSigma)
772 {
773  m_impl->setQuadSigma(quadSigma);
774 }
775 
776 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
777 
780 vp_deprecated void vpDetectorAprilTag::setAprilTagRefineDecode(const bool refineDecode) {
781  m_impl->setRefineDecode(refineDecode);
782 }
783 #endif
784 
799 void vpDetectorAprilTag::setAprilTagRefineEdges(const bool refineEdges)
800 {
801  m_impl->setRefineEdges(refineEdges);
802 }
803 
804 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
805 
808 vp_deprecated void vpDetectorAprilTag::setAprilTagRefinePose(const bool refinePose)
809 {
810  m_impl->setRefinePose(refinePose);
811 }
812 #endif
813 
819 void vpDetectorAprilTag::setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
820 {
821  m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
822 }
823 
824 #elif !defined(VISP_BUILD_SHARED_LIBS)
825 // Work arround to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has
826 // no symbols
827 void dummy_vpDetectorAprilTag() {}
828 #endif
void setAprilTagQuadDecimate(const float quadDecimate)
AprilTag 16h5 pattern.
void setAprilTagRefineEdges(const bool refineEdges)
AprilTag Standard52h13 pattern.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:374
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
DEPRECATED AND WILL NOT DETECT ARTOOLKIT TAGS.
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:141
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
AprilTag Circle21h7 pattern.
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:164
static const vpColor none
Definition: vpColor.h:192
void setAprilTagRefinePose(const bool refinePose)
void setAprilTagRefineDecode(const bool refineDecode)
error that can be emited by ViSP classes.
Definition: vpException.h:71
void set_x(const double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:472
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
static const vpColor green
Definition: vpColor.h:183
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
AprilTag Circle49h12 pattern.
size_t m_nb_objects
Number of detected objects.
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:78
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:474
vpTranslationVector getTranslationVector() const
vpPoseEstimationMethod m_poseEstimationMethod
void setWorldCoordinates(const double oX, const double oY, const double oZ)
Definition: vpPoint.cpp:113
AprilTag Standard41h12 pattern.
unsigned int getHeight() const
Definition: vpImage.h:186
std::vector< std::vector< vpImagePoint > > m_polygon
For each object, defines the polygon that contains the object.
vpDetectorAprilTag(const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS)
void set_uv(const double u, const double v)
Definition: vpImagePoint.h:248
void setAprilTagQuadSigma(const float quadSigma)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
AprilTag Custom48h12 pattern.
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:336
static const vpColor yellow
Definition: vpColor.h:188
bool getPose(size_t tagIndex, const double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=NULL, double *projError=NULL, double *projError2=NULL)
unsigned int m_displayTagThickness
unsigned int getWidth() const
Definition: vpImage.h:244
std::vector< std::string > m_message
Message attached to each object.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
vpAprilTagFamily m_tagFamily
vpRotationMatrix getRotationMatrix() const
void setAprilTagNbThreads(const int nThreads)
AprilTag 25h9 pattern.
void setAprilTagDecodeSharpening(const double decodeSharpening)
static const vpColor blue
Definition: vpColor.h:186
DEPRECATED AND POOR DETECTION PERFORMANCE.
bool detect(const vpImage< unsigned char > &I)