35#include <visp3/core/vpConfig.h>
37#ifdef VISP_HAVE_APRILTAG
41#include <common/homography.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>
58#include <visp3/core/vpDisplay.h>
59#include <visp3/core/vpPixelMeterConversion.h>
60#include <visp3/core/vpPoint.h>
61#include <visp3/vision/vpPose.h>
63#ifndef DOXYGEN_SHOULD_SKIP_THIS
64class vpDetectorAprilTag::Impl
69 m_td(NULL), m_tf(NULL), m_detections(NULL), m_zAlignedWithCameraFrame(false)
73 m_tf = tag36h11_create();
77 m_tf = tag36h10_create();
84 m_tf = tag25h9_create();
88 m_tf = tag25h7_create();
92 m_tf = tag16h5_create();
96 m_tf = tagCircle21h7_create();
100#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
101 m_tf = tagCircle49h12_create();
106#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
107 m_tf = tagCustom48h12_create();
112#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
113 m_tf = tagStandard52h13_create();
118#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
119 m_tf = tagStandard41h12_create();
128 m_td = apriltag_detector_create();
129 apriltag_detector_add_family(m_td, m_tf);
138 m_td(NULL), m_tf(NULL), m_detections(NULL), m_zAlignedWithCameraFrame(o.m_zAlignedWithCameraFrame)
142 m_tf = tag36h11_create();
146 m_tf = tag36h10_create();
153 m_tf = tag25h9_create();
157 m_tf = tag25h7_create();
161 m_tf = tag16h5_create();
165 m_tf = tagCircle21h7_create();
169#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
170 m_tf = tagCircle49h12_create();
175#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
176 m_tf = tagCustom48h12_create();
181#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
182 m_tf = tagStandard52h13_create();
187#if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
188 m_tf = tagStandard41h12_create();
197 m_td = apriltag_detector_create();
198 apriltag_detector_add_family(m_td, m_tf);
204 if (o.m_detections != NULL) {
205 m_detections = apriltag_detections_copy(o.m_detections);
212 apriltag_detector_destroy(m_td);
218 tag36h11_destroy(m_tf);
222 tag36h10_destroy(m_tf);
229 tag25h9_destroy(m_tf);
233 tag25h7_destroy(m_tf);
237 tag16h5_destroy(m_tf);
241 tagCircle21h7_destroy(m_tf);
245 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
246 tagCustom48h12_destroy(m_tf);
251 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
252 tagCustom48h12_destroy(m_tf);
257 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
258 tagStandard52h13_destroy(m_tf);
263 #if defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
264 tagStandard41h12_destroy(m_tf);
274 apriltag_detections_destroy(m_detections);
280 for (
unsigned int i = 0; i < 3; i++) {
281 for (
unsigned int j = 0; j < 3; j++) {
282 cMo[i][j] = MATD_EL(pose.R, i, j);
284 cMo[i][3] = MATD_EL(pose.t, i, 0);
289 std::vector<std::vector<vpImagePoint> > &polygons,
290 std::vector<std::string> &messages,
bool displayTag,
const vpColor color,
291 unsigned int thickness, std::vector<vpHomogeneousMatrix> *cMo_vec,
292 std::vector<vpHomogeneousMatrix> *cMo_vec2, std::vector<double> *projErrors,
293 std::vector<double> *projErrors2)
297 std::cerr <<
"TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
300#if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
302 std::cerr <<
"TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." << std::endl;
307 const bool computePose = (cMo_vec != NULL);
309 image_u8_t im = {(int32_t)I.
getWidth(),
315 apriltag_detections_destroy(m_detections);
319 m_detections = apriltag_detector_detect(m_td, &im);
320 int nb_detections = zarray_size(m_detections);
321 bool detected = nb_detections > 0;
323 polygons.resize(
static_cast<size_t>(nb_detections));
324 messages.resize(
static_cast<size_t>(nb_detections));
325 m_tagsId.resize(
static_cast<size_t>(nb_detections));
327 for (
int i = 0; i < zarray_size(m_detections); i++) {
328 apriltag_detection_t *det;
329 zarray_get(m_detections, i, &det);
331 std::vector<vpImagePoint> polygon;
332 for (
int j = 0; j < 4; j++) {
333 polygon.push_back(
vpImagePoint(det->p[j][1], det->p[j][0]));
335 polygons[
static_cast<size_t>(i)] = polygon;
336 std::stringstream ss;
338 messages[
static_cast<size_t>(i)] = ss.str();
339 m_tagsId[
static_cast<size_t>(i)] = det->id;
360 if (
getPose(
static_cast<size_t>(i), tagSize, cam, cMo, cMo_vec2 ? &cMo2 : NULL,
361 projErrors ? &err1 : NULL, projErrors2 ? &err2 : NULL)) {
362 cMo_vec->push_back(cMo);
364 cMo_vec2->push_back(cMo2);
367 projErrors->push_back(err1);
370 projErrors2->push_back(err2);
381 double *projErrors,
double *projErrors2) {
382 if (m_detections == NULL) {
387 std::cerr <<
"TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
390#if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
392 std::cerr <<
"TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." << std::endl;
397 apriltag_detection_t *det;
398 zarray_get(m_detections,
static_cast<int>(tagIndex), &det);
400 int nb_detections = zarray_size(m_detections);
401 if (tagIndex >= (
size_t)nb_detections) {
417 apriltag_detection_info_t info;
419 info.tagsize = tagSize;
426 getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
427 cMo_homography_ortho_iter = cMo;
436 apriltag_detection_info_t info;
438 info.tagsize = tagSize;
444 apriltag_pose_t pose;
445 estimate_pose_for_tag_homography(&info, &pose);
446 convertHomogeneousMatrix(pose, cMo);
448 matd_destroy(pose.R);
449 matd_destroy(pose.t);
451 cMo_homography = cMo;
459 double x = 0.0, y = 0.0;
460 std::vector<vpPoint> pts(4);
462 imPt.
set_uv(det->p[0][0], det->p[0][1]);
469 imPt.
set_uv(det->p[1][0], det->p[1][1]);
476 imPt.
set_uv(det->p[2][0], det->p[2][1]);
483 imPt.
set_uv(det->p[3][0], det->p[3][1]);
496 double residual_dementhon = std::numeric_limits<double>::max(),
497 residual_lagrange = std::numeric_limits<double>::max();
499 double residual_homography_ortho_iter = pose.
computeResidual(cMo_homography_ortho_iter);
509 std::vector<double> residuals;
510 residuals.push_back(residual_dementhon);
511 residuals.push_back(residual_lagrange);
512 residuals.push_back(residual_homography);
513 residuals.push_back(residual_homography_ortho_iter);
514 std::vector<vpHomogeneousMatrix> poses;
515 poses.push_back(cMo_dementhon);
516 poses.push_back(cMo_lagrange);
517 poses.push_back(cMo_homography);
518 poses.push_back(cMo_homography_ortho_iter);
520 std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
521 cMo = *(poses.begin() + minIndex);
536 double scale = tagSize/2.0;
537 double data_p0[] = {-scale, scale, 0};
538 double data_p1[] = {scale, scale, 0};
539 double data_p2[] = {scale, -scale, 0};
540 double data_p3[] = {-scale, -scale, 0};
541 matd_t* p[4] = {matd_create_data(3, 1, data_p0),
542 matd_create_data(3, 1, data_p1),
543 matd_create_data(3, 1, data_p2),
544 matd_create_data(3, 1, data_p3)};
546 for (
int i = 0; i < 4; i++) {
548 v[i] = matd_create_data(3, 1, data_v);
551 apriltag_pose_t solution1, solution2;
552 const int nIters = 50;
557 get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
559 for (
int i = 0; i < 4; i++) {
565 convertHomogeneousMatrix(solution2, *cMo2);
567 matd_destroy(solution2.R);
568 matd_destroy(solution2.t);
571 matd_destroy(solution1.R);
572 matd_destroy(solution1.t);
580 if (projErrors2 && cMo2) {
584 if (!m_zAlignedWithCameraFrame) {
587 oMo[0][0] = 1; oMo[0][1] = 0; oMo[0][2] = 0;
588 oMo[1][0] = 0; oMo[1][1] = -1; oMo[1][2] = 0;
589 oMo[2][0] = 0; oMo[2][1] = 0; oMo[2][2] = -1;
600 double *err1,
double *err2) {
601 apriltag_pose_t pose1, pose2;
603 estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50);
604 if (err_1 <= err_2) {
605 convertHomogeneousMatrix(pose1, cMo1);
608 convertHomogeneousMatrix(pose2, *cMo2);
614 convertHomogeneousMatrix(pose2, cMo1);
616 convertHomogeneousMatrix(pose1, *cMo2);
620 matd_destroy(pose1.R);
621 matd_destroy(pose1.t);
623 matd_destroy(pose2.t);
625 matd_destroy(pose2.R);
633 bool getZAlignedWithCameraAxis() {
return m_zAlignedWithCameraFrame; }
635 bool getAprilTagDecodeSharpening(
double &decodeSharpening)
const {
637 decodeSharpening = m_td->decode_sharpening;
643 bool getNbThreads(
int &nThreads)
const {
645 nThreads = m_td->nthreads;
651 bool getQuadDecimate(
float &quadDecimate)
const {
653 quadDecimate = m_td->quad_decimate;
659 bool getQuadSigma(
float &quadSigma)
const {
661 quadSigma = m_td->quad_sigma;
667 bool getRefineEdges(
bool &refineEdges)
const {
669 refineEdges = (m_td->refine_edges ? true :
false);
675 bool getZAlignedWithCameraAxis()
const {
676 return m_zAlignedWithCameraFrame;
679 std::vector<int>
getTagsId()
const {
return m_tagsId; }
683 m_td->decode_sharpening = decodeSharpening;
687 void setNbThreads(
int nThreads) {
689 m_td->nthreads = nThreads;
693 void setQuadDecimate(
float quadDecimate) {
695 m_td->quad_decimate = quadDecimate;
699 void setQuadSigma(
float quadSigma) {
701 m_td->quad_sigma = quadSigma;
705 void setRefineDecode(
bool) { }
707 void setRefineEdges(
bool refineEdges) {
709 m_td->refine_edges = refineEdges ? 1 : 0;
713 void setRefinePose(
bool) { }
720 std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
722 std::vector<int> m_tagsId;
724 apriltag_detector_t *m_td;
725 apriltag_family_t *m_tf;
726 zarray_t *m_detections;
727 bool m_zAlignedWithCameraFrame;
733 : m_displayTag(false), m_displayTagColor(
vpColor::none), m_displayTagThickness(2),
734 m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(),
735 m_impl(new Impl(tagFamily, poseEstimationMethod))
741 m_displayTag(false), m_displayTagColor(
vpColor::none), m_displayTagThickness(2),
742 m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(),
743 m_impl(new Impl(*o.m_impl))
768 std::vector<vpHomogeneousMatrix> cMo_vec;
769 const double tagSize = 1.0;
772 NULL, NULL, NULL, NULL);
796 std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
797 std::vector<double> *projErrors, std::vector<double> *projErrors2)
809 &cMo_vec, cMo_vec2, projErrors, projErrors2);
848 double *projError,
double *projError2)
850 return m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2);
871 const std::map<int, double>& tagsSize)
const
873 std::vector<std::vector<vpPoint> > tagsPoints3D;
875 double default_size = -1;
877 std::map<int, double>::const_iterator it = tagsSize.find(-1);
878 if (it != tagsSize.end()) {
879 default_size = it->second;
882 for (
size_t i = 0; i < tagsId.size(); i++) {
883 std::map<int, double>::const_iterator it = tagsSize.find(tagsId[i]);
884 double tagSize = default_size;
885 if (it == tagsSize.end()) {
886 if (default_size < 0) {
890 tagSize = it->second;
892 std::vector<vpPoint> points3D(4);
893 if (m_impl->getZAlignedWithCameraAxis()) {
894 points3D[0] =
vpPoint(-tagSize/2, tagSize/2, 0);
895 points3D[1] =
vpPoint( tagSize/2, tagSize/2, 0);
896 points3D[2] =
vpPoint( tagSize/2, -tagSize/2, 0);
897 points3D[3] =
vpPoint(-tagSize/2, -tagSize/2, 0);
899 points3D[0] =
vpPoint(-tagSize/2, -tagSize/2, 0);
900 points3D[1] =
vpPoint( tagSize/2, -tagSize/2, 0);
901 points3D[2] =
vpPoint( tagSize/2, tagSize/2, 0);
902 points3D[3] =
vpPoint(-tagSize/2, tagSize/2, 0);
904 tagsPoints3D.push_back(points3D);
927 return m_impl->getTagsId();
932 return m_impl->setAprilTagDecodeSharpening(decodeSharpening);
938 double decodeSharpening = 0.25;
939 m_impl->getAprilTagDecodeSharpening(decodeSharpening);
941 m_impl->getNbThreads(nThreads);
942 float quadDecimate = 1;
943 m_impl->getQuadDecimate(quadDecimate);
945 m_impl->getQuadSigma(quadSigma);
946 bool refineEdges =
true;
947 m_impl->getRefineEdges(refineEdges);
948 bool zAxis = m_impl->getZAlignedWithCameraAxis();
952 m_impl->setAprilTagDecodeSharpening(decodeSharpening);
953 m_impl->setNbThreads(nThreads);
954 m_impl->setQuadDecimate(quadDecimate);
955 m_impl->setQuadSigma(quadSigma);
956 m_impl->setRefineEdges(refineEdges);
957 m_impl->setZAlignedWithCameraAxis(zAxis);
968 m_impl->setNbThreads(nThreads);
980 m_impl->setPoseEstimationMethod(poseEstimationMethod);
997 m_impl->setQuadDecimate(quadDecimate);
1014 m_impl->setQuadSigma(quadSigma);
1017#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1022 m_impl->setRefineDecode(refineDecode);
1042 m_impl->setRefineEdges(refineEdges);
1045#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1051 m_impl->setRefinePose(refinePose);
1059 swap(o1.m_impl, o2.m_impl);
1069 m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
1072#elif !defined(VISP_BUILD_SHARED_LIBS)
1075void dummy_vpDetectorAprilTag() {}
Type * data
Address of the first element of the data array.
Generic class defining intrinsic camera parameters.
Class to define RGB colors available for display functionnalities.
static const vpColor none
static const vpColor blue
static const vpColor yellow
static const vpColor green
@ BEST_RESIDUAL_VIRTUAL_VS
@ HOMOGRAPHY_ORTHOGONAL_ITERATION
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
void setAprilTagQuadDecimate(float quadDecimate)
friend void swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2)
virtual ~vpDetectorAprilTag()
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)
vpAprilTagFamily m_tagFamily
void setAprilTagRefineEdges(bool refineEdges)
vpPoseEstimationMethod m_poseEstimationMethod
@ 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)
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 setAprilTagFamily(const vpAprilTagFamily &tagFamily)
void setAprilTagRefinePose(bool refinePose)
vpColor m_displayTagColor
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)
error that can be emited by ViSP classes.
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)
unsigned int getWidth() const
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
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 ...
void set_x(double x)
Set the point x coordinate in the image plane.
void setWorldCoordinates(double oX, double oY, double oZ)
void set_y(double y)
Set the point y coordinate in the image plane.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
void addPoints(const std::vector< vpPoint > &lP)
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)