![]() |
Visual Servoing Platform
version 3.2.0
|
#include <vpDetectorAprilTag.h>
Public Types | |
enum | vpAprilTagFamily { TAG_36h11, TAG_36h10, TAG_36ARTOOLKIT, TAG_25h9, TAG_25h7, TAG_16h5 } |
enum | vpPoseEstimationMethod { HOMOGRAPHY, HOMOGRAPHY_VIRTUAL_VS, DEMENTHON_VIRTUAL_VS, LAGRANGE_VIRTUAL_VS, BEST_RESIDUAL_VIRTUAL_VS } |
Public Member Functions | |
vpDetectorAprilTag (const vpAprilTagFamily &tagFamily=TAG_36h11, const vpPoseEstimationMethod &poseEstimationMethod=HOMOGRAPHY_VIRTUAL_VS) | |
virtual | ~vpDetectorAprilTag () |
bool | detect (const vpImage< unsigned char > &I) |
bool | detect (const vpImage< unsigned char > &I, const double tagSize, const vpCameraParameters &cam, std::vector< vpHomogeneousMatrix > &cMo_vec) |
bool | getPose (size_t tagIndex, const double tagSize, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo) |
vpPoseEstimationMethod | getPoseEstimationMethod () const |
void | setAprilTagNbThreads (const int nThreads) |
void | setAprilTagPoseEstimationMethod (const vpPoseEstimationMethod &poseEstimationMethod) |
void | setAprilTagQuadDecimate (const float quadDecimate) |
void | setAprilTagQuadSigma (const float quadSigma) |
void | setAprilTagRefineDecode (const bool refineDecode) |
void | setAprilTagRefineEdges (const bool refineEdges) |
void | setAprilTagRefinePose (const bool refinePose) |
void | setDisplayTag (const bool display, const vpColor &color=vpColor::none, const 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) |
Protected Attributes | |
bool | m_displayTag |
vpColor | m_displayTagColor |
unsigned int | m_displayTagThickness |
vpPoseEstimationMethod | m_poseEstimationMethod |
vpAprilTagFamily | m_tagFamily |
bool | m_zAlignedWithCameraFrame |
std::vector< std::vector< vpImagePoint > > | m_polygon |
std::vector< std::string > | m_message |
size_t | m_nb_objects |
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]) and AprilTag 2: Efficient and robust fiducial detection ([wang2016iros]).
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 212 of file vpDetectorAprilTag.h.
Enumerator | |
---|---|
TAG_36h11 | AprilTag 36h11 pattern (recommended) |
TAG_36h10 | AprilTag 36h10 pattern |
TAG_36ARTOOLKIT | ARToolKit pattern. |
TAG_25h9 | AprilTag 25h9 pattern |
TAG_25h7 | AprilTag 25h7 pattern |
TAG_16h5 | AprilTag 16h5 pattern |
Definition at line 216 of file vpDetectorAprilTag.h.
Definition at line 236 of file vpDetectorAprilTag.h.
vpDetectorAprilTag::vpDetectorAprilTag | ( | const vpAprilTagFamily & | tagFamily = TAG_36h11 , |
const vpPoseEstimationMethod & | poseEstimationMethod = HOMOGRAPHY_VIRTUAL_VS |
||
) |
Default constructor.
Definition at line 384 of file vpDetectorAprilTag.cpp.
|
virtual |
Destructor that desallocate memory.
Definition at line 395 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 404 of file vpDetectorAprilTag.cpp.
bool vpDetectorAprilTag::detect | ( | const vpImage< unsigned char > & | I, |
const double | tagSize, | ||
const vpCameraParameters & | cam, | ||
std::vector< vpHomogeneousMatrix > & | cMo_vec | ||
) |
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().
I | : Input image. |
tagSize | : Tag size in meter corresponding to the external width of the pattern. |
cam | : Camera intrinsic parameters. |
cMo_vec | : List of tag poses. |
Definition at line 431 of file vpDetectorAprilTag.cpp.
|
inherited |
Return the bounding box of the ith object.
Definition at line 86 of file vpDetectorBase.cpp.
|
inherited |
Return the center of gravity location of the ith object.
Definition at line 73 of file vpDetectorBase.cpp.
|
inlineinherited |
Returns the contained message of the ith object if there is one.
Definition at line 106 of file vpDetectorBase.h.
|
inherited |
Returns the contained message of the ith object if there is one.
Definition at line 61 of file vpDetectorBase.cpp.
|
inlineinherited |
Return the number of objects that are detected.
Definition at line 116 of file vpDetectorBase.h.
|
inlineinherited |
Returns object container box as a vector of points.
Definition at line 121 of file vpDetectorBase.h.
|
inherited |
Returns ith object container box as a vector of points.
Definition at line 49 of file vpDetectorBase.cpp.
bool vpDetectorAprilTag::getPose | ( | size_t | tagIndex, |
const double | tagSize, | ||
const vpCameraParameters & | cam, | ||
vpHomogeneousMatrix & | cMo | ||
) |
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(). |
[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. |
The following code shows how to use this function:
Definition at line 474 of file vpDetectorAprilTag.cpp.
|
inline |
Return the pose estimation method.
Definition at line 262 of file vpDetectorAprilTag.h.
void vpDetectorAprilTag::setAprilTagNbThreads | ( | const int | nThreads | ) |
Set the number of threads for April Tag detection (default is 1).
nThreads | : Number of thread. |
Definition at line 484 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagPoseEstimationMethod | ( | const vpPoseEstimationMethod & | poseEstimationMethod | ) |
Set the method to use to compute the pose,
poseEstimationMethod | : The method to used. |
Definition at line 495 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagQuadDecimate | ( | const 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 513 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagQuadSigma | ( | const 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 527 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagRefineDecode | ( | const bool | refineDecode | ) |
From the AprilTag code:
when non-zero, detections are refined in a way intended to increase the number of detected tags. Especially effective for very small tags near the resolution threshold (e.g. 10px on a side).
Default is 0.
refineDecode | : If true, set refine_decode to 1. |
Definition at line 541 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagRefineEdges | ( | const 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 557 of file vpDetectorAprilTag.cpp.
void vpDetectorAprilTag::setAprilTagRefinePose | ( | const bool | refinePose | ) |
From the AprilTag code:
when non-zero, detections are refined in a way intended to increase the accuracy of the extracted pose. This is done by maximizing the contrast around the black and white border of the tag. This generally increases the number of successfully detected tags, though not as effectively (or quickly) as refine_decode. This option must be enabled in order for "goodness" to be computed.
Default is 0.
refinePose | : If true, set refine_pose to 1. |
Definition at line 575 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 274 of file vpDetectorAprilTag.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 582 of file vpDetectorAprilTag.cpp.
|
protected |
Definition at line 284 of file vpDetectorAprilTag.h.
|
protected |
Definition at line 285 of file vpDetectorAprilTag.h.
|
protected |
Definition at line 286 of file vpDetectorAprilTag.h.
|
protectedinherited |
Message attached to each object.
Definition at line 70 of file vpDetectorBase.h.
|
protectedinherited |
Number of detected objects.
Definition at line 71 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 287 of file vpDetectorAprilTag.h.
|
protected |
Definition at line 288 of file vpDetectorAprilTag.h.
|
protected |
Definition at line 289 of file vpDetectorAprilTag.h.