35 #include <visp3/core/vpConfig.h>
37 #ifdef VISP_HAVE_APRILTAG
41 #include <apriltag_pose.h>
42 #include <common/homography.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>
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
64 class vpDetectorAprilTag::Impl
69 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_tf(nullptr), m_detections(nullptr), 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 !=
nullptr) {
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);
275 m_detections =
nullptr;
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);
285 cMo[i][3] = MATD_EL(pose.t, i, 0);
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)
297 std::cerr <<
"TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
300 #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
303 std::cerr <<
"TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
309 const bool computePose = (cMo_vec !=
nullptr);
311 image_u8_t im = {(int32_t)I.
getWidth(),
317 apriltag_detections_destroy(m_detections);
318 m_detections =
nullptr;
321 m_detections = apriltag_detector_detect(m_td, &im);
322 int nb_detections = zarray_size(m_detections);
323 bool detected = nb_detections > 0;
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));
329 for (
int i = 0; i < zarray_size(m_detections); i++) {
330 apriltag_detection_t *det;
331 zarray_get(m_detections, i, &det);
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]));
337 polygons[
static_cast<size_t>(i)] = polygon;
338 std::stringstream ss;
340 messages[
static_cast<size_t>(i)] = ss.str();
341 m_tagsId[
static_cast<size_t>(i)] = det->id;
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,
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,
362 if (
getPose(
static_cast<size_t>(i), tagSize, cam, cMo, cMo_vec2 ? &cMo2 :
nullptr, projErrors ? &err1 :
nullptr,
363 projErrors2 ? &err2 :
nullptr)) {
364 cMo_vec->push_back(cMo);
366 cMo_vec2->push_back(cMo2);
369 projErrors->push_back(err1);
372 projErrors2->push_back(err2);
385 for (
size_t i = 0; i < cMo_vec.size(); i++) {
394 for (
size_t i = 0; i < cMo_vec.size(); i++) {
401 const vpColor &color,
unsigned int thickness)
const
403 for (
size_t i = 0; i < tagsCorners.size(); i++) {
409 const std::vector<vpImagePoint> &corners = tagsCorners[i];
410 assert(corners.size() == 4);
412 vpDisplay::displayLine(I, (
int)corners[0].get_i(), (
int)corners[0].get_j(), (
int)corners[1].get_i(), (
int)corners[1].get_j(),
414 vpDisplay::displayLine(I, (
int)corners[0].get_i(), (
int)corners[0].get_j(), (
int)corners[3].get_i(), (
int)corners[3].get_j(),
416 vpDisplay::displayLine(I, (
int)corners[1].get_i(), (
int)corners[1].get_j(), (
int)corners[2].get_i(), (
int)corners[2].get_j(),
418 vpDisplay::displayLine(I, (
int)corners[2].get_i(), (
int)corners[2].get_j(), (
int)corners[3].get_i(), (
int)corners[3].get_j(),
424 const vpColor &color,
unsigned int thickness)
const
426 for (
size_t i = 0; i < tagsCorners.size(); i++) {
432 const std::vector<vpImagePoint> &corners = tagsCorners[i];
433 assert(corners.size() == 4);
435 vpDisplay::displayLine(I, (
int)corners[0].get_i(), (
int)corners[0].get_j(), (
int)corners[1].get_i(), (
int)corners[1].get_j(),
437 vpDisplay::displayLine(I, (
int)corners[0].get_i(), (
int)corners[0].get_j(), (
int)corners[3].get_i(), (
int)corners[3].get_j(),
439 vpDisplay::displayLine(I, (
int)corners[1].get_i(), (
int)corners[1].get_j(), (
int)corners[2].get_i(), (
int)corners[2].get_j(),
441 vpDisplay::displayLine(I, (
int)corners[2].get_i(), (
int)corners[2].get_j(), (
int)corners[3].get_i(), (
int)corners[3].get_j(),
449 if (m_detections ==
nullptr) {
454 std::cerr <<
"TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
457 #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY)
460 std::cerr <<
"TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled."
466 apriltag_detection_t *det;
467 zarray_get(m_detections,
static_cast<int>(tagIndex), &det);
469 int nb_detections = zarray_size(m_detections);
470 if (tagIndex >= (
size_t)nb_detections) {
486 apriltag_detection_info_t info;
488 info.tagsize = tagSize;
495 getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
496 cMo_homography_ortho_iter = cMo;
505 apriltag_detection_info_t info;
507 info.tagsize = tagSize;
513 apriltag_pose_t pose;
514 estimate_pose_for_tag_homography(&info, &pose);
515 convertHomogeneousMatrix(pose, cMo);
517 matd_destroy(pose.R);
518 matd_destroy(pose.t);
520 cMo_homography = cMo;
528 double x = 0.0, y = 0.0;
529 std::vector<vpPoint> pts(4);
531 imPt.
set_uv(det->p[0][0], det->p[0][1]);
538 imPt.
set_uv(det->p[1][0], det->p[1][1]);
545 imPt.
set_uv(det->p[2][0], det->p[2][1]);
552 imPt.
set_uv(det->p[3][0], det->p[3][1]);
565 double residual_dementhon = std::numeric_limits<double>::max(),
566 residual_lagrange = std::numeric_limits<double>::max();
568 double residual_homography_ortho_iter = pose.
computeResidual(cMo_homography_ortho_iter);
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);
589 std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
590 cMo = *(poses.begin() + minIndex);
605 double scale = tagSize / 2.0;
606 double data_p0[] = { -scale, scale, 0 };
607 double data_p1[] = { scale, scale, 0 };
608 double data_p2[] = { scale, -scale, 0 };
609 double data_p3[] = { -scale, -scale, 0 };
610 matd_t *p[4] = { matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1),
611 matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3) };
613 for (
int i = 0; i < 4; i++) {
616 v[i] = matd_create_data(3, 1, data_v);
619 apriltag_pose_t solution1, solution2;
620 const int nIters = 50;
625 get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
627 for (
int i = 0; i < 4; i++) {
633 convertHomogeneousMatrix(solution2, *cMo2);
635 matd_destroy(solution2.R);
636 matd_destroy(solution2.t);
639 matd_destroy(solution1.R);
640 matd_destroy(solution1.t);
648 if (projErrors2 && cMo2) {
652 if (!m_zAlignedWithCameraFrame) {
673 void getPoseWithOrthogonalMethod(apriltag_detection_info_t &info,
vpHomogeneousMatrix &cMo1,
676 apriltag_pose_t pose1, pose2;
678 estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50);
679 if (err_1 <= err_2) {
680 convertHomogeneousMatrix(pose1, cMo1);
683 convertHomogeneousMatrix(pose2, *cMo2);
691 convertHomogeneousMatrix(pose2, cMo1);
693 convertHomogeneousMatrix(pose1, *cMo2);
697 matd_destroy(pose1.R);
698 matd_destroy(pose1.t);
700 matd_destroy(pose2.t);
702 matd_destroy(pose2.R);
710 bool getZAlignedWithCameraAxis() {
return m_zAlignedWithCameraFrame; }
712 bool getAprilTagDecodeSharpening(
double &decodeSharpening)
const
715 decodeSharpening = m_td->decode_sharpening;
721 bool getNbThreads(
int &nThreads)
const
724 nThreads = m_td->nthreads;
730 bool getQuadDecimate(
float &quadDecimate)
const
733 quadDecimate = m_td->quad_decimate;
739 bool getQuadSigma(
float &quadSigma)
const
742 quadSigma = m_td->quad_sigma;
748 bool getRefineEdges(
bool &refineEdges)
const
751 refineEdges = (m_td->refine_edges ? true :
false);
757 bool getZAlignedWithCameraAxis()
const {
return m_zAlignedWithCameraFrame; }
759 std::vector<int>
getTagsId()
const {
return m_tagsId; }
764 m_td->decode_sharpening = decodeSharpening;
768 void setNbThreads(
int nThreads)
771 m_td->nthreads = nThreads;
775 void setQuadDecimate(
float quadDecimate)
778 m_td->quad_decimate = quadDecimate;
782 void setQuadSigma(
float quadSigma)
785 m_td->quad_sigma = quadSigma;
789 void setRefineDecode(
bool) { }
791 void setRefineEdges(
bool refineEdges)
794 m_td->refine_edges = refineEdges ? 1 : 0;
798 void setRefinePose(
bool) { }
805 std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
807 std::vector<int> m_tagsId;
809 apriltag_detector_t *m_td;
810 apriltag_family_t *m_tf;
811 zarray_t *m_detections;
812 bool m_zAlignedWithCameraFrame;
818 : m_displayTag(false), m_displayTagColor(
vpColor::none), m_displayTagThickness(2),
819 m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(),
820 m_impl(new Impl(tagFamily, poseEstimationMethod))
824 :
vpDetectorBase(o), m_displayTag(false), m_displayTagColor(
vpColor::none), m_displayTagThickness(2),
825 m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(),
826 m_impl(new Impl(*o.m_impl))
850 std::vector<vpHomogeneousMatrix> cMo_vec;
851 const double tagSize = 1.0;
877 std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
878 std::vector<double> *projErrors, std::vector<double> *projErrors2)
908 m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
924 m_impl->displayFrames(I, cMo_vec, cam, size, color, thickness);
936 const vpColor &color,
unsigned int thickness)
const
938 m_impl->displayTags(I, tagsCorners, color, thickness);
950 const vpColor &color,
unsigned int thickness)
const
952 m_impl->displayTags(I, tagsCorners, color, thickness);
990 return m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2);
1011 const std::map<int, double> &tagsSize)
const
1013 std::vector<std::vector<vpPoint> > tagsPoints3D;
1015 double default_size = -1;
1017 std::map<int, double>::const_iterator it = tagsSize.find(-1);
1018 if (it != tagsSize.end()) {
1019 default_size = it->second;
1022 for (
size_t i = 0; i < tagsId.size(); i++) {
1023 std::map<int, double>::const_iterator it = tagsSize.find(tagsId[i]);
1024 double tagSize = default_size;
1025 if (it == tagsSize.end()) {
1026 if (default_size < 0) {
1028 "Tag with id %d has no 3D size or there is no default 3D size defined", tagsId[i]));
1032 tagSize = it->second;
1034 std::vector<vpPoint> points3D(4);
1035 if (m_impl->getZAlignedWithCameraAxis()) {
1036 points3D[0] =
vpPoint(-tagSize / 2, tagSize / 2, 0);
1037 points3D[1] =
vpPoint(tagSize / 2, tagSize / 2, 0);
1038 points3D[2] =
vpPoint(tagSize / 2, -tagSize / 2, 0);
1039 points3D[3] =
vpPoint(-tagSize / 2, -tagSize / 2, 0);
1042 points3D[0] =
vpPoint(-tagSize / 2, -tagSize / 2, 0);
1043 points3D[1] =
vpPoint(tagSize / 2, -tagSize / 2, 0);
1044 points3D[2] =
vpPoint(tagSize / 2, tagSize / 2, 0);
1045 points3D[3] =
vpPoint(-tagSize / 2, tagSize / 2, 0);
1047 tagsPoints3D.push_back(points3D);
1050 return tagsPoints3D;
1069 return m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1075 double decodeSharpening = 0.25;
1076 m_impl->getAprilTagDecodeSharpening(decodeSharpening);
1078 m_impl->getNbThreads(nThreads);
1079 float quadDecimate = 1;
1080 m_impl->getQuadDecimate(quadDecimate);
1081 float quadSigma = 0;
1082 m_impl->getQuadSigma(quadSigma);
1083 bool refineEdges =
true;
1084 m_impl->getRefineEdges(refineEdges);
1085 bool zAxis = m_impl->getZAlignedWithCameraAxis();
1089 m_impl->setAprilTagDecodeSharpening(decodeSharpening);
1090 m_impl->setNbThreads(nThreads);
1091 m_impl->setQuadDecimate(quadDecimate);
1092 m_impl->setQuadSigma(quadSigma);
1093 m_impl->setRefineEdges(refineEdges);
1094 m_impl->setZAlignedWithCameraAxis(zAxis);
1105 m_impl->setNbThreads(nThreads);
1117 m_impl->setPoseEstimationMethod(poseEstimationMethod);
1148 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1154 m_impl->setRefineDecode(refineDecode);
1174 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1185 swap(o1.m_impl, o2.m_impl);
1195 m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
1198 #elif !defined(VISP_BUILD_SHARED_LIBS)
1201 void 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 functionalities.
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)
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
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() 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)
vpColor m_displayTagColor
bool detect(const vpImage< unsigned char > &I) override
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.
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 &)=nullptr)