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(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);
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 != NULL);
311 image_u8_t im = {(int32_t)I.
getWidth(),
317 apriltag_detections_destroy(m_detections);
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 : NULL, projErrors ? &err1 : NULL,
363 projErrors2 ? &err2 : NULL)) {
364 cMo_vec->push_back(cMo);
366 cMo_vec2->push_back(cMo2);
369 projErrors->push_back(err1);
372 projErrors2->push_back(err2);
385 if (m_detections == NULL) {
390 std::cerr <<
"TAG_36ARTOOLKIT detector is not available anymore." << std::endl;
393 #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY) 396 std::cerr <<
"TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." 402 apriltag_detection_t *det;
403 zarray_get(m_detections, static_cast<int>(tagIndex), &det);
405 int nb_detections = zarray_size(m_detections);
406 if (tagIndex >= (
size_t)nb_detections) {
422 apriltag_detection_info_t info;
424 info.tagsize = tagSize;
431 getPoseWithOrthogonalMethod(info, cMo, cMo2, projErrors, projErrors2);
432 cMo_homography_ortho_iter = cMo;
441 apriltag_detection_info_t info;
443 info.tagsize = tagSize;
449 apriltag_pose_t pose;
450 estimate_pose_for_tag_homography(&info, &pose);
451 convertHomogeneousMatrix(pose, cMo);
453 matd_destroy(pose.R);
454 matd_destroy(pose.t);
456 cMo_homography = cMo;
464 double x = 0.0, y = 0.0;
465 std::vector<vpPoint> pts(4);
467 imPt.
set_uv(det->p[0][0], det->p[0][1]);
474 imPt.
set_uv(det->p[1][0], det->p[1][1]);
481 imPt.
set_uv(det->p[2][0], det->p[2][1]);
488 imPt.
set_uv(det->p[3][0], det->p[3][1]);
501 double residual_dementhon = std::numeric_limits<double>::max(),
502 residual_lagrange = std::numeric_limits<double>::max();
504 double residual_homography_ortho_iter = pose.
computeResidual(cMo_homography_ortho_iter);
514 std::vector<double> residuals;
515 residuals.push_back(residual_dementhon);
516 residuals.push_back(residual_lagrange);
517 residuals.push_back(residual_homography);
518 residuals.push_back(residual_homography_ortho_iter);
519 std::vector<vpHomogeneousMatrix> poses;
520 poses.push_back(cMo_dementhon);
521 poses.push_back(cMo_lagrange);
522 poses.push_back(cMo_homography);
523 poses.push_back(cMo_homography_ortho_iter);
525 std::ptrdiff_t minIndex = std::min_element(residuals.begin(), residuals.end()) - residuals.begin();
526 cMo = *(poses.begin() + minIndex);
540 double scale = tagSize / 2.0;
541 double data_p0[] = {-scale, scale, 0};
542 double data_p1[] = {scale, scale, 0};
543 double data_p2[] = {scale, -scale, 0};
544 double data_p3[] = {-scale, -scale, 0};
545 matd_t *p[4] = {matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1),
546 matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3)};
548 for (
int i = 0; i < 4; i++) {
551 v[i] = matd_create_data(3, 1, data_v);
554 apriltag_pose_t solution1, solution2;
555 const int nIters = 50;
560 get_second_solution(v, p, &solution1, &solution2, nIters, &err2);
562 for (
int i = 0; i < 4; i++) {
568 convertHomogeneousMatrix(solution2, *cMo2);
570 matd_destroy(solution2.R);
571 matd_destroy(solution2.t);
574 matd_destroy(solution1.R);
575 matd_destroy(solution1.t);
583 if (projErrors2 && cMo2) {
587 if (!m_zAlignedWithCameraFrame) {
608 void getPoseWithOrthogonalMethod(apriltag_detection_info_t &info,
vpHomogeneousMatrix &cMo1,
611 apriltag_pose_t pose1, pose2;
613 estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50);
614 if (err_1 <= err_2) {
615 convertHomogeneousMatrix(pose1, cMo1);
618 convertHomogeneousMatrix(pose2, *cMo2);
624 convertHomogeneousMatrix(pose2, cMo1);
626 convertHomogeneousMatrix(pose1, *cMo2);
630 matd_destroy(pose1.R);
631 matd_destroy(pose1.t);
633 matd_destroy(pose2.t);
635 matd_destroy(pose2.R);
643 bool getZAlignedWithCameraAxis() {
return m_zAlignedWithCameraFrame; }
645 bool getAprilTagDecodeSharpening(
double &decodeSharpening)
const 648 decodeSharpening = m_td->decode_sharpening;
654 bool getNbThreads(
int &nThreads)
const 657 nThreads = m_td->nthreads;
663 bool getQuadDecimate(
float &quadDecimate)
const 666 quadDecimate = m_td->quad_decimate;
672 bool getQuadSigma(
float &quadSigma)
const 675 quadSigma = m_td->quad_sigma;
681 bool getRefineEdges(
bool &refineEdges)
const 684 refineEdges = (m_td->refine_edges ? true :
false);
690 bool getZAlignedWithCameraAxis()
const {
return m_zAlignedWithCameraFrame; }
692 std::vector<int>
getTagsId()
const {
return m_tagsId; }
697 m_td->decode_sharpening = decodeSharpening;
701 void setNbThreads(
int nThreads)
704 m_td->nthreads = nThreads;
708 void setQuadDecimate(
float quadDecimate)
711 m_td->quad_decimate = quadDecimate;
715 void setQuadSigma(
float quadSigma)
718 m_td->quad_sigma = quadSigma;
722 void setRefineDecode(
bool) {}
724 void setRefineEdges(
bool refineEdges)
727 m_td->refine_edges = refineEdges ? 1 : 0;
731 void setRefinePose(
bool) {}
738 std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
740 std::vector<int> m_tagsId;
742 apriltag_detector_t *m_td;
743 apriltag_family_t *m_tf;
744 zarray_t *m_detections;
745 bool m_zAlignedWithCameraFrame;
747 #endif // DOXYGEN_SHOULD_SKIP_THIS 753 m_impl(new Impl(tagFamily, poseEstimationMethod))
760 m_impl(new Impl(*o.m_impl))
785 std::vector<vpHomogeneousMatrix> cMo_vec;
786 const double tagSize = 1.0;
812 std::vector<vpHomogeneousMatrix> &cMo_vec, std::vector<vpHomogeneousMatrix> *cMo_vec2,
813 std::vector<double> *projErrors, std::vector<double> *projErrors2)
865 return m_impl->getPose(tagIndex, tagSize, cam, cMo, cMo2, projError, projError2);
886 const std::map<int, double> &tagsSize)
const 888 std::vector<std::vector<vpPoint> > tagsPoints3D;
890 double default_size = -1;
892 std::map<int, double>::const_iterator it = tagsSize.find(-1);
893 if (it != tagsSize.end()) {
894 default_size = it->second;
897 for (
size_t i = 0; i < tagsId.size(); i++) {
898 std::map<int, double>::const_iterator it = tagsSize.find(tagsId[i]);
899 double tagSize = default_size;
900 if (it == tagsSize.end()) {
901 if (default_size < 0) {
903 "Tag with id %d has no 3D size or there is no default 3D size defined", tagsId[i]));
906 tagSize = it->second;
908 std::vector<vpPoint> points3D(4);
909 if (m_impl->getZAlignedWithCameraAxis()) {
910 points3D[0] =
vpPoint(-tagSize / 2, tagSize / 2, 0);
911 points3D[1] =
vpPoint(tagSize / 2, tagSize / 2, 0);
912 points3D[2] =
vpPoint(tagSize / 2, -tagSize / 2, 0);
913 points3D[3] =
vpPoint(-tagSize / 2, -tagSize / 2, 0);
915 points3D[0] =
vpPoint(-tagSize / 2, -tagSize / 2, 0);
916 points3D[1] =
vpPoint(tagSize / 2, -tagSize / 2, 0);
917 points3D[2] =
vpPoint(tagSize / 2, tagSize / 2, 0);
918 points3D[3] =
vpPoint(-tagSize / 2, tagSize / 2, 0);
920 tagsPoints3D.push_back(points3D);
942 return m_impl->setAprilTagDecodeSharpening(decodeSharpening);
948 double decodeSharpening = 0.25;
949 m_impl->getAprilTagDecodeSharpening(decodeSharpening);
951 m_impl->getNbThreads(nThreads);
952 float quadDecimate = 1;
953 m_impl->getQuadDecimate(quadDecimate);
955 m_impl->getQuadSigma(quadSigma);
956 bool refineEdges =
true;
957 m_impl->getRefineEdges(refineEdges);
958 bool zAxis = m_impl->getZAlignedWithCameraAxis();
962 m_impl->setAprilTagDecodeSharpening(decodeSharpening);
963 m_impl->setNbThreads(nThreads);
964 m_impl->setQuadDecimate(quadDecimate);
965 m_impl->setQuadSigma(quadSigma);
966 m_impl->setRefineEdges(refineEdges);
967 m_impl->setZAlignedWithCameraAxis(zAxis);
978 m_impl->setNbThreads(nThreads);
990 m_impl->setPoseEstimationMethod(poseEstimationMethod);
1021 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 1027 m_impl->setRefineDecode(refineDecode);
1047 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 1058 swap(o1.m_impl, o2.m_impl);
1068 m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame);
1071 #elif !defined(VISP_BUILD_SHARED_LIBS) 1074 void dummy_vpDetectorAprilTag() {}
AprilTag Standard52h13 pattern.
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
void setWorldCoordinates(double oX, double oY, double oZ)
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
void setAprilTagQuadSigma(float quadSigma)
void set_uv(double u, double v)
Class to define RGB colors available for display functionnalities.
AprilTag Circle21h7 pattern.
void addPoints(const std::vector< vpPoint > &lP)
static const vpColor none
error that can be emited by ViSP classes.
void setAprilTagDecodeSharpening(double decodeSharpening)
std::vector< std::vector< vpPoint > > getTagsPoints3D(const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const
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.
bool getPose(size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=NULL, double *projError=NULL, double *projError2=NULL)
friend void swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2)
static const vpColor green
vpColor m_displayTagColor
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.
AprilTag Circle49h12 pattern.
void set_y(double y)
Set the point y coordinate in the image plane.
void setAprilTagQuadDecimate(float quadDecimate)
void setAprilTagFamily(const vpAprilTagFamily &tagFamily)
size_t m_nb_objects
Number of detected objects.
void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame)
void setAprilTagRefineDecode(bool refineDecode)
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Generic class defining intrinsic camera parameters.
void setAprilTagNbThreads(int nThreads)
std::vector< std::vector< vpImagePoint > > getTagsCorners() const
std::vector< int > getTagsId() const
vpTranslationVector getTranslationVector() const
vpPoseEstimationMethod m_poseEstimationMethod
AprilTag Standard41h12 pattern.
unsigned int getHeight() const
vpDetectorAprilTag & operator=(vpDetectorAprilTag o)
void setAprilTagRefineEdges(bool refineEdges)
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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
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...
static const vpColor yellow
unsigned int m_displayTagThickness
unsigned int getWidth() const
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
void setAprilTagRefinePose(bool refinePose)
virtual ~vpDetectorAprilTag()
vpRotationMatrix getRotationMatrix() const
static const vpColor blue
DEPRECATED AND POOR DETECTION PERFORMANCE.
bool detect(const vpImage< unsigned char > &I)