Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve AprilTag docs #5895

Merged
merged 3 commits into from
Nov 8, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ public double getCenterY() {

/**
* Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
* around the tag.
* around the tag. Index 0 is the bottom left corner.
*
* @param ndx Corner index (range is 0-3, inclusive)
* @return Corner point X coordinate
Expand All @@ -106,7 +106,7 @@ public double getCornerX(int ndx) {

/**
* Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
* around the tag.
* around the tag. Index 0 is the bottom left corner.
*
* @param ndx Corner index (range is 0-3, inclusive)
* @return Corner point Y coordinate
Expand All @@ -117,7 +117,8 @@ public double getCornerY(int ndx) {

/**
* Gets the corners of the tag in image pixel coordinates. These always wrap counter-clock wise
* around the tag.
* around the tag. The first set of corner coordinates are the coordinates for the bottom left
* corner.
*
* @return Corner point array (X and Y for each corner in order)
*/
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -244,7 +244,7 @@ public void addFamily(String fam) {
* Adds a family of tags to be detected.
*
* @param fam Family name, e.g. "tag16h5"
* @param bitsCorrected maximum number of bits to correct
* @param bitsCorrected Maximum number of bits to correct
* @throws IllegalArgumentException if family name not recognized
*/
public void addFamily(String fam, int bitsCorrected) {
Expand All @@ -270,6 +270,8 @@ public void clearFamilies() {
/**
* Detect tags from an 8-bit image.
*
* <p>The image must be grayscale.
*
* @param img 8-bit OpenCV Mat image
* @return Results (array of AprilTagDetection)
*/
Expand Down
102 changes: 102 additions & 0 deletions apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
Original file line number Diff line number Diff line change
Expand Up @@ -44,31 +44,119 @@ public static void setExtractOnStaticLoad(boolean load) {
}
}

/**
* Constructs an AprilTag detector engine.
*
* @return The detector engine handle
*/
public static native long createDetector();

/**
* Destroys an AprilTag detector engine.
*
* @param det The detector engine handle
*/
public static native void destroyDetector(long det);

/**
* Sets the detector engine configuration.
*
* @param det The detector engine handle
* @param config A configuration
*/
public static native void setDetectorConfig(long det, AprilTagDetector.Config config);

/**
* Gets the detector engine configuration.
*
* @param det The detector engine handle
* @return The configuration
*/
public static native AprilTagDetector.Config getDetectorConfig(long det);

/**
* Sets the detector engine quad threshold parameters.
*
* @param det The detector engine handle
* @param params Quad threshold parameters
*/
public static native void setDetectorQTP(
long det, AprilTagDetector.QuadThresholdParameters params);

/**
* Gets the detector engine quad threshold parameters.
*
* @param det The detector engine handle
* @return Quad threshold parameters
*/
public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det);

/**
* Adds a family of tags to be detected by the detector engine.
*
* @param det The detector engine handle
* @param fam Family name, e.g. "tag16h5"
* @param bitsCorrected Maximum number of bits to correct
* @return False if family can't be found
*/
public static native boolean addFamily(long det, String fam, int bitsCorrected);

/**
* Removes a family of tags from the detector.
*
* @param det The detector engine handle
* @param fam Family name, e.g. "tag16h5"
*/
public static native void removeFamily(long det, String fam);

/**
* Unregister all families.
*
* @param det The detector engine handle
*/
public static native void clearFamilies(long det);

/**
* Detect tags from an 8-bit image.
*
* @param det The detector engine handle
* @param width The width of the image
* @param height The height of the image
* @param stride The number of bytes between image rows (often the same as width)
* @param bufAddr The address of the image buffer
* @return The results (array of AprilTagDetection)
*/
public static native AprilTagDetection[] detect(
long det, int width, int height, int stride, long bufAddr);

/**
* Estimates the pose of the tag using the homography method described in [1].
*
* @param homography Homography 3x3 matrix data
* @param tagSize The tag size, in meters
* @param fx The camera horizontal focal length, in pixels
* @param fy The camera vertical focal length, in pixels
* @param cx The camera horizontal focal center, in pixels
* @param cy The camera vertical focal center, in pixels
* @return Pose estimate
*/
public static native Transform3d estimatePoseHomography(
double[] homography, double tagSize, double fx, double fy, double cx, double cy);

/**
* Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
* the object-space error of each.
*
* @param homography Homography 3x3 matrix data
* @param corners Corner point array (X and Y for each corner in order)
* @param tagSize The tag size, in meters
* @param fx The camera horizontal focal length, in pixels
* @param fy The camera vertical focal length, in pixels
* @param cx The camera horizontal focal center, in pixels
* @param cy The camera vertical focal center, in pixels
* @param nIters Number of iterations
* @return Initial and (possibly) second pose estimates
*/
public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
double[] homography,
double[] corners,
Expand All @@ -79,6 +167,20 @@ public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
double cy,
int nIters);

/**
* Estimates tag pose. This method is an easier to use interface to
* EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
* object-space error.
*
* @param homography Homography 3x3 matrix data
* @param corners Corner point array (X and Y for each corner in order)
* @param tagSize The tag size, in meters
* @param fx The camera horizontal focal length, in pixels
* @param fy The camera vertical focal length, in pixels
* @param cx The camera horizontal focal center, in pixels
* @param cy The camera vertical focal center, in pixels
* @return Pose estimate
*/
public static native Transform3d estimatePose(
double[] homography,
double[] corners,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class WPILIB_DLLEXPORT AprilTagDetection final {

/**
* Gets a corner of the tag in image pixel coordinates. These always
* wrap counter-clock wise around the tag.
* wrap counter-clock wise around the tag. Index 0 is the bottom left corner.
*
* @param ndx Corner index (range is 0-3, inclusive)
* @return Corner point
Expand All @@ -104,7 +104,8 @@ class WPILIB_DLLEXPORT AprilTagDetection final {

/**
* Gets the corners of the tag in image pixel coordinates. These always
* wrap counter-clock wise around the tag.
* wrap counter-clock wise around the tag. The first set of corner coordinates
* are the coordinates for the bottom left corner.
*
* @param cornersBuf Corner point array (X and Y for each corner in order)
* @return Corner point array (copy of cornersBuf span)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,7 @@ class WPILIB_DLLEXPORT AprilTagDetector {
* Adds a family of tags to be detected.
*
* @param fam Family name, e.g. "tag16h5"
* @param bitsCorrected
* @param bitsCorrected Maximum number of bits to correct
* @return False if family can't be found
*/
bool AddFamily(std::string_view fam, int bitsCorrected = 2);
Expand All @@ -226,6 +226,7 @@ class WPILIB_DLLEXPORT AprilTagDetector {

/**
* Detect tags from an 8-bit image.
* The image must be grayscale.
*
* @param width width of the image
* @param height height of the image
Expand All @@ -237,6 +238,7 @@ class WPILIB_DLLEXPORT AprilTagDetector {

/**
* Detect tags from an 8-bit image.
* The image must be grayscale.
*
* @param width width of the image
* @param height height of the image
Expand Down