![]() |
Visual Servoing Platform
version 3.3.0
|
#include <vpDetectorAprilTag.h>
Public Member Functions | |
vpDetectorAprilTag (const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS) | |
vpDetectorAprilTag (const vpDetectorAprilTag &o) | |
vpDetectorAprilTag & | operator= (vpDetectorAprilTag o) |
virtual | ~vpDetectorAprilTag () |
bool | detect (const vpImage< unsigned char > &I) |
bool | detect (const vpImage< unsigned char > &I, double tagSize, const vpCameraParameters &cam, std::vector< vpHomogeneousMatrix > &cMo_vec, std::vector< vpHomogeneousMatrix > *cMo_vec2=NULL, std::vector< double > *projErrors=NULL, std::vector< double > *projErrors2=NULL) |
bool | getPose (size_t tagIndex, double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, vpHomogeneousMatrix *cMo2=NULL, double *projError=NULL, double *projError2=NULL) |
vpPoseEstimationMethod | getPoseEstimationMethod () const |
std::vector< std::vector< vpImagePoint > > | getTagsCorners () const |
std::vector< int > | getTagsId () const |
std::vector< std::vector< vpPoint > > | getTagsPoints3D (const std::vector< int > &tagsId, const std::map< int, double > &tagsSize) const |
void | setAprilTagDecodeSharpening (double decodeSharpening) |
void | setAprilTagFamily (const vpAprilTagFamily &tagFamily) |
void | setAprilTagNbThreads (int nThreads) |
void | setAprilTagPoseEstimationMethod (const vpPoseEstimationMethod &poseEstimationMethod) |
void | setAprilTagQuadDecimate (float quadDecimate) |
void | setAprilTagQuadSigma (float quadSigma) |
void | setAprilTagRefineDecode (bool refineDecode) |
void | setAprilTagRefineEdges (bool refineEdges) |
void | setAprilTagRefinePose (bool refinePose) |
void | setDisplayTag (bool display, const vpColor &color=vpColor::none, unsigned int thickness=2) |
void | setZAlignedWithCameraAxis (bool zAlignedWithCameraFrame) |
Inherited functionalities from vpDetectorBase | |
vpRect | getBBox (size_t i) const |
vpImagePoint | getCog (size_t i) const |
std::vector< std::string > & | getMessage () |
std::string & | getMessage (size_t i) |
size_t | getNbObjects () const |
std::vector< std::vector< vpImagePoint > > & | getPolygon () |
std::vector< vpImagePoint > & | getPolygon (size_t i) |
void | setTimeout (unsigned long timeout_ms) |
Protected Attributes | |
bool | m_displayTag |
vpColor | m_displayTagColor |
unsigned int | m_displayTagThickness |
vpPoseEstimationMethod | m_poseEstimationMethod |
vpAprilTagFamily | m_tagFamily |
std::vector< std::vector< vpImagePoint > > | m_polygon |
std::vector< std::string > | m_message |
size_t | m_nb_objects |
unsigned long | m_timeout_ms |
Friends | |
void | swap (vpDetectorAprilTag &o1, vpDetectorAprilTag &o2) |
Base class for AprilTag detector. This class is a wrapper over AprilTag. There is no need to download and install AprilTag from source code or existing pre-built packages since the source code is embedded in ViSP. Reference papers are AprilTag: A robust and flexible visual fiducial system ([olson2011tags]), AprilTag 2: Efficient and robust fiducial detection ([wang2016iros]) and Flexible Layouts for Fiducial Tags (Under Review) ([krogius2019iros]).
The detect() function allows to detect multiple tags in an image. Once detected, for each tag it is possible to retrieve the location of the corners using getPolygon(), the encoded message using getMessage(), the bounding box using getBBox() and the center of gravity using getCog().
If camera parameters and the size of the tag are provided, you can also estimate the 3D pose of the tag in terms of position and orientation wrt the camera considering 2 cases:
The following sample code shows how to use this class to detect the location of 36h11 AprilTag patterns in an image.
The previous example may produce results like:
This other example shows how to estimate the 3D pose of 36h11 AprilTag patterns considering that all the tags have the same size (in our example 0.053 m).
The previous example may produce results like:
In this other example we estimate the 3D pose of 36h11 AprilTag patterns considering that tag 36h11 with id 0 (in that case the tag message is "36h11 id: 0") has a size of 0.040 m, while all the others have a size of 0.053m.
With respect to the previous example, this example may now produce a different pose for tag with id 0:
Other examples are also provided in tutorial-apriltag-detector.cpp and tutorial-apriltag-detector-live.cpp
Definition at line 215 of file vpDetectorAprilTag.h.
Definition at line 218 of file vpDetectorAprilTag.h.
Definition at line 232 of file vpDetectorAprilTag.h.
vpDetectorAprilTag::vpDetectorAprilTag | ( | const vpAprilTagFamily & | tagFamily = TAG_36h11 , |
const vpPoseEstimationMethod & | poseEstimationMethod = HOMOGRAPHY_VIRTUAL_VS |
||
) |
Definition at line 730 of file vpDetectorAprilTag.cpp.
vpDetectorAprilTag::vpDetectorAprilTag | ( | const vpDetectorAprilTag & | o | ) |
Definition at line 738 of file vpDetectorAprilTag.cpp.
|
virtual |
Definition at line 752 of file vpDetectorAprilTag.cpp.
|
virtual |
Detect AprilTag tags in the image. Return true if at least one tag is detected, false otherwise.
I | : Input image. |
Implements vpDetectorBase.
Definition at line 761 of file vpDetectorAprilTag.cpp.
Referenced by VispDetector::detectAprilTag:px:py:.
bool vpDetectorAprilTag::detect | ( | const vpImage< unsigned char > & | I, |
double | tagSize, | ||
const vpCameraParameters & | cam, | ||
std::vector< vpHomogeneousMatrix > & | cMo_vec, | ||
std::vector< vpHomogeneousMatrix > * | cMo_vec2 = NULL , |
||
std::vector< double > * | projErrors = NULL , |
||
std::vector< double > * | projErrors2 = NULL |
||
) |
Detect AprilTag tags in the image and compute the corresponding tag poses considering that all the tags have the same size.
If tags with different sizes have to be considered, you may use getPose().
[in] | I | : Input image. |
[in] | tagSize | : Tag size in meter corresponding to the external width of the pattern. |
[in] | cam | : Camera intrinsic parameters. |
[out] | cMo_vec | : List of tag poses. |
[out] | cMo_vec2 | : Optional second list of tag poses, since there are 2 solutions for planar pose estimation. |
[out] | projErrors | : Optional (sum of squared) projection errors in the normalized camera frame. |
[out] | projErrors2 | : Optional (sum of squared) projection errors for the 2nd solution in the normalized camera frame. |
Definition at line 794 of file vpDetectorAprilTag.cpp.
|
inherited |
Return the bounding box of the ith object.
Definition at line 91 of file vpDetectorBase.cpp.
|
inherited |
Return the center of gravity location of the ith object.
Definition at line 78 of file vpDetectorBase.cpp.
Referenced by VispDetector::detectAprilTag:px:py:.
|
inlineinherited |
Returns the contained message of the ith object if there is one.
Definition at line 105 of file vpDetectorBase.h.
|
inherited |
Returns the contained message of the ith object if there is one.
Definition at line 66 of file vpDetectorBase.cpp.
|
inlineinherited |
Return the number of objects that are detected.
Definition at line 115 of file vpDetectorBase.h.
Referenced by VispDetector::detectAprilTag:px:py:.
|
inlineinherited |
Returns object container box as a vector of points.
Definition at line 120 of file vpDetectorBase.h.
Referenced by VispDetector::detectAprilTag:px:py:.
|
inherited |
Returns ith object container box as a vector of points.
Definition at line 54 of file vpDetectorBase.cpp.
bool vpDetectorAprilTag::getPose | ( | size_t | tagIndex, |
double | tagSize, | ||
const vpCameraParameters & | cam, | ||
vpHomogeneousMatrix & | cMo, | ||
vpHomogeneousMatrix * | cMo2 = NULL , |
||
double * | projError = NULL , |
||
double * | projError2 = NULL |
||
) |
Get the pose of a tag depending on its size and camera parameters. This function is useful to get the pose of tags with different sizes, while detect(const vpImage<unsigned char> &, const double, const vpCameraParameters &, std::vector<vpHomogeneousMatrix> &) considers that all the tags have the same size.
[in] | tagIndex | : Index of the tag. Value should be in range [0, nb tags-1] with nb_tags = getNbObjects(). Note that this index doesn't correspond to the tag id. |
[in] | tagSize | : Tag size in meter corresponding to the external width of the pattern. |
[in] | cam | : Camera intrinsic parameters. |
[out] | cMo | : Pose of the tag. |
[out] | cMo2 | : Optional second pose of the tag. |
[out] | projError | : Optional (sum of squared) projection errors in the normalized camera frame. |
[out] | projError2 | : Optional (sum of squared) projection errors for the 2nd solution in the normalized camera frame. |
The following code shows how to use this function:
Definition at line 841 of file vpDetectorAprilTag.cpp.
|
inline |
Return the pose estimation method.
Definition at line 264 of file vpDetectorAprilTag.h.
std::vector< std::vector< vpImagePoint > > vpDetectorAprilTag::getTagsCorners | ( | ) | const |
Return the corners coordinates for the detected tags.
Definition at line 910 of file vpDetectorAprilTag.cpp.
std::vector< int > vpDetectorAprilTag::getTagsId | ( | ) | const |
Return the decoded Apriltag id for each detection.
Definition at line 920 of file vpDetectorAprilTag.cpp.
std::vector< std::vector< vpPoint > > vpDetectorAprilTag::getTagsPoints3D | ( | const std::vector< int > & | tagsId, |
const std::map< int, double > & | tagsSize | ||
) | const |
Return a vector that contains for each tag id the corresponding tag 3D corners coordinates in the tag frame.
tagsId | : A vector containing the id of each tag that is detected. It's size corresponds to the number of tags that are detected. This vector is returned by getTagsId(). |
tagsSize | : a map that contains as first element a tag id and as second elements its 3D size in meter. When first element of this map is -1, the second element corresponds to the default tag size. std::map<int, double> tagsSize;
tagsSize[-1] = 0.05; // Default tag size in meter, used when detected tag id is not in this map
tagsSize[10] = 0.1; // All tags with id 10 are 0.1 meter large
tagsSize[20] = 0.2; // All tags with id 20 are 0.2 meter large
|
Definition at line 865 of file vpDetectorAprilTag.cpp.
vpDetectorAprilTag & vpDetectorAprilTag::operator= | ( | vpDetectorAprilTag | o | ) |
Definition at line 746 of file vpDetectorAprilTag.cpp.
References swap.
void vpDetectorAprilTag::setAprilTagDecodeSharpening | ( | double | decodeSharpening | ) |
Definition at line 925 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagFamily | ( | const vpAprilTagFamily & | tagFamily | ) |
Definition at line 930 of file vpDetectorAprilTag.cpp.
References m_poseEstimationMethod.
void vpDetectorAprilTag::setAprilTagNbThreads | ( | int | nThreads | ) |
Set the number of threads for April Tag detection (default is 1).
nThreads | : Number of thread. |
Definition at line 960 of file vpDetectorAprilTag.cpp.
Referenced by VispDetector::detectAprilTag:px:py:.
void vpDetectorAprilTag::setAprilTagPoseEstimationMethod | ( | const vpPoseEstimationMethod & | poseEstimationMethod | ) |
Set the method to use to compute the pose,
poseEstimationMethod | : The method to used. |
Definition at line 972 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagQuadDecimate | ( | float | quadDecimate | ) |
From the AprilTag code:
detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution.
Default is 1.0, increase this value to reduce the computation time.
quadDecimate | : Value for quad_decimate. |
Definition at line 990 of file vpDetectorAprilTag.cpp.
Referenced by VispDetector::detectAprilTag:px:py:.
void vpDetectorAprilTag::setAprilTagQuadSigma | ( | float | quadSigma | ) |
From the AprilTag code:
What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values (e.g. 0.8).
Default is 0.0.
quadSigma | : Value for quad_sigma. |
Definition at line 1007 of file vpDetectorAprilTag.cpp.
vp_deprecated void vpDetectorAprilTag::setAprilTagRefineDecode | ( | bool | refineDecode | ) |
Deprecated parameter from AprilTag 2 version.
Definition at line 1016 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagRefineEdges | ( | bool | refineEdges | ) |
From the AprilTag code:
When non-zero, the edges of the each quad are adjusted to "snap to" strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (1). Very computationally inexpensive. Option is ignored if quad_decimate = 1.
Default is 1.
refineEdges | : If true, set refine_edges to 1. |
Definition at line 1035 of file vpDetectorAprilTag.cpp.
vp_deprecated void vpDetectorAprilTag::setAprilTagRefinePose | ( | bool | refinePose | ) |
Deprecated parameter from AprilTag 2 version.
Definition at line 1044 of file vpDetectorAprilTag.cpp.
|
inline |
Allow to enable the display of overlay tag information in the windows (vpDisplay) associated to the input image.
Definition at line 282 of file vpDetectorAprilTag.h.
|
inlineinherited |
Set detector timeout in milli-seconds. When set to 0, there is no timeout.
Definition at line 128 of file vpDetectorBase.h.
void vpDetectorAprilTag::setZAlignedWithCameraAxis | ( | bool | zAlignedWithCameraFrame | ) |
Modify the resulting tag pose returned by getPose() in order to get a pose where z-axis is aligned when the camera plane is parallel to the tag.
zAlignedWithCameraFrame | : Flag to get a pose where z-axis is aligned with the camera frame. |
Definition at line 1062 of file vpDetectorAprilTag.cpp.
|
friend |
Definition at line 1050 of file vpDetectorAprilTag.cpp.
Referenced by operator=().
|
protected |
Definition at line 294 of file vpDetectorAprilTag.h.
|
protected |
Definition at line 295 of file vpDetectorAprilTag.h.
|
protected |
Definition at line 296 of file vpDetectorAprilTag.h.
|
protectedinherited |
Message attached to each object.
Definition at line 67 of file vpDetectorBase.h.
|
protectedinherited |
Number of detected objects.
Definition at line 68 of file vpDetectorBase.h.
|
protectedinherited |
For each object, defines the polygon that contains the object.
Definition at line 66 of file vpDetectorBase.h.
|
protected |
Definition at line 297 of file vpDetectorAprilTag.h.
Referenced by setAprilTagFamily().
|
protected |
Definition at line 298 of file vpDetectorAprilTag.h.
|
protectedinherited |
Detection timeout.
Definition at line 69 of file vpDetectorBase.h.