diff --git a/.styleguide b/.styleguide index 05958bd400..9df73f7ec1 100644 --- a/.styleguide +++ b/.styleguide @@ -18,6 +18,7 @@ generatedFileExclude { FRCNetComm\.java$ simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$ fieldImages/src/main/native/resources/ + apriltag/src/test/resources/ } repoRootNameOverride { diff --git a/apriltag/build.gradle b/apriltag/build.gradle index d6d30749fb..960af58680 100644 --- a/apriltag/build.gradle +++ b/apriltag/build.gradle @@ -4,6 +4,11 @@ ext { nativeName = 'apriltag' devMain = 'edu.wpi.first.apriltag.DevMain' useJava = true + useCpp = true + sharedCvConfigs = [ + apriltagDev : [], + apriltagTest: []] + staticCvConfigs = [] def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project) @@ -41,7 +46,6 @@ sourceSets { } } - model { components {} binaries { diff --git a/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java b/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java index 2479cb58f1..1047088044 100644 --- a/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java +++ b/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java @@ -4,14 +4,16 @@ package edu.wpi.first.apriltag; -import edu.wpi.first.apriltag.jni.AprilTagJNI; - public final class DevMain { /** Main entry point. */ public static void main(String[] args) { System.out.println("Hello World!"); - var detector = AprilTagJNI.aprilTagCreate("tag16h5", 2.0, 0.0, 1, false, false); - AprilTagJNI.aprilTagDestroy(detector); + AprilTagDetector detector = new AprilTagDetector(); + detector.addFamily("tag16h5"); + AprilTagDetector.Config config = new AprilTagDetector.Config(); + config.refineEdges = false; + detector.setConfig(config); + detector.close(); } private DevMain() {} diff --git a/apriltag/src/dev/native/cpp/main.cpp b/apriltag/src/dev/native/cpp/main.cpp index a3e363efca..515202edf0 100644 --- a/apriltag/src/dev/native/cpp/main.cpp +++ b/apriltag/src/dev/native/cpp/main.cpp @@ -2,4 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -int main() {} +#include "frc/apriltag/AprilTagDetector.h" + +int main() { + frc::AprilTagDetector detector; + detector.AddFamily("tag16h5"); + detector.SetConfig({.refineEdges = false}); +} diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java new file mode 100644 index 0000000000..9270cc9ff0 --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java @@ -0,0 +1,190 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N3; +import java.util.Arrays; + +/** A detection of an AprilTag tag. */ +public class AprilTagDetection { + /** + * Gets the decoded tag's family name. + * + * @return Decoded family name + */ + public String getFamily() { + return m_family; + } + + /** + * Gets the decoded ID of the tag. + * + * @return Decoded ID + */ + public int getId() { + return m_id; + } + + /** + * Gets how many error bits were corrected. Note: accepting large numbers of corrected errors + * leads to greatly increased false positive rates. NOTE: As of this implementation, the detector + * cannot detect tags with a hamming distance greater than 2. + * + * @return Hamming distance (number of corrected error bits) + */ + public int getHamming() { + return m_hamming; + } + + /** + * Gets a measure of the quality of the binary decoding process: the average difference between + * the intensity of a data bit versus the decision threshold. Higher numbers roughly indicate + * better decodes. This is a reasonable measure of detection accuracy only for very small tags-- + * not effective for larger tags (where we could have sampled anywhere within a bit cell and still + * gotten a good detection.) + * + * @return Decision margin + */ + public float getDecisionMargin() { + return m_decisionMargin; + } + + /** + * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at + * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image. + * + * @return Homography matrix data + */ + @SuppressWarnings("PMD.MethodReturnsInternalArray") + public double[] getHomography() { + return m_homography; + } + + /** + * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at + * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image. + * + * @return Homography matrix + */ + public Matrix getHomographyMatrix() { + return new MatBuilder<>(Nat.N3(), Nat.N3()).fill(m_homography); + } + + /** + * Gets the center of the detection in image pixel coordinates. + * + * @return Center point X coordinate + */ + public double getCenterX() { + return m_centerX; + } + + /** + * Gets the center of the detection in image pixel coordinates. + * + * @return Center point Y coordinate + */ + public double getCenterY() { + return m_centerY; + } + + /** + * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise + * around the tag. + * + * @param ndx Corner index (range is 0-3, inclusive) + * @return Corner point X coordinate + */ + public double getCornerX(int ndx) { + return m_corners[ndx * 2]; + } + + /** + * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise + * around the tag. + * + * @param ndx Corner index (range is 0-3, inclusive) + * @return Corner point Y coordinate + */ + public double getCornerY(int ndx) { + return m_corners[ndx * 2 + 1]; + } + + /** + * Gets the corners of the tag in image pixel coordinates. These always wrap counter-clock wise + * around the tag. + * + * @return Corner point array (X and Y for each corner in order) + */ + @SuppressWarnings("PMD.MethodReturnsInternalArray") + public double[] getCorners() { + return m_corners; + } + + private final String m_family; + private final int m_id; + private final int m_hamming; + private final float m_decisionMargin; + private final double[] m_homography; + private final double m_centerX; + private final double m_centerY; + private final double[] m_corners; + + /** + * Constructs a new detection result. Used from JNI. + * + * @param family family + * @param id id + * @param hamming hamming + * @param decisionMargin dm + * @param homography homography + * @param centerX centerX + * @param centerY centerY + * @param corners corners + */ + @SuppressWarnings("PMD.ArrayIsStoredDirectly") + public AprilTagDetection( + String family, + int id, + int hamming, + float decisionMargin, + double[] homography, + double centerX, + double centerY, + double[] corners) { + m_family = family; + m_id = id; + m_hamming = hamming; + m_decisionMargin = decisionMargin; + m_homography = homography; + m_centerX = centerX; + m_centerY = centerY; + m_corners = corners; + } + + @Override + public String toString() { + return "DetectionResult [centerX=" + + m_centerX + + ", centerY=" + + m_centerY + + ", corners=" + + Arrays.toString(m_corners) + + ", decisionMargin=" + + m_decisionMargin + + ", hamming=" + + m_hamming + + ", homography=" + + Arrays.toString(m_homography) + + ", family=" + + m_family + + ", id=" + + m_id + + "]"; + } +} diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java new file mode 100644 index 0000000000..b8e9cb00b6 --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java @@ -0,0 +1,281 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +import edu.wpi.first.apriltag.jni.AprilTagJNI; +import org.opencv.core.Mat; + +/** + * An AprilTag detector engine. This is expensive to set up and tear down, so most use cases should + * only create one of these, add a family to it, set up any other configuration, and repeatedly call + * Detect(). + */ +public class AprilTagDetector implements AutoCloseable { + /** Detector configuration. */ + @SuppressWarnings("MemberName") + public static class Config { + /** + * How many threads should be used for computation. Default is single-threaded operation (1 + * thread). + */ + public int numThreads = 1; + + /** + * Quad decimation. 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 2.0. + */ + public float quadDecimate = 2.0f; + + /** + * What Gaussian blur should be applied to the segmented image (used for quad detection). Very + * noisy images benefit from non-zero values (e.g. 0.8). Default is 0.0. + */ + public float quadSigma; + + /** + * When true, 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 (true). Default is true. + * + *

Very computationally inexpensive. Option is ignored if quad_decimate = 1. + */ + public boolean refineEdges = true; + + /** + * How much sharpening should be done to decoded images. This can help decode small tags but may + * or may not help in odd lighting conditions or low light conditions. Default is 0.25. + */ + public double decodeSharpening = 0.25; + + /** + * Debug mode. When true, the decoder writes a variety of debugging images to the current + * working directory at various stages through the detection process. This is slow and should + * *not* be used on space-limited systems such as the RoboRIO. Default is disabled (false). + */ + public boolean debug; + + public Config() {} + + Config( + int numThreads, + float quadDecimate, + float quadSigma, + boolean refineEdges, + double decodeSharpening, + boolean debug) { + this.numThreads = numThreads; + this.quadDecimate = quadDecimate; + this.quadSigma = quadSigma; + this.refineEdges = refineEdges; + this.decodeSharpening = decodeSharpening; + this.debug = debug; + } + + @Override + public int hashCode() { + return numThreads + + Float.hashCode(quadDecimate) + + Float.hashCode(quadSigma) + + Boolean.hashCode(refineEdges) + + Double.hashCode(decodeSharpening) + + Boolean.hashCode(debug); + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof Config)) { + return false; + } + + Config other = (Config) obj; + return numThreads == other.numThreads + && quadDecimate == other.quadDecimate + && quadSigma == other.quadSigma + && refineEdges == other.refineEdges + && decodeSharpening == other.decodeSharpening + && debug == other.debug; + } + } + + /** Quad threshold parameters. */ + @SuppressWarnings("MemberName") + public static class QuadThresholdParameters { + /** Threshold used to reject quads containing too few pixels. Default is 5 pixels. */ + public int minClusterPixels = 5; + + /** + * How many corner candidates to consider when segmenting a group of pixels into a quad. Default + * is 10. + */ + public int maxNumMaxima = 10; + + /** + * Critical angle, in radians. The detector will reject quads where pairs of edges have angles + * that are close to straight or close to 180 degrees. Zero means that no quads are rejected. + * Default is 10 degrees. + */ + public double criticalAngle = 10 * Math.PI / 180.0; + + /** + * When fitting lines to the contours, the maximum mean squared error allowed. This is useful in + * rejecting contours that are far from being quad shaped; rejecting these quads "early" saves + * expensive decoding processing. Default is 10.0. + */ + public float maxLineFitMSE = 10.0f; + + /** + * Minimum brightness offset. When we build our model of black & white pixels, we add an + * extra check that the white model must be (overall) brighter than the black model. How much + * brighter? (in pixel values, [0,255]). Default is 5. + */ + public int minWhiteBlackDiff = 5; + + /** + * Whether the thresholded image be should be deglitched. Only useful for very noisy images. + * Default is disabled (false). + */ + public boolean deglitch; + + public QuadThresholdParameters() {} + + QuadThresholdParameters( + int minClusterPixels, + int maxNumMaxima, + double criticalAngle, + float maxLineFitMSE, + int minWhiteBlackDiff, + boolean deglitch) { + this.minClusterPixels = minClusterPixels; + this.maxNumMaxima = maxNumMaxima; + this.criticalAngle = criticalAngle; + this.maxLineFitMSE = maxLineFitMSE; + this.minWhiteBlackDiff = minWhiteBlackDiff; + this.deglitch = deglitch; + } + + @Override + public int hashCode() { + return minClusterPixels + + maxNumMaxima + + Double.hashCode(criticalAngle) + + Float.hashCode(maxLineFitMSE) + + minWhiteBlackDiff + + Boolean.hashCode(deglitch); + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof QuadThresholdParameters)) { + return false; + } + + QuadThresholdParameters other = (QuadThresholdParameters) obj; + return minClusterPixels == other.minClusterPixels + && maxNumMaxima == other.maxNumMaxima + && criticalAngle == other.criticalAngle + && maxLineFitMSE == other.maxLineFitMSE + && minWhiteBlackDiff == other.minWhiteBlackDiff + && deglitch == other.deglitch; + } + } + + public AprilTagDetector() { + m_native = AprilTagJNI.createDetector(); + } + + @Override + public void close() { + if (m_native != 0) { + AprilTagJNI.destroyDetector(m_native); + } + m_native = 0; + } + + /** + * Sets detector configuration. + * + * @param config Configuration + */ + public void setConfig(Config config) { + AprilTagJNI.setDetectorConfig(m_native, config); + } + + /** + * Gets detector configuration. + * + * @return Configuration + */ + public Config getConfig() { + return AprilTagJNI.getDetectorConfig(m_native); + } + + /** + * Sets quad threshold parameters. + * + * @param params Parameters + */ + public void setQuadThresholdParameters(QuadThresholdParameters params) { + AprilTagJNI.setDetectorQTP(m_native, params); + } + + /** + * Gets quad threshold parameters. + * + * @return Parameters + */ + public QuadThresholdParameters getQuadThresholdParameters() { + return AprilTagJNI.getDetectorQTP(m_native); + } + + /** + * Adds a family of tags to be detected. + * + * @param fam Family name, e.g. "tag16h5" + * @throws IllegalArgumentException if family name not recognized + */ + public void addFamily(String fam) { + addFamily(fam, 2); + } + + /** + * Adds a family of tags to be detected. + * + * @param fam Family name, e.g. "tag16h5" + * @param bitsCorrected maximum number of bits to correct + * @throws IllegalArgumentException if family name not recognized + */ + public void addFamily(String fam, int bitsCorrected) { + if (!AprilTagJNI.addFamily(m_native, fam, bitsCorrected)) { + throw new IllegalArgumentException("unknown family name '" + fam + "'"); + } + } + + /** + * Removes a family of tags from the detector. + * + * @param fam Family name, e.g. "tag16h5" + */ + public void removeFamily(String fam) { + AprilTagJNI.removeFamily(m_native, fam); + } + + /** Unregister all families. */ + public void clearFamilies() { + AprilTagJNI.clearFamilies(m_native); + } + + /** + * Detect tags from an 8-bit image. + * + * @param img 8-bit OpenCV Mat image + * @return Results (array of AprilTagDetection) + */ + public AprilTagDetection[] detect(Mat img) { + return AprilTagJNI.detect(m_native, img.cols(), img.rows(), img.cols(), img.dataAddr()); + } + + private long m_native; +} diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java new file mode 100644 index 0000000000..7bf079a0f7 --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +import edu.wpi.first.math.geometry.Transform3d; + +/** A pair of AprilTag pose estimates. */ +@SuppressWarnings("MemberName") +public class AprilTagPoseEstimate { + /** + * Constructs a pose estimate. + * + * @param pose1 first pose + * @param pose2 second pose + * @param error1 error of first pose + * @param error2 error of second pose + */ + public AprilTagPoseEstimate(Transform3d pose1, Transform3d pose2, double error1, double error2) { + this.pose1 = pose1; + this.pose2 = pose2; + this.error1 = error1; + this.error2 = error2; + } + + /** + * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be + * ambiguous. + * + * @return The ratio of pose reprojection errors. + */ + public double getAmbiguity() { + double min = Math.min(error1, error2); + double max = Math.max(error1, error2); + + if (max > 0) { + return min / max; + } else { + return -1; + } + } + + /** Pose 1. */ + public final Transform3d pose1; + + /** Pose 2. */ + public final Transform3d pose2; + + /** Object-space error of pose 1. */ + public final double error1; + + /** Object-space error of pose 2. */ + public final double error2; +} diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java new file mode 100644 index 0000000000..2b7f68a526 --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java @@ -0,0 +1,190 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +import edu.wpi.first.apriltag.jni.AprilTagJNI; +import edu.wpi.first.math.geometry.Transform3d; + +/** Pose estimators for AprilTag tags. */ +public class AprilTagPoseEstimator { + /** Configuration for the pose estimator. */ + @SuppressWarnings("MemberName") + public static class Config { + /** + * Creates a pose estimator configuration. + * + * @param tagSize tag size, in meters + * @param fx camera horizontal focal length, in pixels + * @param fy camera vertical focal length, in pixels + * @param cx camera horizontal focal center, in pixels + * @param cy camera vertical focal center, in pixels + */ + public Config(double tagSize, double fx, double fy, double cx, double cy) { + this.tagSize = tagSize; + this.fx = fx; + this.fy = fy; + this.cx = cx; + this.cy = cy; + } + + public double tagSize; + public double fx; + public double fy; + public double cx; + public double cy; + + @Override + public int hashCode() { + return Double.hashCode(tagSize) + + Double.hashCode(fx) + + Double.hashCode(fy) + + Double.hashCode(cx) + + Double.hashCode(cy); + } + + @Override + public boolean equals(Object obj) { + if (!(obj instanceof Config)) { + return false; + } + + Config other = (Config) obj; + return tagSize == other.tagSize + && fx == other.fx + && fy == other.fy + && cx == other.cx + && cy == other.cy; + } + } + + /** + * Creates estimator. + * + * @param config Configuration + */ + public AprilTagPoseEstimator(Config config) { + m_config = new Config(config.tagSize, config.fx, config.fy, config.cx, config.cy); + } + + /** + * Sets estimator configuration. + * + * @param config Configuration + */ + public void setConfig(Config config) { + m_config.tagSize = config.tagSize; + m_config.fx = config.fx; + m_config.fy = config.fy; + m_config.cx = config.cx; + m_config.cy = config.cy; + } + + /** + * Gets estimator configuration. + * + * @return Configuration + */ + public Config getConfig() { + return new Config(m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy); + } + + /** + * Estimates the pose of the tag using the homography method described in [1]. + * + * @param detection Tag detection + * @return Pose estimate + */ + public Transform3d estimateHomography(AprilTagDetection detection) { + return estimateHomography(detection.getHomography()); + } + + /** + * Estimates the pose of the tag using the homography method described in [1]. + * + * @param homography Homography 3x3 matrix data + * @return Pose estimate + */ + public Transform3d estimateHomography(double[] homography) { + return AprilTagJNI.estimatePoseHomography( + homography, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.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. + * + *

This uses the homography method described in [1] for the initial estimate. Then Orthogonal + * Iteration [2] is used to refine this estimate. Then [3] is used to find a potential second + * local minima and Orthogonal Iteration is used to refine this second estimate. + * + *

[1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in 2011 IEEE + * International Conference on Robotics and Automation, May 2011, pp. 3400–3407. + * + *

[2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose estimation from + * video images," in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no. + * 6, pp. 610-622, June 2000. doi: 10.1109/34.862199 + * + *

[3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar Target," in IEEE + * Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 12, pp. 2024-2030, Dec. + * 2006. doi: 10.1109/TPAMI.2006.252 + * + * @param detection Tag detection + * @param nIters Number of iterations + * @return Initial and (possibly) second pose estimates + */ + public AprilTagPoseEstimate estimateOrthogonalIteration(AprilTagDetection detection, int nIters) { + return estimateOrthogonalIteration(detection.getHomography(), detection.getCorners(), nIters); + } + + /** + * 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 nIters Number of iterations + * @return Initial and (possibly) second pose estimates + */ + public AprilTagPoseEstimate estimateOrthogonalIteration( + double[] homography, double[] corners, int nIters) { + return AprilTagJNI.estimatePoseOrthogonalIteration( + homography, + corners, + m_config.tagSize, + m_config.fx, + m_config.fy, + m_config.cx, + m_config.cy, + 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 detection Tag detection + * @return Pose estimate + */ + public Transform3d estimate(AprilTagDetection detection) { + return estimate(detection.getHomography(), detection.getCorners()); + } + + /** + * 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) + * @return Pose estimate + */ + public Transform3d estimate(double[] homography, double[] corners) { + return AprilTagJNI.estimatePose( + homography, corners, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy); + } + + private final Config m_config; +} diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java index 1b3c172e6c..1a7c692d0c 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java @@ -4,10 +4,13 @@ package edu.wpi.first.apriltag.jni; +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; -import org.opencv.core.Mat; public class AprilTagJNI { static boolean libraryLoaded = false; @@ -41,49 +44,47 @@ public class AprilTagJNI { } } - // Returns a pointer to a apriltag_detector_t - public static native int aprilTagCreate( - String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges); + public static native long createDetector(); - // Destroy and free a previously created detector. - public static native void aprilTagDestroy(int detector); + public static native void destroyDetector(long det); - private static native Object[] aprilTagDetectInternal( - int detector, - long imgAddr, - int rows, - int cols, - boolean doPoseEstimation, - double tagWidth, + public static native void setDetectorConfig(long det, AprilTagDetector.Config config); + + public static native AprilTagDetector.Config getDetectorConfig(long det); + + public static native void setDetectorQTP( + long det, AprilTagDetector.QuadThresholdParameters params); + + public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det); + + public static native boolean addFamily(long det, String fam, int bitsCorrected); + + public static native void removeFamily(long det, String fam); + + public static native void clearFamilies(long det); + + public static native AprilTagDetection[] detect( + long det, int width, int height, int stride, long bufAddr); + + public static native Transform3d estimatePoseHomography( + double[] homography, double tagSize, double fx, double fy, double cx, double cy); + + public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration( + double[] homography, + double[] corners, + double tagSize, double fx, double fy, double cx, double cy, int nIters); - // Detect targets given a GRAY frame. Returns a pointer toa zarray - public static DetectionResult[] aprilTagDetect( - int detector, - Mat img, - boolean doPoseEstimation, - double tagWidth, + public static native Transform3d estimatePose( + double[] homography, + double[] corners, + double tagSize, double fx, double fy, double cx, - double cy, - int nIters) { - return (DetectionResult[]) - aprilTagDetectInternal( - detector, - img.dataAddr(), - img.rows(), - img.cols(), - doPoseEstimation, - tagWidth, - fx, - fy, - cx, - cy, - nIters); - } + double cy); } diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java deleted file mode 100644 index dd811b504a..0000000000 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java +++ /dev/null @@ -1,226 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.apriltag.jni; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N3; -import java.util.Arrays; -import org.ejml.data.DMatrixRMaj; -import org.ejml.dense.row.factory.DecompositionFactory_DDRM; -import org.ejml.simple.SimpleMatrix; - -public class DetectionResult { - public int getId() { - return m_id; - } - - public int getHamming() { - return m_hamming; - } - - public float getDecisionMargin() { - return m_decisionMargin; - } - - public void setDecisionMargin(float decisionMargin) { - this.m_decisionMargin = decisionMargin; - } - - @SuppressWarnings("PMD.MethodReturnsInternalArray") - public double[] getHomography() { - return m_homography; - } - - @SuppressWarnings("PMD.ArrayIsStoredDirectly") - public void setHomography(double[] homography) { - this.m_homography = homography; - } - - public double getCenterX() { - return m_centerX; - } - - public void setCenterX(double centerX) { - this.m_centerX = centerX; - } - - public double getCenterY() { - return m_centerY; - } - - public void setCenterY(double centerY) { - this.m_centerY = centerY; - } - - @SuppressWarnings("PMD.MethodReturnsInternalArray") - public double[] getCorners() { - return m_corners; - } - - @SuppressWarnings("PMD.ArrayIsStoredDirectly") - public void setCorners(double[] corners) { - this.m_corners = corners; - } - - public double getError1() { - return m_error1; - } - - public double getError2() { - return m_error2; - } - - public Transform3d getPoseResult1() { - return m_poseResult1; - } - - public Transform3d getPoseResult2() { - return m_poseResult2; - } - - private final int m_id; - private final int m_hamming; - private float m_decisionMargin; - private double[] m_homography; - private double m_centerX; - private double m_centerY; - private double[] m_corners; - - private final Transform3d m_poseResult1; - private final double m_error1; - private final Transform3d m_poseResult2; - private final double m_error2; - - /** - * Constructs a new detection result. Used from JNI. - * - * @param id id - * @param hamming hamming - * @param decisionMargin dm - * @param homography homography - * @param centerX centerX - * @param centerY centerY - * @param corners corners - * @param pose1TransArr pose1TransArr - * @param pose1RotArr pose1RotArr - * @param err1 err1 - * @param pose2TransArr pose2TransArr - * @param pose2RotArr pose2RotArr - * @param err2 err2 - */ - @SuppressWarnings("PMD.ArrayIsStoredDirectly") - public DetectionResult( - int id, - int hamming, - float decisionMargin, - double[] homography, - double centerX, - double centerY, - double[] corners, - double[] pose1TransArr, - double[] pose1RotArr, - double err1, - double[] pose2TransArr, - double[] pose2RotArr, - double err2) { - this.m_id = id; - this.m_hamming = hamming; - this.m_decisionMargin = decisionMargin; - this.m_homography = homography; - this.m_centerX = centerX; - this.m_centerY = centerY; - this.m_corners = corners; - - this.m_error1 = err1; - this.m_poseResult1 = - new Transform3d( - new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]), - new Rotation3d( - orthogonalizeRotationMatrix( - new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr)))); - this.m_error2 = err2; - this.m_poseResult2 = - new Transform3d( - new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]), - new Rotation3d( - orthogonalizeRotationMatrix( - new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)))); - } - - /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. - * - * @return The ratio of pose reprojection errors. - */ - public double getPoseAmbiguity() { - var min = Math.min(m_error1, m_error2); - var max = Math.max(m_error1, m_error2); - - if (max > 0) { - return min / max; - } else { - return -1; - } - } - - @Override - public String toString() { - return "DetectionResult [centerX=" - + m_centerX - + ", centerY=" - + m_centerY - + ", corners=" - + Arrays.toString(m_corners) - + ", decisionMargin=" - + m_decisionMargin - + ", error1=" - + m_error1 - + ", error2=" - + m_error2 - + ", hamming=" - + m_hamming - + ", homography=" - + Arrays.toString(m_homography) - + ", id=" - + m_id - + ", poseResult1=" - + m_poseResult1 - + ", poseResult2=" - + m_poseResult2 - + "]"; - } - - private static Matrix orthogonalizeRotationMatrix(Matrix input) { - var a = DecompositionFactory_DDRM.qr(3, 3); - if (!a.decompose(input.getStorage().getDDRM())) { - // best we can do is return the input - return input; - } - - // Grab results (thanks for this _great_ api, EJML) - var Q = new DMatrixRMaj(3, 3); - var R = new DMatrixRMaj(3, 3); - a.getQ(Q, false); - a.getR(R, false); - - // Fix signs in R if they're < 0 so it's close to an identity matrix - // (our QR decomposition implementation sometimes flips the signs of columns) - for (int colR = 0; colR < 3; ++colR) { - if (R.get(colR, colR) < 0) { - for (int rowQ = 0; rowQ < 3; ++rowQ) { - Q.set(rowQ, colR, -Q.get(rowQ, colR)); - } - } - } - - return new Matrix<>(new SimpleMatrix(Q)); - } -} diff --git a/apriltag/src/main/native/cpp/AprilTagDetection.cpp b/apriltag/src/main/native/cpp/AprilTagDetection.cpp new file mode 100644 index 0000000000..aaa80be8c6 --- /dev/null +++ b/apriltag/src/main/native/cpp/AprilTagDetection.cpp @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagDetection.h" + +#include + +#ifdef _WIN32 +#pragma warning(disable : 4200) +#elif defined(__clang__) +#pragma clang diagnostic ignored "-Wc99-extensions" +#elif defined(__GNUC__) +#pragma GCC diagnostic ignored "-Wpedantic" +#endif + +#include "apriltag.h" + +using namespace frc; + +static_assert(sizeof(AprilTagDetection) == sizeof(apriltag_detection_t), + "structure sizes don't match"); +static_assert(std::is_standard_layout_v, + "AprilTagDetection is not standard layout?"); + +std::string_view AprilTagDetection::GetFamily() const { + return static_cast(family)->name; +} + +std::span AprilTagDetection::GetHomography() const { + return std::span{static_cast(H)->data, 9}; +} + +Eigen::Matrix3d AprilTagDetection::GetHomographyMatrix() const { + return Eigen::Map>{ + static_cast(H)->data}; +} diff --git a/apriltag/src/main/native/cpp/AprilTagDetector.cpp b/apriltag/src/main/native/cpp/AprilTagDetector.cpp new file mode 100644 index 0000000000..cae9c0a5e5 --- /dev/null +++ b/apriltag/src/main/native/cpp/AprilTagDetector.cpp @@ -0,0 +1,200 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagDetector.h" + +#include +#include + +#ifdef _WIN32 +#pragma warning(disable : 4200) +#elif defined(__clang__) +#pragma clang diagnostic ignored "-Wc99-extensions" +#elif defined(__GNUC__) +#pragma GCC diagnostic ignored "-Wpedantic" +#endif + +#include "apriltag.h" +#include "tag16h5.h" +#include "tag25h9.h" +#include "tag36h11.h" +#include "tagCircle21h7.h" +#include "tagCircle49h12.h" +#include "tagCustom48h12.h" +#include "tagStandard41h12.h" +#include "tagStandard52h13.h" + +using namespace frc; + +AprilTagDetector::Results::Results(void* impl, const private_init&) + : span{reinterpret_cast( + static_cast(impl)->data), + static_cast(static_cast(impl)->size)}, + m_impl{impl} {} + +AprilTagDetector::Results& AprilTagDetector::Results::operator=(Results&& rhs) { + Destroy(); + m_impl = rhs.m_impl; + rhs.m_impl = nullptr; + return *this; +} + +void AprilTagDetector::Results::Destroy() { + if (m_impl) { + apriltag_detections_destroy(static_cast(m_impl)); + } +} + +AprilTagDetector::AprilTagDetector() : m_impl{apriltag_detector_create()} {} + +AprilTagDetector& AprilTagDetector::operator=(AprilTagDetector&& rhs) { + Destroy(); + m_impl = rhs.m_impl; + rhs.m_impl = nullptr; + m_families = std::move(rhs.m_families); + rhs.m_families.clear(); + m_qtpCriticalAngle = rhs.m_qtpCriticalAngle; + return *this; +} + +void AprilTagDetector::SetConfig(const Config& config) { + auto& impl = *static_cast(m_impl); + impl.nthreads = config.numThreads; + impl.quad_decimate = config.quadDecimate; + impl.quad_sigma = config.quadSigma; + impl.refine_edges = config.refineEdges; + impl.decode_sharpening = config.decodeSharpening; + impl.debug = config.debug; +} + +AprilTagDetector::Config AprilTagDetector::GetConfig() const { + auto& impl = *static_cast(m_impl); + return { + .numThreads = impl.nthreads, + .quadDecimate = impl.quad_decimate, + .quadSigma = impl.quad_sigma, + .refineEdges = impl.refine_edges, + .decodeSharpening = impl.decode_sharpening, + .debug = impl.debug, + }; +} + +void AprilTagDetector::SetQuadThresholdParameters( + const QuadThresholdParameters& params) { + auto& qtp = static_cast(m_impl)->qtp; + qtp.min_cluster_pixels = params.minClusterPixels; + qtp.max_nmaxima = params.maxNumMaxima; + qtp.critical_rad = params.criticalAngle.value(); + qtp.cos_critical_rad = std::cos(params.criticalAngle.value()); + qtp.max_line_fit_mse = params.maxLineFitMSE; + qtp.min_white_black_diff = params.minWhiteBlackDiff; + qtp.deglitch = params.deglitch; + + m_qtpCriticalAngle = params.criticalAngle; +} + +AprilTagDetector::QuadThresholdParameters +AprilTagDetector::GetQuadThresholdParameters() const { + auto& qtp = static_cast(m_impl)->qtp; + return { + .minClusterPixels = qtp.min_cluster_pixels, + .maxNumMaxima = qtp.max_nmaxima, + .criticalAngle = m_qtpCriticalAngle, + .maxLineFitMSE = qtp.max_line_fit_mse, + .minWhiteBlackDiff = qtp.min_white_black_diff, + .deglitch = qtp.deglitch != 0, + }; +} + +bool AprilTagDetector::AddFamily(std::string_view fam, int bitsCorrected) { + auto& data = m_families[fam]; + if (data) { + return true; // already detecting + } + // create the family + if (fam == "tag16h5") { + data = tag16h5_create(); + } else if (fam == "tag25h9") { + data = tag25h9_create(); + } else if (fam == "tag36h11") { + data = tag36h11_create(); + } else if (fam == "tagCircle21h7") { + data = tagCircle21h7_create(); + } else if (fam == "tagCircle49h12") { + data = tagCircle49h12_create(); + } else if (fam == "tagStandard41h12") { + data = tagStandard41h12_create(); + } else if (fam == "tagStandard52h13") { + data = tagStandard52h13_create(); + } else if (fam == "tagCustom48h12") { + data = tagCustom48h12_create(); + } + if (!data) { + m_families.erase(fam); // don't keep null value + return false; // can't add + } + apriltag_detector_add_family_bits(static_cast(m_impl), + static_cast(data), + bitsCorrected); + return true; +} + +void AprilTagDetector::RemoveFamily(std::string_view fam) { + auto it = m_families.find(fam); + if (it != m_families.end()) { + apriltag_detector_remove_family( + static_cast(m_impl), + static_cast(it->second)); + DestroyFamily(it->getKey(), it->second); + m_families.erase(it); + } +} + +void AprilTagDetector::ClearFamilies() { + apriltag_detector_clear_families(static_cast(m_impl)); + DestroyFamilies(); + m_families.clear(); +} + +AprilTagDetector::Results AprilTagDetector::Detect(int width, int height, + int stride, uint8_t* buf) { + image_u8_t img{width, height, stride, buf}; + return { + apriltag_detector_detect(static_cast(m_impl), &img), + Results::private_init{}}; +} + +void AprilTagDetector::Destroy() { + if (m_impl) { + apriltag_detector_destroy(static_cast(m_impl)); + } + DestroyFamilies(); +} + +void AprilTagDetector::DestroyFamilies() { + for (auto&& entry : m_families) { + DestroyFamily(entry.getKey(), entry.second); + } +} + +void AprilTagDetector::DestroyFamily(std::string_view name, void* data) { + auto fam = static_cast(data); + if (name == "tag16h5") { + tag16h5_destroy(fam); + } else if (name == "tag25h9") { + tag25h9_destroy(fam); + } else if (name == "tag36h11") { + tag36h11_destroy(fam); + } else if (name == "tagCircle21h7") { + tagCircle21h7_destroy(fam); + } else if (name == "tagCircle49h12") { + tagCircle49h12_destroy(fam); + } else if (name == "tagStandard41h12") { + tagStandard41h12_destroy(fam); + } else if (name == "tagStandard52h13") { + tagStandard52h13_destroy(fam); + } else if (name == "tagCustom48h12") { + tagCustom48h12_destroy(fam); + } +} diff --git a/apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp b/apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp new file mode 100644 index 0000000000..f755fd539c --- /dev/null +++ b/apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagPoseEstimate.h" + +#include + +using namespace frc; + +double AprilTagPoseEstimate::GetAmbiguity() const { + auto min = (std::min)(error1, error2); + auto max = (std::max)(error1, error2); + + if (max > 0) { + return min / max; + } else { + return -1; + } +} diff --git a/apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp b/apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp new file mode 100644 index 0000000000..dc4acdecc0 --- /dev/null +++ b/apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp @@ -0,0 +1,154 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagPoseEstimator.h" + +#include + +#include "frc/apriltag/AprilTagDetection.h" + +#ifdef _WIN32 +#pragma warning(disable : 4200) +#elif defined(__clang__) +#pragma clang diagnostic ignored "-Wc99-extensions" +#elif defined(__GNUC__) +#pragma GCC diagnostic ignored "-Wpedantic" +#endif + +#include "apriltag.h" +#include "apriltag_pose.h" + +using namespace frc; + +static Eigen::Matrix3d OrthogonalizeRotationMatrix( + const Eigen::Matrix3d& input) { + Eigen::HouseholderQR qr{input}; + + Eigen::Matrix3d Q = qr.householderQ(); + Eigen::Matrix3d R = qr.matrixQR().triangularView(); + + // Fix signs in R if they're < 0 so it's close to an identity matrix + // (our QR decomposition implementation sometimes flips the signs of + // columns) + for (int colR = 0; colR < 3; ++colR) { + if (R(colR, colR) < 0) { + for (int rowQ = 0; rowQ < 3; ++rowQ) { + Q(rowQ, colR) = -Q(rowQ, colR); + } + } + } + + return Q; +} + +static Transform3d MakePose(const apriltag_pose_t& pose) { + if (!pose.R || !pose.t) { + return {}; + } + return {Translation3d{units::meter_t{pose.t->data[0]}, + units::meter_t{pose.t->data[1]}, + units::meter_t{pose.t->data[2]}}, + Rotation3d{OrthogonalizeRotationMatrix( + Eigen::Map>{ + pose.R->data})}}; +} + +static apriltag_detection_info_t MakeDetectionInfo( + const apriltag_detection_t* det, + const AprilTagPoseEstimator::Config& config) { + return {const_cast(det), + config.tagSize.value(), + config.fx, + config.fy, + config.cx, + config.cy}; +} + +static apriltag_detection_t MakeBasicDet( + std::span homography, + const std::span* corners) { + apriltag_detection_t detection; + detection.H = matd_create(3, 3); + std::memcpy(detection.H->data, homography.data(), 9 * sizeof(double)); + if (corners) { + for (int i = 0; i < 4; i++) { + detection.p[i][0] = (*corners)[i * 2]; + detection.p[i][1] = (*corners)[i * 2 + 1]; + } + } + return detection; +} + +static Transform3d DoEstimateHomography( + const apriltag_detection_t* detection, + const AprilTagPoseEstimator::Config& config) { + auto info = MakeDetectionInfo(detection, config); + apriltag_pose_t pose; + estimate_pose_for_tag_homography(&info, &pose); + return MakePose(pose); +} + +Transform3d AprilTagPoseEstimator::EstimateHomography( + const AprilTagDetection& detection) const { + return DoEstimateHomography( + reinterpret_cast(&detection), m_config); +} + +Transform3d AprilTagPoseEstimator::EstimateHomography( + std::span homography) const { + auto detection = MakeBasicDet(homography, nullptr); + auto rv = DoEstimateHomography(&detection, m_config); + matd_destroy(detection.H); + return rv; +} + +static AprilTagPoseEstimate DoEstimateOrthogonalIteration( + const apriltag_detection_t* detection, + const AprilTagPoseEstimator::Config& config, int nIters) { + auto info = MakeDetectionInfo(detection, config); + apriltag_pose_t pose1, pose2; + double err1, err2; + estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, &pose2, + nIters); + return {MakePose(pose1), MakePose(pose2), err1, err2}; +} + +AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration( + const AprilTagDetection& detection, int nIters) const { + return DoEstimateOrthogonalIteration( + reinterpret_cast(&detection), m_config, + nIters); +} + +AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration( + std::span homography, std::span corners, + int nIters) const { + auto detection = MakeBasicDet(homography, &corners); + auto rv = DoEstimateOrthogonalIteration(&detection, m_config, nIters); + matd_destroy(detection.H); + return rv; +} + +static Transform3d DoEstimate(const apriltag_detection_t* detection, + const AprilTagPoseEstimator::Config& config) { + auto info = MakeDetectionInfo(detection, config); + apriltag_pose_t pose; + estimate_tag_pose(&info, &pose); + return MakePose(pose); +} + +Transform3d AprilTagPoseEstimator::Estimate( + const AprilTagDetection& detection) const { + return DoEstimate(reinterpret_cast(&detection), + m_config); +} + +Transform3d AprilTagPoseEstimator::Estimate( + std::span homography, + std::span corners) const { + auto detection = MakeBasicDet(homography, &corners); + auto rv = DoEstimate(&detection, m_config); + matd_destroy(detection.H); + return rv; +} diff --git a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp index 6c5df5ec66..31fd4f979f 100644 --- a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp +++ b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp @@ -2,302 +2,594 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include +#include + #include #include "edu_wpi_first_apriltag_jni_AprilTagJNI.h" +#include "frc/apriltag/AprilTagDetector.h" +#include "frc/apriltag/AprilTagPoseEstimator.h" -#ifdef _WIN32 -#pragma warning(push) -#pragma warning(disable : 4200) -#endif - -#if defined(__GNUC__) -#pragma GCC diagnostic ignored "-Wpedantic" -#endif -#include "apriltag.h" -#ifdef _WIN32 -#pragma warning(pop) -#endif - -#include "tag36h11.h" -#include "tag25h9.h" -#include "tag16h5.h" -#include "tagCircle21h7.h" -#include "tagCircle49h12.h" -#include "tagCustom48h12.h" -#include "tagStandard41h12.h" -#include "tagStandard52h13.h" -#include "apriltag_pose.h" - -#include -#include -#include - -#include - +using namespace frc; using namespace wpi::java; -namespace { -template -using LambdaUniquePtr = std::unique_ptr; +static JavaVM* jvm = nullptr; -struct DetectorState { - LambdaUniquePtr tf; - LambdaUniquePtr td; -}; -} // namespace +static JClass detectionCls; +static JClass detectorConfigCls; +static JClass detectorQTPCls; +static JClass poseEstimateCls; +static JClass quaternionCls; +static JClass rotation3dCls; +static JClass transform3dCls; +static JClass translation3dCls; +static JException illegalArgEx; +static JException nullPointerEx; -static wpi::mutex detectorMutex; -static wpi::DenseMap detectors; -static jint detectorCount; +static const JClassInit classes[] = { + {"edu/wpi/first/apriltag/AprilTagDetection", &detectionCls}, + {"edu/wpi/first/apriltag/AprilTagDetector$Config", &detectorConfigCls}, + {"edu/wpi/first/apriltag/AprilTagDetector$QuadThresholdParameters", + &detectorQTPCls}, + {"edu/wpi/first/apriltag/AprilTagPoseEstimate", &poseEstimateCls}, + {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls}, + {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls}, + {"edu/wpi/first/math/geometry/Transform3d", &transform3dCls}, + {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}}; + +static const JExceptionInit exceptions[] = { + {"java/lang/IllegalArgumentException", &illegalArgEx}, + {"java/lang/NullPointerException", &nullPointerEx}}; extern "C" { -/* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI - * Method: aprilTagCreate - * Signature: (Ljava/lang/String;DDIZZ)I - */ -JNIEXPORT jint JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagCreate - (JNIEnv* env, jclass cls, jstring jstr, jdouble decimate, jdouble blur, - jint threads, jboolean debug, jboolean refine_edges) -{ - // Initialize tag detector with options - LambdaUniquePtr tf{nullptr, [](apriltag_family_t*) {}}; - - JStringRef famname(env, jstr); - - using namespace std::string_view_literals; - if (famname.str().compare("tag36h11"sv) == 0) { - tf = {tag36h11_create(), tag36h11_destroy}; - } else if (famname.str().compare("tag25h9"sv) == 0) { - tf = {tag25h9_create(), tag25h9_destroy}; - } else if (famname.str().compare("tag16h5"sv) == 0) { - tf = {tag16h5_create(), tag16h5_destroy}; - } else if (famname.str().compare("tagCircle21h7"sv) == 0) { - tf = {tagCircle21h7_create(), tagCircle21h7_destroy}; - } else if (famname.str().compare("tagCircle49h12"sv) == 0) { - tf = {tagCircle49h12_create(), tagCircle49h12_destroy}; - } else if (famname.str().compare("tagStandard41h12"sv) == 0) { - tf = {tagStandard41h12_create(), tagStandard41h12_destroy}; - } else if (famname.str().compare("tagStandard52h13"sv) == 0) { - tf = {tagStandard52h13_create(), tagStandard52h13_destroy}; - } else if (famname.str().compare("tagCustom48h12"sv) == 0) { - tf = {tagCustom48h12_create(), tagCustom48h12_destroy}; - } else { - std::printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n"); - return -1; - } - - if (tf == nullptr) { - std::printf("Failed to allocate tag\n"); - return -1; - } - - LambdaUniquePtr td{apriltag_detector_create(), - apriltag_detector_destroy}; - if (td == nullptr) { - std::printf("Failed to allocate detector\n"); - return -1; - } - - apriltag_detector_add_family(td.get(), tf.get()); - td->quad_decimate = static_cast(decimate); - td->quad_sigma = static_cast(blur); - td->nthreads = threads; - td->debug = debug; - td->refine_edges = refine_edges; - - std::scoped_lock lock{detectorMutex}; - jint idx = detectorCount++; - detectors.insert({idx, DetectorState{std::move(tf), std::move(td)}}); - return idx; -} - -static JClass detectionClass; - JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { + jvm = vm; + JNIEnv* env; if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { return JNI_ERR; } - detectionClass = JClass(env, "edu/wpi/first/apriltag/jni/DetectionResult"); + // Cache references to classes + for (auto& c : classes) { + *c.cls = JClass(env, c.name); + if (!*c.cls) { + std::fprintf(stderr, "could not load class %s\n", c.name); + return JNI_ERR; + } + } - if (!detectionClass) { - std::printf("Couldn't find class!"); - return JNI_ERR; + for (auto& c : exceptions) { + *c.cls = JException(env, c.name); + if (!*c.cls) { + std::fprintf(stderr, "could not load exception %s\n", c.name); + return JNI_ERR; + } } return JNI_VERSION_1_6; } -static jobject MakeJObject(JNIEnv* env, const apriltag_detection_t* detect, - apriltag_pose_t& pose1, apriltag_pose_t& pose2, - double error1, double error2) { - // Constructor signature must match Java! I = int, F = float, [D = double - // array - static jmethodID constructor = - env->GetMethodID(detectionClass, "", "(IIF[DDD[D[D[DD[D[DD)V"); +JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { + JNIEnv* env; + if (vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6) != JNI_OK) { + return; + } + // Delete global references + for (auto& c : classes) { + c.cls->free(env); + } + for (auto& c : exceptions) { + c.cls->free(env); + } + jvm = nullptr; +} +} // extern "C" + +// +// Conversions from Java to C++ objects +// + +static AprilTagDetector::Config FromJavaDetectorConfig(JNIEnv* env, + jobject jconfig) { + if (!jconfig) { + return {}; + } +#define FIELD(name, sig) \ + static jfieldID name##Field = nullptr; \ + if (!name##Field) { \ + name##Field = env->GetFieldID(detectorConfigCls, #name, sig); \ + } + + FIELD(numThreads, "I"); + FIELD(quadDecimate, "F"); + FIELD(quadSigma, "F"); + FIELD(refineEdges, "Z"); + FIELD(decodeSharpening, "D"); + FIELD(debug, "Z"); + +#undef FIELD + +#define FIELD(ctype, jtype, name) \ + .name = static_cast(env->Get##jtype##Field(jconfig, name##Field)) + + return { + FIELD(int, Int, numThreads), + FIELD(float, Float, quadDecimate), + FIELD(float, Float, quadSigma), + FIELD(bool, Boolean, refineEdges), + FIELD(double, Double, decodeSharpening), + FIELD(bool, Boolean, debug), + }; + +#undef GET +#undef FIELD +} + +static AprilTagDetector::QuadThresholdParameters FromJavaDetectorQTP( + JNIEnv* env, jobject jparams) { + if (!jparams) { + return {}; + } +#define FIELD(name, sig) \ + static jfieldID name##Field = nullptr; \ + if (!name##Field) { \ + name##Field = env->GetFieldID(detectorQTPCls, #name, sig); \ + } + + FIELD(minClusterPixels, "I"); + FIELD(maxNumMaxima, "I"); + FIELD(criticalAngle, "D"); + FIELD(maxLineFitMSE, "F"); + FIELD(minWhiteBlackDiff, "I"); + FIELD(deglitch, "Z"); + +#undef FIELD + +#define FIELD(ctype, jtype, name) \ + .name = static_cast(env->Get##jtype##Field(jparams, name##Field)) + + return { + FIELD(int, Int, minClusterPixels), + FIELD(int, Int, maxNumMaxima), + .criticalAngle = units::radian_t{static_cast( + env->GetDoubleField(jparams, criticalAngleField))}, + FIELD(float, Float, maxLineFitMSE), + FIELD(int, Int, minWhiteBlackDiff), + FIELD(bool, Boolean, deglitch), + }; + +#undef GET +#undef FIELD +} + +// +// Conversions from C++ to Java objects +// + +static jobject MakeJObject(JNIEnv* env, const AprilTagDetection& detect) { + static jmethodID constructor = env->GetMethodID( + detectionCls, "", "(Ljava/lang/String;IIF[DDD[D)V"); if (!constructor) { return nullptr; } - if (!detect) { - return nullptr; - } + JLocal fam{env, MakeJString(env, detect.GetFamily())}; - // We have to copy the homography matrix and coners into jdoubles - jdouble h[9]; // = new jdouble[9]{}; - for (int i = 0; i < 9; i++) { - h[i] = detect->H->data[i]; - } + auto homography = detect.GetHomography(); + JLocal harr{ + env, MakeJDoubleArray( + env, {reinterpret_cast(homography.data()), + homography.size()})}; - jdouble corners[8]; // = new jdouble[8]{}; - for (int i = 0; i < 4; i++) { - corners[i * 2] = detect->p[i][0]; - corners[i * 2 + 1] = detect->p[i][1]; - } + double cornersBuf[8]; + auto corners = detect.GetCorners(cornersBuf); + JLocal carr{ + env, + MakeJDoubleArray(env, {reinterpret_cast(corners.data()), + corners.size()})}; - jdoubleArray harr = MakeJDoubleArray(env, {h, 9}); - jdoubleArray carr = MakeJDoubleArray(env, {corners, 8}); + auto center = detect.GetCenter(); - // The rotation of the target is encoded as a 3 by 3 rotation matrix, we'll - // convert to a row-major array - jdouble pose1RotMat[9] = {0}; - jdouble pose2RotMat[9] = {0}; - - for (int i = 0; i < 9; i++) { - if (pose1.R) { - pose1RotMat[i] = pose1.R->data[i]; - } - if (pose2.R) { - pose2RotMat[i] = pose2.R->data[i]; - } - } - - // And translation a 3x1 vector (todo check axis order) - jdouble pose1Trans[3] = {0}; - jdouble pose2Trans[3] = {0}; - for (int i = 0; i < 3; i++) { - if (pose1.t) { - pose1Trans[i] = pose1.t->data[i]; - } - if (pose2.t) { - pose2Trans[i] = pose2.t->data[i]; - } - } - - jdoubleArray pose1rotArr = MakeJDoubleArray(env, {pose1RotMat, 9}); - jdoubleArray pose2rotArr = MakeJDoubleArray(env, {pose2RotMat, 9}); - jdoubleArray pose1transArr = MakeJDoubleArray(env, {pose1Trans, 3}); - jdoubleArray pose2transArr = MakeJDoubleArray(env, {pose2Trans, 3}); - jdouble err1 = error1; - jdouble err2 = error2; - - // Actually call the constructor - auto ret = env->NewObject( - detectionClass, constructor, (jint)detect->id, (jint)detect->hamming, - (jfloat)detect->decision_margin, harr, (jdouble)detect->c[0], - (jdouble)detect->c[1], carr, pose1transArr, pose1rotArr, err1, - pose2transArr, pose2rotArr, err2); - - return ret; + return env->NewObject(detectionCls, constructor, fam.obj(), + static_cast(detect.GetId()), + static_cast(detect.GetHamming()), + static_cast(detect.GetDecisionMargin()), + harr.obj(), static_cast(center.x), + static_cast(center.y), carr.obj()); } -/* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI - * Method: aprilTagDetectInternal - * Signature: (IJIIZDDDDDI)[Ljava/lang/Object; - */ -JNIEXPORT jobjectArray JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDetectInternal - (JNIEnv* env, jclass cls, jint detectIdx, jlong pData, jint rows, jint cols, - jboolean doPoseEstimation, jdouble tagWidthMeters, jdouble fx, jdouble fy, - jdouble cx, jdouble cy, jint nIters) -{ - // No image, can't do anything - if (!pData) { - return nullptr; - } - - // Make an image_u8_t header for the Mat data - image_u8_t im = {static_cast(cols), static_cast(rows), - static_cast(cols), - reinterpret_cast(pData)}; - - // Get our detector - DetectorState* state; - { - std::scoped_lock lock{detectorMutex}; - auto detectorIterator = detectors.find((jint)detectIdx); - if (detectorIterator == detectors.end()) { - return nullptr; - } - - state = &detectorIterator->second; - } - - // And run the detector on our new image - zarray_t* detections = apriltag_detector_detect(state->td.get(), &im); - - int size = zarray_size(detections); - - // Object array to return to Java - jobjectArray jarr = env->NewObjectArray(size, detectionClass, nullptr); +static jobjectArray MakeJObject(JNIEnv* env, + std::span arr) { + jobjectArray jarr = env->NewObjectArray(arr.size(), detectionCls, nullptr); if (!jarr) { - std::printf("Couldn't make array\n"); return nullptr; } - - // Global pose - apriltag_pose_t pose1; - std::memset(&pose1, 0, sizeof(pose1)); - - apriltag_pose_t pose2; - std::memset(&pose2, 0, sizeof(pose2)); - - // std::printf("Created array %llu! Got %i targets!\n", &jarr, size); - // Add our detected targets to the array - for (int i = 0; i < size; ++i) { - apriltag_detection_t* det = nullptr; - zarray_get(detections, i, &det); - - if (det != nullptr) { - double err1 = - HUGE_VAL; // Should get overwritten if pose estimation is happening - double err2 = HUGE_VAL; - if (doPoseEstimation) { - // Feed results to the pose estimator - apriltag_detection_info_t info{det, tagWidthMeters, fx, fy, cx, cy}; - estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, - &pose2, nIters); - } - - jobject obj = MakeJObject(env, det, pose1, pose2, err1, err2); - - env->SetObjectArrayElement(jarr, i, obj); - } + for (size_t i = 0; i < arr.size(); ++i) { + JLocal elem{env, MakeJObject(env, *arr[i])}; + env->SetObjectArrayElement(jarr, i, elem.obj()); } - - // Now that stuff's in our Java-side array, we can clean up native memory - apriltag_detections_destroy(detections); - return jarr; } +static jobject MakeJObject(JNIEnv* env, + const AprilTagDetector::Config& config) { + static jmethodID constructor = + env->GetMethodID(detectorConfigCls, "", "(IFFZDZ)V"); + if (!constructor) { + return nullptr; + } + + return env->NewObject(detectorConfigCls, constructor, + static_cast(config.numThreads), + static_cast(config.quadDecimate), + static_cast(config.quadSigma), + static_cast(config.refineEdges), + static_cast(config.decodeSharpening), + static_cast(config.debug)); +} + +static jobject MakeJObject( + JNIEnv* env, const AprilTagDetector::QuadThresholdParameters& params) { + static jmethodID constructor = + env->GetMethodID(detectorQTPCls, "", "(IIDFIZ)V"); + if (!constructor) { + return nullptr; + } + + return env->NewObject(detectorQTPCls, constructor, + static_cast(params.minClusterPixels), + static_cast(params.maxNumMaxima), + static_cast(params.criticalAngle), + static_cast(params.maxLineFitMSE), + static_cast(params.minWhiteBlackDiff), + static_cast(params.deglitch)); +} + +static jobject MakeJObject(JNIEnv* env, const Translation3d& xlate) { + static jmethodID constructor = + env->GetMethodID(translation3dCls, "", "(DDD)V"); + if (!constructor) { + return nullptr; + } + + return env->NewObject( + translation3dCls, constructor, static_cast(xlate.X()), + static_cast(xlate.Y()), static_cast(xlate.Z())); +} + +static jobject MakeJObject(JNIEnv* env, const Quaternion& q) { + static jmethodID constructor = + env->GetMethodID(quaternionCls, "", "(DDDD)V"); + if (!constructor) { + return nullptr; + } + + return env->NewObject(quaternionCls, constructor, static_cast(q.W()), + static_cast(q.X()), + static_cast(q.Y()), + static_cast(q.Z())); +} + +static jobject MakeJObject(JNIEnv* env, const Rotation3d& rot) { + static jmethodID constructor = env->GetMethodID( + rotation3dCls, "", "(Ledu/wpi/first/math/geometry/Quaternion;)V"); + if (!constructor) { + return nullptr; + } + + JLocal q{env, MakeJObject(env, rot.GetQuaternion())}; + return env->NewObject(rotation3dCls, constructor, q.obj()); +} + +static jobject MakeJObject(JNIEnv* env, const Transform3d& xform) { + static jmethodID constructor = + env->GetMethodID(transform3dCls, "", + "(Ledu/wpi/first/math/geometry/Translation3d;" + "Ledu/wpi/first/math/geometry/Rotation3d;)V"); + if (!constructor) { + return nullptr; + } + + JLocal xlate{env, MakeJObject(env, xform.Translation())}; + JLocal rot{env, MakeJObject(env, xform.Rotation())}; + return env->NewObject(transform3dCls, constructor, xlate.obj(), rot.obj()); +} + +static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) { + static jmethodID constructor = + env->GetMethodID(poseEstimateCls, "", + "(Ledu/wpi/first/math/geometry/Transform3d;" + "Ledu/wpi/first/math/geometry/Transform3d;DD)V"); + if (!constructor) { + return nullptr; + } + + JLocal pose1{env, MakeJObject(env, est.pose1)}; + JLocal pose2{env, MakeJObject(env, est.pose2)}; + return env->NewObject(poseEstimateCls, constructor, pose1.obj(), pose2.obj(), + static_cast(est.error1), + static_cast(est.error2)); +} + +extern "C" { + /* * Class: edu_wpi_first_apriltag_jni_AprilTagJNI - * Method: aprilTagDestroy - * Signature: (I)V + * Method: createDetector + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_createDetector + (JNIEnv* env, jclass) +{ + return reinterpret_cast(new AprilTagDetector); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: destroyDetector + * Signature: (J)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDestroy - (JNIEnv* env, jclass clazz, jint detectIdx) +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_destroyDetector + (JNIEnv* env, jclass, jlong det) { - std::scoped_lock lock{detectorMutex}; - detectors.erase(detectIdx); + delete reinterpret_cast(det); } + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: setDetectorConfig + * Signature: (JLjava/lang/Object;)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorConfig + (JNIEnv* env, jclass, jlong det, jobject config) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return; + } + reinterpret_cast(det)->SetConfig( + FromJavaDetectorConfig(env, config)); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: getDetectorConfig + * Signature: (J)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorConfig + (JNIEnv* env, jclass, jlong det) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return nullptr; + } + return MakeJObject(env, + reinterpret_cast(det)->GetConfig()); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: setDetectorQTP + * Signature: (JLjava/lang/Object;)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorQTP + (JNIEnv* env, jclass, jlong det, jobject params) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return; + } + reinterpret_cast(det)->SetQuadThresholdParameters( + FromJavaDetectorQTP(env, params)); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: getDetectorQTP + * Signature: (J)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorQTP + (JNIEnv* env, jclass, jlong det) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return nullptr; + } + return MakeJObject( + env, + reinterpret_cast(det)->GetQuadThresholdParameters()); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: addFamily + * Signature: (JLjava/lang/String;I)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_addFamily + (JNIEnv* env, jclass, jlong det, jstring fam, jint bitsCorrected) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return false; + } + if (!fam) { + nullPointerEx.Throw(env, "fam cannot be null"); + return false; + } + return reinterpret_cast(det)->AddFamily( + JStringRef{env, fam}, bitsCorrected); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: removeFamily + * Signature: (JLjava/lang/String;)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_removeFamily + (JNIEnv* env, jclass, jlong det, jstring fam) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return; + } + if (!fam) { + nullPointerEx.Throw(env, "fam cannot be null"); + return; + } + reinterpret_cast(det)->RemoveFamily(JStringRef{env, fam}); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: clearFamilies + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_clearFamilies + (JNIEnv* env, jclass, jlong det) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return; + } + reinterpret_cast(det)->ClearFamilies(); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: detect + * Signature: (JIIIJ)[Ljava/lang/Object; + */ +JNIEXPORT jobjectArray JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_detect + (JNIEnv* env, jclass, jlong det, jint width, jint height, jint stride, + jlong bufAddr) +{ + if (det == 0) { + nullPointerEx.Throw(env, "det cannot be null"); + return nullptr; + } + if (bufAddr == 0) { + nullPointerEx.Throw(env, "bufAddr cannot be null"); + return nullptr; + } + return MakeJObject( + env, reinterpret_cast(det)->Detect( + width, height, stride, reinterpret_cast(bufAddr))); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: estimatePoseHomography + * Signature: ([DDDDDD)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseHomography + (JNIEnv* env, jclass, jdoubleArray homography, jdouble tagSize, jdouble fx, + jdouble fy, jdouble cx, jdouble cy) +{ + if (!homography) { + nullPointerEx.Throw(env, "homography cannot be null"); + return nullptr; + } + JDoubleArrayRef harr{env, homography}; + if (harr.size() != 9) { + illegalArgEx.Throw(env, "homography array must be size 9"); + return nullptr; + } + + AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy}); + return MakeJObject(env, estimator.EstimateHomography( + std::span{harr.array()})); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: estimatePoseOrthogonalIteration + * Signature: ([D[DDDDDDI)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration + (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners, + jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy, jint nIters) +{ + // homography + if (!homography) { + nullPointerEx.Throw(env, "homography cannot be null"); + return nullptr; + } + JDoubleArrayRef harr{env, homography}; + if (harr.size() != 9) { + illegalArgEx.Throw(env, "homography array must be size 9"); + return nullptr; + } + + // corners + if (!corners) { + nullPointerEx.Throw(env, "corners cannot be null"); + return nullptr; + } + JDoubleArrayRef carr{env, corners}; + if (carr.size() != 8) { + illegalArgEx.Throw(env, "corners array must be size 8"); + return nullptr; + } + + AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy}); + return MakeJObject(env, + estimator.EstimateOrthogonalIteration( + std::span{harr.array()}, + std::span{carr.array()}, nIters)); +} + +/* + * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Method: estimatePose + * Signature: ([D[DDDDDD)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose + (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners, + jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy) +{ + // homography + if (!homography) { + nullPointerEx.Throw(env, "homography cannot be null"); + return nullptr; + } + JDoubleArrayRef harr{env, homography}; + if (harr.size() != 9) { + illegalArgEx.Throw(env, "homography array must be size 9"); + return nullptr; + } + + // corners + if (!corners) { + nullPointerEx.Throw(env, "corners cannot be null"); + return nullptr; + } + JDoubleArrayRef carr{env, corners}; + if (carr.size() != 8) { + illegalArgEx.Throw(env, "corners array must be size 8"); + return nullptr; + } + + AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy}); + return MakeJObject( + env, estimator.Estimate(std::span{harr.array()}, + std::span{carr.array()})); +} + } // extern "C" diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h new file mode 100644 index 0000000000..5225f2176b --- /dev/null +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h @@ -0,0 +1,160 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include + +#include "frc/EigenCore.h" + +namespace frc { + +/** + * A detection of an AprilTag tag. + */ +class WPILIB_DLLEXPORT AprilTagDetection final { + public: + AprilTagDetection() = delete; + AprilTagDetection(const AprilTagDetection&) = delete; + AprilTagDetection& operator=(const AprilTagDetection&) = delete; + + /** A point. Used for center and corner points. */ + struct Point { + double x; + double y; + }; + + /** + * Gets the decoded tag's family name. + * + * @return Decoded family name + */ + std::string_view GetFamily() const; + + /** + * Gets the decoded ID of the tag. + * + * @return Decoded ID + */ + int GetId() const { return id; } + + /** + * Gets how many error bits were corrected. Note: accepting large numbers of + * corrected errors leads to greatly increased false positive rates. + * NOTE: As of this implementation, the detector cannot detect tags with + * a hamming distance greater than 2. + * + * @return Hamming distance (number of corrected error bits) + */ + int GetHamming() const { return hamming; } + + /** + * Gets a measure of the quality of the binary decoding process: the + * average difference between the intensity of a data bit versus + * the decision threshold. Higher numbers roughly indicate better + * decodes. This is a reasonable measure of detection accuracy + * only for very small tags-- not effective for larger tags (where + * we could have sampled anywhere within a bit cell and still + * gotten a good detection.) + * + * @return Decision margin + */ + float GetDecisionMargin() const { return decision_margin; } + + /** + * Gets the 3x3 homography matrix describing the projection from an + * "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1, + * -1)) to pixels in the image. + * + * @return Homography matrix data + */ + std::span GetHomography() const; + + /** + * Gets the 3x3 homography matrix describing the projection from an + * "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1, + * -1)) to pixels in the image. + * + * @return Homography matrix + */ + Eigen::Matrix3d GetHomographyMatrix() const; + + /** + * Gets the center of the detection in image pixel coordinates. + * + * @return Center point + */ + const Point& GetCenter() const { return *reinterpret_cast(c); } + + /** + * Gets a corner of the tag in image pixel coordinates. These always + * wrap counter-clock wise around the tag. + * + * @param ndx Corner index (range is 0-3, inclusive) + * @return Corner point + */ + const Point& GetCorner(int ndx) const { + return *reinterpret_cast(p[ndx]); + } + + /** + * Gets the corners of the tag in image pixel coordinates. These always + * wrap counter-clock wise around the tag. + * + * @param cornersBuf Corner point array (X and Y for each corner in order) + * @return Corner point array (copy of cornersBuf span) + */ + std::span GetCorners(std::span cornersBuf) const { + for (int i = 0; i < 4; i++) { + cornersBuf[i * 2] = p[i][0]; + cornersBuf[i * 2 + 1] = p[i][1]; + } + return cornersBuf; + } + + private: + // This class *must* be standard-layout-compatible with apriltag_detection + // as we use reinterpret_cast from that structure. This means the below + // members must exactly match the contents of the apriltag_detection struct. + + // The tag family. + void* family; + + // The decoded ID of the tag. + int id; + + // How many error bits were corrected? Note: accepting large numbers of + // corrected errors leads to greatly increased false positive rates. + // NOTE: As of this implementation, the detector cannot detect tags with + // a hamming distance greater than 2. + int hamming; + + // A measure of the quality of the binary decoding process: the + // average difference between the intensity of a data bit versus + // the decision threshold. Higher numbers roughly indicate better + // decodes. This is a reasonable measure of detection accuracy + // only for very small tags-- not effective for larger tags (where + // we could have sampled anywhere within a bit cell and still + // gotten a good detection.) + float decision_margin; + + // The 3x3 homography matrix describing the projection from an + // "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1, + // -1)) to pixels in the image. + void* H; + + // The center of the detection in image pixel coordinates. + double c[2]; + + // The corners of the tag in image pixel coordinates. These always + // wrap counter-clock wise around the tag. + double p[4][2]; +}; + +} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h new file mode 100644 index 0000000000..8a78d1b358 --- /dev/null +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h @@ -0,0 +1,260 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "frc/apriltag/AprilTagDetection.h" + +namespace frc { + +/** + * An AprilTag detector engine. This is expensive to set up and tear down, so + * most use cases should only create one of these, add a family to it, set up + * any other configuration, and repeatedly call Detect(). + */ +class WPILIB_DLLEXPORT AprilTagDetector { + public: + /** Detector configuration. */ + struct Config { + bool operator==(const Config&) const = default; + + /** + * How many threads should be used for computation. Default is + * single-threaded operation (1 thread). + */ + int numThreads = 1; + + /** + * Quad decimation. 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 2.0. + */ + float quadDecimate = 2.0f; + + /** + * What Gaussian blur should be applied to the segmented image (used for + * quad detection). Very noisy images benefit from non-zero values (e.g. + * 0.8). Default is 0.0. + */ + float quadSigma = 0.0f; + + /** + * When true, 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 (true). Default is true. + * + * Very computationally inexpensive. Option is ignored if + * quad_decimate = 1. + */ + bool refineEdges = true; + + /** + * How much sharpening should be done to decoded images. This can help + * decode small tags but may or may not help in odd lighting conditions or + * low light conditions. Default is 0.25. + */ + double decodeSharpening = 0.25; + + /** + * Debug mode. When true, the decoder writes a variety of debugging images + * to the current working directory at various stages through the detection + * process. This is slow and should *not* be used on space-limited systems + * such as the RoboRIO. Default is disabled (false). + */ + bool debug = false; + }; + + /** Quad threshold parameters. */ + struct QuadThresholdParameters { + bool operator==(const QuadThresholdParameters&) const = default; + + /** + * Threshold used to reject quads containing too few pixels. Default is 5 + * pixels. + */ + int minClusterPixels = 5; + + /** + * How many corner candidates to consider when segmenting a group of pixels + * into a quad. Default is 10. + */ + int maxNumMaxima = 10; + + /** + * Critical angle. The detector will reject quads where pairs of edges have + * angles that are close to straight or close to 180 degrees. Zero means + * that no quads are rejected. Default is 10 degrees. + */ + units::radian_t criticalAngle = 10_deg; + + /** + * When fitting lines to the contours, the maximum mean squared error + * allowed. This is useful in rejecting contours that are far from being + * quad shaped; rejecting these quads "early" saves expensive decoding + * processing. Default is 10.0. + */ + float maxLineFitMSE = 10.0f; + + /** + * Minimum brightness offset. When we build our model of black & white + * pixels, we add an extra check that the white model must be (overall) + * brighter than the black model. How much brighter? (in pixel values, + * [0,255]). Default is 5. + */ + int minWhiteBlackDiff = 5; + + /** + * Whether the thresholded image be should be deglitched. Only useful for + * very noisy images. Default is disabled (false). + */ + bool deglitch = false; + }; + + /** + * Array of detection results. Each array element is a pointer to an + * AprilTagDetection. + */ + class WPILIB_DLLEXPORT Results + : public std::span { + struct private_init {}; + friend class AprilTagDetector; + + public: + Results() = default; + Results(void* impl, const private_init&); + ~Results() { Destroy(); } + Results(const Results&) = delete; + Results& operator=(const Results&) = delete; + Results(Results&& rhs) : span{std::move(rhs)}, m_impl{rhs.m_impl} { + rhs.m_impl = nullptr; + } + Results& operator=(Results&& rhs); + + private: + void Destroy(); + void* m_impl = nullptr; + }; + + AprilTagDetector(); + ~AprilTagDetector() { Destroy(); } + AprilTagDetector(const AprilTagDetector&) = delete; + AprilTagDetector& operator=(const AprilTagDetector&) = delete; + AprilTagDetector(AprilTagDetector&& rhs) + : m_impl{rhs.m_impl}, + m_families{std::move(rhs.m_families)}, + m_qtpCriticalAngle{rhs.m_qtpCriticalAngle} { + rhs.m_impl = nullptr; + } + AprilTagDetector& operator=(AprilTagDetector&& rhs); + + /** + * @{ + * @name Configuration functions + */ + + /** + * Sets detector configuration. + * + * @param config Configuration + */ + void SetConfig(const Config& config); + + /** + * Gets detector configuration. + * + * @return Configuration + */ + Config GetConfig() const; + + /** + * Sets quad threshold parameters. + * + * @param params Parameters + */ + void SetQuadThresholdParameters(const QuadThresholdParameters& params); + + /** + * Gets quad threshold parameters. + * + * @return Parameters + */ + QuadThresholdParameters GetQuadThresholdParameters() const; + + /** @} */ + + /** + * @{ + * @name Tag family functions + */ + + /** + * Adds a family of tags to be detected. + * + * @param fam Family name, e.g. "tag16h5" + * @param bitsCorrected + * @return False if family can't be found + */ + bool AddFamily(std::string_view fam, int bitsCorrected = 2); + + /** + * Removes a family of tags from the detector. + * + * @param fam Family name, e.g. "tag16h5" + */ + void RemoveFamily(std::string_view fam); + + /** + * Unregister all families. + */ + void ClearFamilies(); + + /** @} */ + + /** + * Detect tags from an 8-bit image. + * + * @param width width of the image + * @param height height of the image + * @param stride number of bytes between image rows (often the same as width) + * @param buf image buffer + * @return Results (array of AprilTagDetection pointers) + */ + Results Detect(int width, int height, int stride, uint8_t* buf); + + /** + * Detect tags from an 8-bit image. + * + * @param width width of the image + * @param height height of the image + * @param buf image buffer + * @return Results (array of AprilTagDetection pointers) + */ + Results Detect(int width, int height, uint8_t* buf) { + return Detect(width, height, width, buf); + } + + private: + void Destroy(); + void DestroyFamilies(); + void DestroyFamily(std::string_view name, void* data); + + void* m_impl; + wpi::StringMap m_families; + units::radian_t m_qtpCriticalAngle = 10_deg; +}; + +} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h new file mode 100644 index 0000000000..7231430ed3 --- /dev/null +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h @@ -0,0 +1,18 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/apriltag/AprilTagDetector.h" + +namespace frc { + +inline AprilTagDetector::Results AprilTagDetect(AprilTagDetector& detector, + cv::Mat& image) { + return detector.Detect(image.cols, image.rows, image.data); +} + +} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h new file mode 100644 index 0000000000..28c7d51a6b --- /dev/null +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Transform3d.h" + +namespace frc { + +/** A pair of AprilTag pose estimates. */ +struct WPILIB_DLLEXPORT AprilTagPoseEstimate { + /** Pose 1. */ + Transform3d pose1; + + /** Pose 2. */ + Transform3d pose2; + + /** Object-space error of pose 1. */ + double error1; + + /** Object-space error of pose 2. */ + double error2; + + /** + * Gets the ratio of pose reprojection errors, called ambiguity. Numbers + * above 0.2 are likely to be ambiguous. + * + * @return The ratio of pose reprojection errors. + */ + double GetAmbiguity() const; +}; + +} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h new file mode 100644 index 0000000000..1e1e852427 --- /dev/null +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h @@ -0,0 +1,145 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include "frc/apriltag/AprilTagPoseEstimate.h" +#include "frc/geometry/Transform3d.h" + +namespace frc { + +class AprilTagDetection; + +/** Pose estimators for AprilTag tags. */ +class WPILIB_DLLEXPORT AprilTagPoseEstimator { + public: + /** Configuration for the pose estimator. */ + struct Config { + bool operator==(const Config&) const = default; + + /** The tag size. */ + units::meter_t tagSize; + + /** Camera horizontal focal length, in pixels. */ + double fx; + + /** Camera vertical focal length, in pixels. */ + double fy; + + /** Camera horizontal focal center, in pixels. */ + double cx; + + /** Camera vertical focal center, in pixels. */ + double cy; + }; + + /** + * Creates estimator. + * + * @param config Configuration + */ + explicit AprilTagPoseEstimator(const Config& config) : m_config{config} {} + + /** + * Sets estimator configuration. + * + * @param config Configuration + */ + void SetConfig(const Config& config) { m_config = config; } + + /** + * Gets estimator configuration. + * + * @return Configuration + */ + const Config& GetConfig() const { return m_config; } + + /** + * Estimates the pose of the tag using the homography method described in [1]. + * + * @param detection Tag detection + * @return Pose estimate + */ + Transform3d EstimateHomography(const AprilTagDetection& detection) const; + + /** + * Estimates the pose of the tag using the homography method described in [1]. + * + * @param homography Homography 3x3 matrix data + * @return Pose estimate + */ + Transform3d EstimateHomography(std::span homography) const; + + /** + * Estimates the pose of the tag. This returns one or two possible poses for + * the tag, along with the object-space error of each. + * + * This uses the homography method described in [1] for the initial estimate. + * Then Orthogonal Iteration [2] is used to refine this estimate. Then [3] is + * used to find a potential second local minima and Orthogonal Iteration is + * used to refine this second estimate. + * + * [1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in + * 2011 IEEE International Conference on Robotics and Automation, + * May 2011, pp. 3400–3407. + * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose + * estimation from video images," in IEEE Transactions on Pattern + * Analysis and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000. + * doi: 10.1109/34.862199 + * [3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar + * Target," in IEEE Transactions on Pattern Analysis and Machine Intelligence, + * vol. 28, no. 12, pp. 2024-2030, Dec. 2006. doi: 10.1109/TPAMI.2006.252 + * + * @param detection Tag detection + * @param nIters Number of iterations + * @return Initial and (possibly) second pose estimates + */ + AprilTagPoseEstimate EstimateOrthogonalIteration( + const AprilTagDetection& detection, int nIters) const; + + /** + * 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 nIters Number of iterations + * @return Initial and (possibly) second pose estimates + */ + AprilTagPoseEstimate EstimateOrthogonalIteration( + std::span homography, std::span corners, + int nIters) const; + + /** + * 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 detection Tag detection + * @return Pose estimate + */ + Transform3d Estimate(const AprilTagDetection& detection) const; + + /** + * 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) + * @return Pose estimate + */ + Transform3d Estimate(std::span homography, + std::span corners) const; + + private: + Config m_config; +}; + +} // namespace frc diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java new file mode 100644 index 0000000000..e91e0b48a9 --- /dev/null +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java @@ -0,0 +1,264 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.fail; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.RuntimeLoader; +import java.io.ByteArrayOutputStream; +import java.io.IOException; +import java.io.InputStream; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; + +@SuppressWarnings("PMD.MutableStaticState") +class AprilTagDetectorTest { + @SuppressWarnings("MemberName") + AprilTagDetector detector; + + static RuntimeLoader loader; + + @BeforeAll + static void beforeAll() { + try { + loader = + new RuntimeLoader<>( + Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class); + loader.loadLibrary(); + } catch (IOException ex) { + fail(ex); + } + } + + @BeforeEach + void beforeEach() { + detector = new AprilTagDetector(); + } + + @AfterEach + void afterEach() { + detector.close(); + } + + @Test + void testConfigDefaults() { + var config = detector.getConfig(); + assertEquals(new AprilTagDetector.Config(), config); + } + + @Test + void testQtpDefaults() { + var params = detector.getQuadThresholdParameters(); + assertEquals(new AprilTagDetector.QuadThresholdParameters(), params); + } + + @Test + void testSetConfigNumThreads() { + var newConfig = new AprilTagDetector.Config(); + newConfig.numThreads = 2; + detector.setConfig(newConfig); + var config = detector.getConfig(); + assertEquals(2, config.numThreads); + } + + @Test + void testQtpMinClusterPixels() { + var newParams = new AprilTagDetector.QuadThresholdParameters(); + newParams.minClusterPixels = 8; + detector.setQuadThresholdParameters(newParams); + var params = detector.getQuadThresholdParameters(); + assertEquals(8, params.minClusterPixels); + } + + @Test + void testAdd16h5() { + assertDoesNotThrow(() -> detector.addFamily("tag16h5")); + // duplicate addition is also okay + assertDoesNotThrow(() -> detector.addFamily("tag16h5")); + } + + @Test + void testAdd25h9() { + assertDoesNotThrow(() -> detector.addFamily("tag25h9")); + } + + @Test + void testAdd36h11() { + assertDoesNotThrow(() -> detector.addFamily("tag36h11")); + } + + @Test + void testAddMultiple() { + assertDoesNotThrow(() -> detector.addFamily("tag16h5")); + assertDoesNotThrow(() -> detector.addFamily("tag36h11")); + } + + @Test + void testRemoveFamily() { + // okay to remove non-existent family + detector.removeFamily("tag16h5"); + + // add and remove + detector.addFamily("tag16h5"); + detector.removeFamily("tag16h5"); + } + + @SuppressWarnings("PMD.AssignmentInOperand") + public Mat loadImage(String resource) throws IOException { + Mat encoded; + try (InputStream is = getClass().getResource(resource).openStream()) { + try (ByteArrayOutputStream os = new ByteArrayOutputStream(is.available())) { + byte[] buffer = new byte[4096]; + int bytesRead; + while ((bytesRead = is.read(buffer)) != -1) { + os.write(buffer, 0, bytesRead); + } + encoded = new Mat(1, os.size(), CvType.CV_8U); + encoded.put(0, 0, os.toByteArray()); + } + } + Mat image = Imgcodecs.imdecode(encoded, Imgcodecs.IMREAD_COLOR); + encoded.release(); + Imgproc.cvtColor(image, image, Imgproc.COLOR_BGR2GRAY); + return image; + } + + @Test + void testDecodeAndPose() { + detector.addFamily("tag16h5"); + detector.addFamily("tag36h11"); + + Mat image; + try { + image = loadImage("tag1_640_480.jpg"); + } catch (IOException ex) { + fail(ex); + return; + } + try { + AprilTagDetection[] results = detector.detect(image); + assertEquals(1, results.length); + assertEquals("tag36h11", results[0].getFamily()); + assertEquals(1, results[0].getId()); + assertEquals(0, results[0].getHamming()); + + var estimator = + new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0.2, 500, 500, 320, 240)); + AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50); + assertEquals(new Transform3d(), est.pose2); + Transform3d pose = estimator.estimate(results[0]); + assertEquals(est.pose1, pose); + } finally { + image.release(); + } + } + + /** + * This tag is rotated such that the top is closer to the camera than the bottom. In the camera + * frame, with +x to the right, this is a rotation about +X by 45 degrees. + */ + @Test + void testPoseRotatedX() { + detector.addFamily("tag16h5"); + + Mat image; + try { + image = loadImage("tag2_45deg_X.png"); + } catch (IOException ex) { + fail(ex); + return; + } + try { + AprilTagDetection[] results = detector.detect(image); + assertEquals(1, results.length); + + var estimator = + new AprilTagPoseEstimator( + new AprilTagPoseEstimator.Config( + 0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0)); + AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50); + + assertEquals(Units.degreesToRadians(45), est.pose1.getRotation().getX(), 0.1); + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getY(), 0.1); + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1); + } finally { + image.release(); + } + } + + /** + * This tag is rotated such that the right is closer to the camera than the left. In the camera + * frame, with +y down, this is a rotation of 45 degrees about +y. + */ + @Test + void testPoseRotatedY() { + detector.addFamily("tag16h5"); + + Mat image; + try { + image = loadImage("tag2_45deg_y.png"); + } catch (IOException ex) { + fail(ex); + return; + } + try { + AprilTagDetection[] results = detector.detect(image); + assertEquals(1, results.length); + + var estimator = + new AprilTagPoseEstimator( + new AprilTagPoseEstimator.Config( + 0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0)); + AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50); + + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getX(), 0.1); + assertEquals(Units.degreesToRadians(45), est.pose1.getRotation().getY(), 0.1); + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1); + } finally { + image.release(); + } + } + + /** This tag is facing right at the camera -- no rotation should be observed. */ + @Test + void testPoseStraightOn() { + detector.addFamily("tag16h5"); + + Mat image; + try { + image = loadImage("tag2_16h5_straight.png"); + } catch (IOException ex) { + fail(ex); + return; + } + try { + AprilTagDetection[] results = detector.detect(image); + assertEquals(1, results.length); + + var estimator = + new AprilTagPoseEstimator( + new AprilTagPoseEstimator.Config( + 0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0)); + AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 50); + + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getX(), 0.1); + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getY(), 0.1); + assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1); + } finally { + image.release(); + } + } +} diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/jni/JNITest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/jni/JNITest.java deleted file mode 100644 index fa82e6f75c..0000000000 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/jni/JNITest.java +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.apriltag.jni; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import org.junit.jupiter.api.Test; - -public class JNITest { - @Test - void validCreationTest() { - var detector = AprilTagJNI.aprilTagCreate("tag16h5", 2.0, 0.0, 1, false, false); - assertTrue(detector >= 0); - AprilTagJNI.aprilTagDestroy(detector); - } - - @Test - void invalidCreationTest() { - var detector = AprilTagJNI.aprilTagCreate("badtag", 2.0, 0.0, 1, false, false); - assertEquals(detector, -1); - // Still call destroy to ensure passing a bad destroy value doesn't break - AprilTagJNI.aprilTagDestroy(detector); - } -} diff --git a/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp b/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp new file mode 100644 index 0000000000..8f93bb3b02 --- /dev/null +++ b/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagDetector.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(AprilTagDetectorTest, ConfigDefaults) { + AprilTagDetector detector; + auto config = detector.GetConfig(); + ASSERT_EQ(config, AprilTagDetector::Config{}); +} + +TEST(AprilTagDetectorTest, QtpDefaults) { + AprilTagDetector detector; + auto params = detector.GetQuadThresholdParameters(); + ASSERT_EQ(params, AprilTagDetector::QuadThresholdParameters{}); +} + +TEST(AprilTagDetectorTest, SetConfigNumThreads) { + AprilTagDetector detector; + detector.SetConfig({.numThreads = 2}); + auto config = detector.GetConfig(); + ASSERT_EQ(config.numThreads, 2); +} + +TEST(AprilTagDetectorTest, QtpMinClusterPixels) { + AprilTagDetector detector; + detector.SetQuadThresholdParameters({.minClusterPixels = 8}); + auto params = detector.GetQuadThresholdParameters(); + ASSERT_EQ(params.minClusterPixels, 8); +} + +TEST(AprilTagDetectorTest, Add16h5) { + AprilTagDetector detector; + ASSERT_TRUE(detector.AddFamily("tag16h5")); + // duplicate addition is also okay + ASSERT_TRUE(detector.AddFamily("tag16h5")); +} + +TEST(AprilTagDetectorTest, Add25h9) { + AprilTagDetector detector; + ASSERT_TRUE(detector.AddFamily("tag25h9")); +} + +TEST(AprilTagDetectorTest, Add36h11) { + AprilTagDetector detector; + ASSERT_TRUE(detector.AddFamily("tag36h11")); +} + +TEST(AprilTagDetectorTest, AddMultiple) { + AprilTagDetector detector; + ASSERT_TRUE(detector.AddFamily("tag16h5")); + ASSERT_TRUE(detector.AddFamily("tag36h11")); +} + +TEST(AprilTagDetectorTest, RemoveFamily) { + AprilTagDetector detector; + // okay to remove non-existent family + detector.RemoveFamily("tag16h5"); + + // add and remove + detector.AddFamily("tag16h5"); + detector.RemoveFamily("tag16h5"); +} diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag1_640_480.jpg b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag1_640_480.jpg new file mode 100644 index 0000000000..a18957d657 Binary files /dev/null and b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag1_640_480.jpg differ diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_16h5_straight.png b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_16h5_straight.png new file mode 100644 index 0000000000..39f6595bc0 Binary files /dev/null and b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_16h5_straight.png differ diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_X.png b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_X.png new file mode 100644 index 0000000000..8d3521d08b Binary files /dev/null and b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_X.png differ diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_y.png b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_y.png new file mode 100644 index 0000000000..f024f5cdb1 Binary files /dev/null and b/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_y.png differ