From 1f940e2b601fc840e88719fd091ae3fadd625cd2 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 25 Dec 2022 08:15:43 -0800 Subject: [PATCH] [apriltag] Add C++ wrappers, rewrite Java/JNI to match (#4842) This provides a consistent class-based interface to the underlying C library from both C++ and Java. Co-authored-by: Matt --- .styleguide | 1 + apriltag/build.gradle | 6 +- .../java/edu/wpi/first/apriltag/DevMain.java | 10 +- apriltag/src/dev/native/cpp/main.cpp | 8 +- .../wpi/first/apriltag/AprilTagDetection.java | 190 +++++ .../wpi/first/apriltag/AprilTagDetector.java | 281 +++++++ .../first/apriltag/AprilTagPoseEstimate.java | 55 ++ .../first/apriltag/AprilTagPoseEstimator.java | 190 +++++ .../wpi/first/apriltag/jni/AprilTagJNI.java | 71 +- .../first/apriltag/jni/DetectionResult.java | 226 ----- .../src/main/native/cpp/AprilTagDetection.cpp | 37 + .../src/main/native/cpp/AprilTagDetector.cpp | 200 +++++ .../main/native/cpp/AprilTagPoseEstimate.cpp | 20 + .../main/native/cpp/AprilTagPoseEstimator.cpp | 154 ++++ .../src/main/native/cpp/jni/AprilTagJNI.cpp | 794 ++++++++++++------ .../include/frc/apriltag/AprilTagDetection.h | 160 ++++ .../include/frc/apriltag/AprilTagDetector.h | 260 ++++++ .../frc/apriltag/AprilTagDetector_cv.h | 18 + .../frc/apriltag/AprilTagPoseEstimate.h | 36 + .../frc/apriltag/AprilTagPoseEstimator.h | 145 ++++ .../first/apriltag/AprilTagDetectorTest.java | 264 ++++++ .../edu/wpi/first/apriltag/jni/JNITest.java | 27 - .../test/native/cpp/AprilTagDetectorTest.cpp | 67 ++ .../edu/wpi/first/apriltag/tag1_640_480.jpg | Bin 0 -> 15576 bytes .../wpi/first/apriltag/tag2_16h5_straight.png | Bin 0 -> 10027 bytes .../edu/wpi/first/apriltag/tag2_45deg_X.png | Bin 0 -> 12625 bytes .../edu/wpi/first/apriltag/tag2_45deg_y.png | Bin 0 -> 11275 bytes 27 files changed, 2675 insertions(+), 545 deletions(-) create mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java create mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java create mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java create mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java delete mode 100644 apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java create mode 100644 apriltag/src/main/native/cpp/AprilTagDetection.cpp create mode 100644 apriltag/src/main/native/cpp/AprilTagDetector.cpp create mode 100644 apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp create mode 100644 apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp create mode 100644 apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h create mode 100644 apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h create mode 100644 apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h create mode 100644 apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h create mode 100644 apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h create mode 100644 apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java delete mode 100644 apriltag/src/test/java/edu/wpi/first/apriltag/jni/JNITest.java create mode 100644 apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp create mode 100644 apriltag/src/test/resources/edu/wpi/first/apriltag/tag1_640_480.jpg create mode 100644 apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_16h5_straight.png create mode 100644 apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_X.png create mode 100644 apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_y.png 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 0000000000000000000000000000000000000000..a18957d65702d9f089c246431edb863eccdb4ff2 GIT binary patch literal 15576 zcmZX5Wn5K3_wF1*B%~Vzq`SL8y1PrdBo5uEba!_*QW8=kNOwpm(v5T@-1FWK|9kIy z=fgR(Xa6Sl%(K>-XU&@D#pfr0rS#6zf`^}#gNvJ$gPk2buYkAUB?<~E3hJx>oiH&l z|4+ik#>T_LgZ*M+Vq@dt;^7ex5Wosxe?&w?WMrhIR8-VVOkB_XAP+zQ96SU90soWW z;1LlZFOXhB!2eqg00a&m0TC0x!ND@{FJ7V|zJyi%Pcat!8w6|)M2UCmi8wD@IVnHj zHsCdWn=VY&(vZSmrz)C3lC(63x+Ph-(@=A9pT6a3I{UbxiS?guVGaDJoBw2CEn#uI zfs;^&$A0HZiNN{cxffu)gsi~@!$T+m95#T*2G7d?4ORu#BNh+?w~$`6_HM8BiJyL= z08(FfZ?i3&+7%4B`*WhAVd7N)>Q}@XytU_oOz%eka#ynEEkJ8xtzKaA&B%|-5rm(V zw5FPWs+>A-cOmv&)mN;znCXc2@SouAn@c)wQWg2W!8Bj9?~4vk<`Oj(vd{F z?{&2M-Ty`;NutiRErjzcq8CmmKuzL`6780cX~Tb{%2yRP8}`LD*Gu>unB(+SVZ~z0 z+8)0qU-;qD15av0pFvRf44yD5Hx3Rz)-*8E$U%!P-9AmE&cjV$ouXf$rnysk9C+-> zOzr=Q3fZO#b<%PeWi&2yCR6a_Dt6@6X2{|kvsdLh=AMy1^wm$SOy($OSbC8YolbP5 zchLtOw!&X6twLxMM$BEg6PeSGW%(29gg=@lsAE*f7!CcO!JNS$ zv*#r*u6<(G!0bUzey^oPYlwi<#3Qb%0Efoq#ph*J+Lmoj!Y^36qJb|bG>U{NZ-VNQ zuL!G(2-gwi@j2aPV!5b8Dl}(^bLz4CBWWzolS>(28Bp~a$`Th;wHEcTtyPt}3RN>- zCyo@;3^Pbn-Re)k%{H_AB{OTBuKJ2br}gtai-2ZN_~DjK7Key}*!dEEs>H;+Otv*$ z`uJ+RB+DYdX}%4(BKeV}bcX&w*>6K*8N-fqN&uACoRldvZ zdbbq1W1~zJH7%l#FOQQ?+^?<~8-_=zBYWt4;(oqj{XV@5d(fL znq)c#6;z<}ZB*3hf`+kSDBuXK-_zGR7zTCNh64%z&h`(vKLOwy_-p#G$FO_otN3jneS66 z(zj)?opr2Vv$KNA{S&#J`T8zvlD4L*WQi49C$tR+rD1 zrm_W#+7TD~qVk15tqh5<+EMGDYaDM48>x$gZvuOXN#UyQZ6(Qn=O*ZBmZWagQ`&4U zIc~2%f4p|?%8p{N8)~8UjP6AJX2(<{RY4SBLi9#f9)jpkjjo+$>q@JGf5~oN7HO+5 zw~eEXPFpaaBac(uNgHUOZJ^oWu_#e)^_Mvv{bO>|$G16^>z1$but{lFXWP8l)Qt(p z4^qC|>Bi8U6IsV9_dDZuDMdenRFnQn|0Lzr64Z0ak{M~9x2rt|C3Xuso)+OMKmaLU z7xk_e*!b3R$;<`Ai~2D)lvDut1RnqhUa%U4rUk<#RW9ZS1Hp-NWY+Ih5g+~<*<7mS z^AwH>1-?A#bjqk(`yH92^znq6e1nayR4DF32-5>E5M4ijzd0?`Hj4l^RkrTTx}}g? z*D^J23#33vUSpytxz7Sn;*v@ z`oogL5vr(DamoJurEQY*BTb6ZRz^*LDAs~OPutKS_Q#Tnj!1Q~cEXt^-9GxyYca9J zcv5jmY!nGXQWf|L`H6K@aWpw}wth4GM3?l%rE$#1xVe(4)5KOIblwu)63W;`TJn_q z+8LJRA*lrFWZ3TWmyBho`*^2Ia{2rJn4*@2hMK}6#MDl#{nH9{)YC){YhU$8L2D7; zBo%Ld&A)FaxXC)VwKoZS8&tN-?Qp++p0O$8=3_cBNC9R8)9Ifw9|0oQ>gg846u?km z2WXE~(5^@T=@;xd;CylcFJeW$cY6c4XE!lM{9Dai`52o#_t0VrY6$PqV1lD%*3dwQz`Falcid4gT4Y zJ21UPOj+XLMxMk-XSYPe9Zjj5#<}j#_$T;QWv{Bi$P25-J~MGJ_xiy5aj{9pNLfTV zM(9^Yb?buIrPhq^osCJcfnn;#TeRZ#AEibof88ByR$LxNRKg+}<3?RZ__IBAiX9i# z8i$T>TI3Cx*LYpmQu3n4;zUBW{++nnO$!PThNALv9PcL1M2p~yqzMP4AMzCwlkU}0 znw?$V1YS%HZ?0H`)0}3fD2DK$%-NR`)2c+`X>g}bGt%{iq8m|Q$sOYq$-m-WXDUuH zeH&6C8OrS$-KH{iAVFW;Y~1G=4wqC`!J=q;{KD?hK$}NOG5i%ym^-7TPr6#M21QjU z{%I6##jrLCi@K1EjgP*+x>TagOFNXhk6P2jEXAo~c@!v7224rKj@&wfr=RisKUK-J zwx1~G)`wDI`=WnEe|`C9oBH5u^#MDe@reLOMlt9Y5plXjFz$nRG@v5 zznNM6Ofb9}tsvMnrn!2L#fLXkW@y=G(f{z7{4TmY-Dvl@cf+1`}eQM}Cvt=jp zmv5TLj%Ml9h7#O&I4>1`TVwO3bt&!3upG)DarGqN53RQEj?1=;I^4+S7D>}L}P z=&J-GcCG+I>mLa-`1oae%>b$XTbli2Fy!4{A_XvsWrqUf6heUbsfAn|bk~`;Q@~BU zCM-O$gff8_)p*`2kCvYHxwo0o?EFnNcD9a>PAuov1gUrDZAvjEBi`Gu?I&dH6{ZF8 zS%az;T=^}3_O8Uta&*vosAT4D`5tQ?c%*9-{)k0N9LCh72{gZH{u5K4t7S@H`;9s6 zimzeYS&HVhvSl5|&QL3I75hcbYHUKB3Bj0zTIYi97~SYwXXohgKR&VF+k7+qH8tf6 zQ}zR+zG&cFtCT?Prg<4S(-*q%sUq<;xWej-HqD93aj4=s(_O<^qp7PHoAkGhLuS}d zhv~1gZ!gM^(cG|^om|q?hnk0D0JUD?obnq?K7M(1>c|-)%&LQ7SkW?kaO!CktAK>~ zOPAti{epD>R~D!Ko|N!KD5>R8M6XmV4&?w^>u96kxY_ z+Zl&b%UmH`Q3N1+C9j2~6fNiuaVwB?%-uA#PcuC)n}~>H;uNXM>P9G&ZP!0}o zqtk6jMbB&Fo8MXbpJ_E7R+b?_-fdcIER0%YZdTnY<9y-qh`jW_99H$}Z!>Q>6NpAQ zck?BjS=`R?;gv9nlAuY=?#o;CXZdCN%NLC`p6FG7UeerUwsGHZDbH05+Z^L%f~O*D zB<`B$UJsvH6Iv9{G$#XoiA_A%bna9xD%#tK1O3-3^VhlSl6|Gp$L`jf!z3)86OUySl6d`63j1y5s8A6D zEee8nXwe4{&A){H1|EHB^o^C1yk`uid?Tw|k$%xvWt%n$Apw`+?k%?oD(oAUYvR9d z0i_ap#Zq}X=DJ&4TB~C+`TjX;)!5@>(35U>}Ofj071kXyBz?&$s+y+z*G?$ z1yFD!0MH>bGyF#*btUKWfdUZTa)0cf*26>1E&MJ1nJSje)&(G`V1n15Hnsc#9E`IX zRTE1_Q?#@WU9M2)ioMw>dQ??za0tt$o^#gyB#r`;owyT%$Xji%5*`ir!% zzE(6Af}wmkZK@zF6OGNKtr9Z5%pX|@uhJHGou*c#q^=Q@&Ie<0I%lJF@&PfO56wE#5s{2s@&8(dr0#(uk)niB_1*auAvsIW-!4iiP3KP50O; zZmtJ8N%%_Ui*!34+7Ze`e{cRbq!JRbKi!DT>MzF1upmJ%A`hpUvcS3Fm-=m%v;_IgeI49i`)vX^6Fg zheA2>8y`wBfP&HCTAAusui0lgqfw8ux}}i4EgLdX1-(i!wQK6xR03q$yY1C&lMAZ= zj4A{Jrn2_=V{+Ic8BqfqW&KNAK%)lLF$0+E{$*-Fi#|tt3r2P#G(Q8lXuJY|;vOGf z$fqUV!J(nr=GM5jwYRMSGoyH00K1C47sSD#p3$34bBDIm}A< zhpFtTP(@oneYhoIJl_?~o>ZAt`oPiQ(Zs0+5RpYO=CE=?-?*?()TxxsD}u2 zD?WqatH-Ahk5>c2*)c4&LP6>(V&XS#r9^|gsH&YkG$K~gQSE`MZ=jhDg4T%SgC#|B7(PkACN7I&fl| z`1WFDWph2N*^%1kb=ignf6sVvMjHZ zlQx(AryV&?9RCV}5Z8OX2kLp7vhYQHc{|1<5& ztxTFt+~8Aa92Vi}5SeQyBM%~98NdEBr|LVK=&f6s7!7dv1u~M-KLe?)Csg!cxXc3% zGcZ$rI{ERc0ML|2hi(CML=zZE%jR*Mhp8NXMz-$&?$hv&+FM=W4DZtfv0c0Owe6k zUj8QGlcrp(R&n^jFZ&_`;M-N2UfuF$&_)F<*lsiVBIoa~T5nw{_gDwZ=t{uv4j^|I zcOR>(Hv@%i0~pA~V(If`pWfBqrXg*%nz}DtATmn%RbL5uKII*Vsn+kbUZ15IO+_6C z)!blBQ+wxrIVTay#~|@_EB4x5*$y8StCaRH<216{4SfceHl}GoO+T+pzTuay`6vav zSI}nG>eQ`s6NkQm&x4_7P{R)5v@*PApf83S~R9Ox?rLxW^@uf1d*7> z?QLT6I>Tt(3@#>)3>Gi0;(C0d?{~Vzkp?N^U(~nCw@2sQYSvr6p1Ko>S$k-FFV{s# zeeItrN!a%nJ89m~e;RSgy?B4Fbj@a7{d?dU)BnSL$b? zXVBO#j8E1~m-R3H68A(q2Z5(qp>$SZ!tG@0ls9!BhN3K z^^AMve+aVslUICOc5!N26TeRWp-ixU3>Er`kgM5gtE|rYkCNmPk(afCwLbI{EKW;K;b~xA9`!*k> zl^=c%-8G4RdmM;8;T7B3y0^VkYH&%9Xf0c1B|pq&BAZQEw;nZtyt$lQH!&Ql!qD9R zClWNW{tQr_!6d_lOq+pw070px=JMOrI_jU{or@=kV^GZ-&IK!dUqFhpZ1oRX9h^GF)?V+L!)%9#O;HU*FJ2RpWtYo_Vc&{=K8JeBYUE8I z`z1sLmb__l$Va)NqDMg>c3qKnd!NqC|I=o5G#Fs2MQtUr|Ic9w0K|qBXVgxqmhrIj ziCzy|mvP^C{^`SxRZ${|+u~6)HR7>Pb1kXd6ut_?OzEzlNAe1?t3rORzo9mnR(!S3 zQlO(VpC+F`cA2A0I`2=Hbr3jXQ{hYIP-Q&$U87wo>Q_XUa4?wtC!_v2?<@}LB;=QZ z;d<78coo6yX+1|tR;D-z&FXAqaseplREVR^0QH{od=}0%9t-YKF;g>yYWl<;LN}hs zeGn1PQ~>e_Z0s-v5g`NS_2}7cU1@f;%10(+AxunDwYp>Kt5Z}uo}vg@QdlfV!R^Si zLR$PYGM$aag>WNHF(a3{#+nvS7vcNGC{`V!xr&z0pn3h?Hseb^(czVB3Cg|bGkA=A z4DY>{RUBR5bABJ?`bo{r$*7Y1tBB1W*_ZlK=k#m;9_jpiK@8mhf`GGpuHD%|W+!2P z3k&VQ_C+e; zIP4*;{rYXk|BpcBA98{l3+?Z#-p;B;t0y)vW36=c3~rdJ@Bh^MKXb{o)yz*ezCj@&;vOISKmrEbW-czdaUJPs$}Xc(Rn~exLu)A9TNT z=8>>-)z@TE4pX3x75`vUnNm8o@iSoq#Zqx9u`APVrR$5E#ae&KqvCzwfuDbsT?_ev z3H{V~b}aoZo}`v~+cR)H^8IRR@-2y|pd86Ads-Mn^0&*5cRs`UGr-n8C%b1Is;c?) zjtcssM*VBdSg`5K1E(J}-qT%ipa44t?Rr>?C^m+hi^M|>gS(u9SNuhA@c#w$|72bE z0Q~UPd>P-Nu%XeU&rixcVdt)Yvf;MbBe6VnBL=)gS_#CFwbTzN2G;Yq4RkY~Ti!}T zr@B(*YDi^fGfB zF7@Vz(rNAYzL5SF{2G58pH=n4tP&xalp*+ZvVGS=))(<9D*FZ^+)>CyB{mUV^tY&*)HGfH{Zpvtbi+_gNfwCbOm$M@YT&DQ(!F}^A6|2BJa=vU)~$7b znUSzj0ZWw}r_|_oCY!=du7z`VjHizm;n`h9%g%fqlULy3-yMH1=*8Bkz)nhOl7^F$ z$!zi&GEeK_1v3)WoO2i3!OHJ1#@+H8YolaDE*t9Zchz64oGl;S=5%RiGp$p4b zUL7RK`Er+{(MpF*V~Ttflz#kpmZ&Sz{FEY>t5ALEikNX}I_B{}miG*La(yf?KHnN@ z1?=8MBe8tx>v@K8?NJ1OLA~nC7~=?#Zb_Rsuu1#*`I{s-F|CHt{i%AWnRxGe z&G!rp&_zyf(i3M*bo`eD`CNx@Glo{>TomJ59a~q7M{D%8^0y9&e5jMp$--XV(lWGE zIkB}}`LQ;1_Rd?mnI?+`_?T3WxV3byzWH)|i^0EgxKsMY}!M63Yf&Yo-O48dezj>s=|(1ZLMv&Vy?*Csu*bI=QF5&IT$GAqW2bBqKr}=3|Q&S zNL=uPSeTdWt;Qa|JIgoo3J)rV)@x4R^aTrJD5s%ya?;T{DNf_Sqeba4XR!^F{GJ13 zacmD@rSOwXZP{kKCUIT&_*r~?2HDk`_nZzEpW_G~^k9e(W2EvW{eE2KQFW(O&Um{2 z?@{V$rQrc7pl;IbNyW5a@}n<9GCphT7atqK!EnZnWCgyR-}aYR@zS;}-b!-D*Y z?ljb^PGnW1lj464_~iMPUfqZ(-Yn{>Mty4w&$*cD;YoBooqY^?U5hth-cAJS4* z(rLvg6;(Zl@uv0s7v(em&=38{Qscc|?dV2ND3TW3(sA!|KZE@r%APo0Le75;;p#CO zc2|!*$C;N-H=MP~`3HKK!~{GZ+s-{|SB*Hsl$^P(h@^iR%kXLcu{NBUC~ZFQd-U;a z@_9qb$f|-9-qpVvO>O?*kAc3mS+!$ve;n(7yVsc$SbmVGTJB5$GldfQc$Zl9c?1vL zrfEp6zkO|{FODuOVwwcDzVn~nRkHc2+1KumUkH;IvUcpSj(*P!JP1-J>7X8VZ?=ke zc2M~rnI!KIs{DDk- z%RF2yJKF~snl8uRF|vYD5|-|sfp$B9@Lsc@SOX+vp_zJdU@3f=07zeveVGM_*p>s! z0ABr!mkGQ&?!>Jdr?b1zx8`ozl;3mdu+P3q#b-WdO0o!kUG$dcpUZ&`{T|-2n9D9J z5t{b_tNb%iDf=XIFjn}YQ=8bQ@1n0{HM!k@O~(c9pScHrojAZRmEbR56z?JAx?H(} z*3XW1O#k98eLs&~+;-Q4{kYC{{b>4k{eghge`f}(W1~OyetqoD@gMv+UD*k->Gp6> z%6;DS>I(eXK+FBQRl<3sALxyUg;XVWe6g&41jaUyB7Nx)E{8`y5~+dzM?9 zfvfzywRx9x6rFCQ!x)F=iI&3dGrBQV;&n0-)4hLz-7T?3qgYFfmYAVt9KOub4C#f= zW~qi)fp4KAVL8Sm92P=EVO&J$->Hy>-UN@CrF{wTnm)=g8}3bUj|uw!KP&+-c=8O68~w={h0)scPCT5^{>g@>bMzut zbA~x8m6m`PP4xDos}+^O?1d@Aly|u7RQ#1Q(ky916_EUSW#V=Zt@rXN@_Mre>-FVh z;)t$D*);(knod(d)yCk?h8|G5ll&1{_Fq{9WOk?7Bf-F>M9&?-y>AdErvRoZtS-T$ zj_J6MWz-{%{Vtbdr=OB3UVN<j!06PL;vybelHtvR?byY=#p|$?V7NH*PGxCx1XQl_0|YG~7AuQ-DOGs51j3 zrqpf-3545z*10|?JPEWsS|F{zx&9Em@*;eKgzbk*AU3(3=D}})K#x!VY{kihd&(K_ zNgtZt1$CL~-~GWcQhJyyGgf`qw%6WZ{%a#oUiFm>=a)lD=f7fNKeQG}o$rJG$(dHy zYb+dPS=VeYLiHK+6-KhZuwW=&4?lAJ`_tr`LzRWAiT$hH`;ze%rA2VP3H^7q#n@t8 zSfjg96#qv@Dtol~sAk80&%7+dDPLPlS9=TBJ?C%XaI0fngiHC$maZuV{<(S1I9z^S zL+-V*btLirLvXDRrA`5yvC;usNie(w zq8dQSis%IJYX%uUD`krBs2ib2*6b@QTNj>#ii!Oi0?tt;joCtcjx}{S8aOQynDAYx zI@q2bf=mcQG=e%E6Tl9R9w5@t!XgTepcL|gl&G}Npc=-x1$4_)P>P3;C|<)Hq2(C-&506@C?pKFmC)! zrOC+;M3X%n{eJhq`I_^FbcqVLI4&tB%XIpkvrTrLkRQ=AsCfpah9>du-M?PlpIhFS zMe9oLJ6za4a%RI$vov?Rd&DMMRUbJXC>*vA{yCwXJGFj3YGsi2kHlUG><}yK84LKi zqmp@BC)RUJbHS%%n)U9S95OKHyYP2G4%nAtZLfiAB-1F68iAikp>{BO7X|H5~d9PhW$!?|}}I5Cfx16OR|lw4BtVfyX9nARd&Eb&l> za1^z=q3u#M59N47%4B(M@=9RSK1Sk2S6|H% zK}h}xC#0M{Yiv2WSj8y}g*Z}!KGA`}%gj`1`COXQ{rxCuo9VlH`g%UkmshSDG4_q& zg))xG3W+r^HO^!Tw{&NO^;Jjf)+t7G(=Cs?KGe%ItJ;fluc?G(TO3j4ftS5x2ml6{ zE`w{{ngW&!rxtqv^W$rPu&TXKU6peW5TnMpfKPzf1v?EuIlGJ3gEE6Kki2I_2skha%>j!lF<^;2Cqe!1@BS^CyVKazCUmPK`w zDeuNdR4>xD{@k@X&MtfW-kBvHjHl~pXOLFt(yij~;lrTbW)QJl3H)-Li>r8=L0ux!dEUZaIY|&C1e(^<++y4xhZj-Oz>g zsrY{zoQc9UPPfJfaM8w!!DB$+<-hBGJ zUIZ;yS(W6*s(mBczS-w}zz-bO9Utm#0Rj07XPuGG{c^pD8H~>!zov|Z1XGfr>gvXg z{;uxnz5ezZUzzh{nclAi3Xi|@+r&Cth|&U>@}%J^uMv0Ou5~i&jH+w+rVT_A#?tJk zSMwzLCYE1Sgw)^vismb%A|a63qP8u`v07*1I~)DnFEQO7r=z59Bt({sb|oV7U1*Sd zR#=At%JrZ>D-~nSblA4jD9LGDHHsFbnvTCLhQj&cjoE)eP}HwWyJMuM`{|!*A-U** zpwmTA^ntiI$1L}j)2V_lwJhsWAFD=^srL{=RI-4H2DU?LmQcK4Nsgml-J5k4YU6Q}O zDBWUpx$sY|qPGhZF0SjQwa#ywy~a)hVw~HDI7Wx;Rk)n58wg09IF?XXq6Gnl=DnH5aW0gII1@tZ{xXV9#&| z$x;eTqzYg|jlE!;aO2U67@2!RjAF7xZ)YkRdzw3scx?;_fco}*e-jDmp zD@1)~P$}BBL_SsQ4VJJ{-fHu}ZUnZ=ZvfoUOkVr}?9kbG#N7bAOmHzm^+hiZ?uZgA zp^etY#h3Y!QHS!y%#o2tlPzNC#D>I`m%?r!{fA5_>M$o(TG_P#O<~k0KJz6H$L@nU8iCTzFfQA@m96tfm$>-nxY|YvzKlm zcMuh&wlk*e`kt*fuYL%0L^n!)wm)B(e#U^XGm}$hKI4i-#+-4?c~Go#gUgAZ zD)0r(ZSh zf6nTottmy~ycF>a5?iq{7B)>(!`tG!XifxWO@x<7+Ja$8{Q;mWvGM79 zm7fCqnnP`KzBNGIpBoIM+iFB#0@S==%@kX}NLniZJ4vSAvjy}~cmQD~N-!6|Q2;P| zr4NoMkXIp0w^dHdKMz5MKozgsmG&P7p+LwNs{ztj=A6QT2x)p!7z*5w}nZ{7t%vTRp-LB!`?h<{qTJSxtu!o(uFc4+mqy>$;3l=M^6FGX`s z&-dT5IDE4?+(jX!U*Vw`{ev9A$&eO|u)wfABW8XI0ahZ~RWK3Mq8u z_DWaP)bg^_R>O&u&>tO<2f87X=~UOrsy%NtCly2nr|Iam`IpY`TN8e6|){} z+hx^z{IYUT%9N&Q99g^c^~UY2Qq7DT&TP_*lAF|~dvBFQlXGKLOmr@6#3)=bFvynj zSO3fY`O7*I59=bAA&XVTsQR~o-ka%qdyjrZJ7-;v6K_S-$qKI0$Tm!Dfl-EIEh_*+ zr2up*9#rbX0!VYIMw9@GOOGD(C3txrz<8Pq5b<+hsC0@0U>~ypNW2vX@GJlxQQX6g zAHpJ>(zZCLEV>zM%snkJYbqAPJCS(+P2H26fHzhqs}{9=ufo4x$~&&64Db4CnXc*RnP~T-wB}T_QylNX( zw{kY)B7|xhG@oRNCbu52+KG6`4zKO!g`*qgIkAd_B^BxNwbus7PKk=qOc>z#2ei5- zuIC*3qwi$7c``VD6d0$sowD!3)0lO!Kk1a zZfy6EXH_O+_-j7yvz_wQo%JsKEZwzEq^6^&J}}59X7p$Aiv|9XppMS^BjHwJ|0ToG zIFV=I^7w0Cn4{aHTZC@Xr%_PN`=`Tock$4P8^czzap!oZQ=^y_b!QopPm+e$%5p`% ze>&a$V|8ClCR6j2&!0%6a-(q~#mxK)5eHlRt&x+qg_QOS#*|K!zY)6URiUj0O(vRD z4Emah?9ypTa}~$aeKkWS?P45(uT>%yloJr3t|gC+U4q$0v7wpgJRihHUWu)Cf18@Z;iP^xz=W-I zVh1<$IlNC5pLWz-bF70lhjv5z%3tyCxJRLRqHU$%N=$5q=!GQhoPj}>*OLFL zzcC+srgqle#w>0+$4ur9uIh@6)S~f`P@FgXEhL{H|K~KxozJ_skfysaZo@m$w$ixI zom?<9=$C{#xS(M8L_Zrf*EeEZ>Lh0bmqd=ooaCToFJM}8lxY&OiaI}mTc`Fub-1ba zK)TCcx_y?rPMQwA+xK-Soc~d2#=Yqj^4O@U$01F#Tfk`^5 zI7A%VZXuS`xi^B8cII*dKq9)V0URni)Bqq|@fg6w9dipn5c_)pr;s1h5FnHX5E8ir zKv4j|OEaK$M%y7Aja8RvZh_pS+?ZE#G}pM8S~6Cz-Gx6|ON^pSAfwpdL$w}F^m6EH zR6F4nsQyF9;K>BsFII6|s)9E`udDYR_hj-t-eiDi>($<4CQ7O;Xk26uUSR*&cyr|_mz zEa_vDI<)1>V>P-jQz%sHFL~Di_(vAz?*PFhg`cPZF-aj=#`E}NP(g_f;Q6g29n}aR zY47|+5&&Mwr>h~rq+Tg14!-pP2qYB%Iw~}PW}*TB6=MOQlvo2qSiz7FFcAn$gh4$R za8eZjJ83oWx>T(lfNiB2L4fi`4xpuq1440_#=?LF;TLt&0!ZTO0DxCTDm6xexN^KP z4+yQy+OGJ?9@Ao#CNsjEG{qw(ck;e@v@MqVVN~7rn2ySWqkr6e@HNRHgJd_`CKYoi zx{mIEmA$#VV~JP9PaD#{P@kP}|lapp(MPJ`9mhu)hZ_IPI z`RcHZE3uDi7`HfZkg&0b83*oO@Ok7V0F(Owf=K}|#|i+WmO&MOXbaZN0$@iB5S4kv zf!Zu(Fo0tO2-X<@aI*k}CuLfIfQ||1;bG*CHIiN3*$mVmiUUjzegIHr0K7_QFeHQ! z5E0DY!}~e6#yKr>Wpt2?b^S_IeOTyL#M}GU*+4xh+rCHA$-WU|k;v+gt$g=dj3y82 zlJZ)f`(YmMD~_NF#y%`QG9@{lGr83Ib#+ZUXRy$4e##f`X3CPNS;uJUD|7QIXNPR2 z4FSh!biBil)5eYhwAz%y5iOKzUxtRHi~Qw89{rY4U}4}IapD@~?mD(D(g&^G`^N=q)L0AWPJQ!rdNZZSqMpuA5W2N20}7*oR%P!VA}1 zaR(?KG8>~qfalNm*H{|c;6=rBTU{_j_AG{t0!V0C6#x#gX{HE(f?RqDUNH&+R3!?4 zVCDoM@GaZ`J(U%VRty$^M=XbH*;KoXhshpILS?~0&QSJ zW}n?TCJF}VMX(8OUf)*(%3M1H2>?!#9lT0X=Rc!|B0Lu=x4-irh28`~cFvtRc zE?FuR3?w69vTLsb+Zd?8-b=%ZNnyd6rHtMHZ4MR{ZGQ@DP~ji~V4I@@@F4&IIA@r? zEy?ej0)j5nG7iT)gStP@ GOaBYz-gA-w literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..39f6595bc00d9719cab16623b75d4cb6ea16993f GIT binary patch literal 10027 zcmeHNXIN8Nx85KKVgnfqiWn5jFe->By@(9h5EP_XD4zmCq?Zs7$e^R}jS3c26asU{ zf>Hv5^cEFGVE_>!5PBDq5FwHXA@Hpoy?^g>@8A1y{v_v|UDjUfUGIALPU38iS}v7a zD~AwTdhmexafGC42rW>OT?kKPD&D<<52+xtgLblTU6ef+3-?OFzd8on`gsJC+ydQ^ zr>~!n`;MTqf$r|ULFfH~xeF?dq0tW9Xn&x)Tdt>A!)2;!UbR}Bl$4Q* zSYZ5fhlJUIfexgquu*kmk6e+&!0lIYJqwRqF{emLY}{w|_hz*@w`-iq&=l0d9y`(y z-0-P=GNNnx;-jTs>O+mVP1m(&OA0(543mAqN`&Ok74pSj&=;-owfGxFw{MU5NkA1* z{Mi@F5Pw!F&?NAe*iFt0@Ta_G8TJw)+z4BWKTVbA2blN4JPzh_VV)1>IdQ%&U>=yq z!T;ogoh>Q|^=zs9_C|x*J{2j)xFL26+=gc%R;%t$#9V~t+bsSql%CYZ~Z1DZ`Bji{}1oBfDkfxV)|PpAHY^*;Cgeiawt3zrOWeuN@&%8$+d$sR}r# zhpKK_R+AT>HqpNyp)?(MMA1D}A}k3!oo#4<&=mBdNZZ{AOZ4QrzeowYD2vd_I1qK_ z%;P^zev=2g55eS)2L^Q=$$V!+;{j+Xg9!J)NYl*xzdkb=E)S}&B4iT>Pjb>~Bs4sDCZ7wPeF_4wryh!TPY<6^}P_dK801`0q-uEY<@OfQysSC!JF zFCg?0&%QpERC_Aosbpw95qz^0t4;@52s)jK?L8{hGniFwOL7@5+lx@YB52Pdd+)H* z5j!OehVv!RnulP^hnxN~v(eC{6VIT0XYG0hwYC$!en`b{ohG+AM$S4*v6C4Kk?Rrc zQYBeZZC;bH6c}+F_vujIqn~ZWh$XoQ_P$brXfDSNJz{Qf*G4oB`0(G3z4_Opg?0JE zCxz4H*TQ!qG%63))E|&%pXHR_nEsZKFOWm@0=y>W9eN&H8d_er)B}L{cmhv*BmFH6 zCJj7q-e&Hgv4zpW)mn+8pAtGZoLlLP=Y<5>JAMR$KRwkst2@wF(}q}A^S z)Tb?$;CXB^!AW`-FZ1$>wfAn8Ro`3?I?*M8()Zye5ARTtv@GRtY~*mbTsKu!D%5{y z`YcouT?F{!4N=ab_X;5c)mM{+Cn1^f$g(%n)Ag;^tX0P{^_)r$-Z=6j~6lW z@Lz|4V|DmOKDOr+Cd%>4;#LPdM_xuD)T)jfG-s_p&!~vnXdr=HeINohF$tlGDce~E zk=ez5ILBT806(NHQRO^u4Ao+i@2a_i2ln7t%~*z~uY}08u7H8I{c2uXbM&rzDkBfD zP;Ujxr`%J!${c7z$n_u|_(m8hF(sM76!I|-6<`uvb2+Cd(^J^N75G$ky9kBEvD0)& z-(cMQ7Uiwpqv(2^z82W{CFXYQYG0Rg8z2h!-aXEfk#~=GGNSv`#o$EB|@* z8~({e#q z1n1=?862R;zyx#~^E_5ttyYDI<5%bx$vTyl-`_V`Oq!hQ;Ue_7gv=Fw{%I4Z#wNB6bmvFL*dLr2cLWtnV~nHa6FQaW8x7P zboSZTD1LItgC}pCGd>=Lt{5WpeDy1}hju$8<<}$w4d4@!y;k>2o!*FGw3^HlL$Ijr zu<9K0D`n#IiKz^1T0hR-qGNB)JNXQO|B}EnyRet7?vz?V$K+zk2x-)F8Z*zF{GhLW zfJvgq-d}#uiR<}9GuDbcDyuA!FNaL6U~qbnRPn<$>*&PL(_Z#at+4gZ8{c(i_C-Cf zJMGnVC{GgE{DH0AV)sWTxKJAg6BliUe7~=Kh)MHMwzl76ryum!bF}J^^U!VyM8Abi zSa7+h{Y1yH{G>W4arU@W>B#rJwc|t@d31SuN+L=-w!PzU^ZAoLV}~RR+?OHbjXB*{ zwW>3F^v3hLGv7@}I0)X+jgcoA80g0zipa)Rkj0V^Q3cz>jy459?XIkyq(*0nqM|tr zPN<$BvdoYdQkmLU0JilcBy;H!=+$QFmv$#eb^&AU9!K?UKW<0pQ!n6_qFD6D*HBVt zVJ?d3AfOY^#QQ?i=JN4qezmTm`1$U^sR> z`!n#t6MjSVY@TRl*d#vOu$nj2!kjCcv={a+k8V{liBeYl7cN&882rfxD6F&8Vg7ja z2Aa{^>$A|Mu(q}~zgpMnqi4$|bI=v`Lq0nY>Xj;LZ)=>gYY)jM!!%bR2&OulENaNq z!_tc{ubu!l&{xr7Ky{DGk!zB#05^y!c2}n1qqW8Dj zF5^lg^d1LTI_|gX+^5Q2i$Nmc@MfB6*Y%czju#6NP1P@9iKG)`KI>X2sP|DES$%oQ zLbn$J@(Ugip;(v2$%iG6Mx2b8B~w?YxN+b)eK*X7G8B*Gg_%Ho!n_=;CaFT3$X0KN zdKU}8Nm~VrP1~*TWBmOVhKfI6nUk>REcatWMbvBvlpd@ULB~<5Gb%Dhc`lw+0L=tz zj3q^hFJaB3{I@f}zg`bY%A+O)Qiu|c7qxo@ig$=~XdbSt$8-JAwxhW(re_#R<3|uW zhl@h}kr@N4$;cC_bNZX8ix9;SPbF7X=`%7FC5UR7R5gD6qtM7(Z>t-Klt5`)!D!db zue!aSb2FxAu8Q?x3oaj-2fqCnD*;P3dI>foaZ&WsXBD(j-UhAFVuTzqXPD|W@AsOV zTCOyC8k{Xw4uol+hU45R!-iw2sgK}PI2~6`S=H$jkIRwQ{jabdNrwPZvBfd6QBRdr zZ`r5LZP}E%6j5gJESfRhBhR^+(=$3sQvlJg7@|^ZdvEjw88GUpXFNw(oq~M6FcJjf zVynFEsqP4knc*3>Os8Z|)I&fX-fUQiOgG?=y?&;Ah^sI3EhLtn;TH-LyH`Uc*ASMT zf|K#erTT+LH2zfjUJZm;m}lg^EMYfIG8x)d8Z)Er16Vy)vqWR0N}^w-sFL$~xrr{rku;G{ zi_8+SJxz#yW#blw#M6Z|&xmkiNf&|V6EAOCn{BD?c+@YKBV^9L5*my+_{sfQ#n<~|J?Xj3o{_1q;@AaoHW54cf?AC zI59W24bfcKM%R?COKF{dAlWj!8L1%B7G40hp5|%o&5NgOhA&MDfO>zT||q3-4N| zms26TZQk=|trjA}r;ly{tUm);!%)CSaVtU$>u)ws9i(8I9JrkD&#i!C=`Sv^T7jR$ z8@o%2>V>(NfK#=tETcuMLSgrcHKQrPEr1*vo|uAA!VwIv&D#U7*>a1bk{V#2^$ZW~ z`l=6x4(CIUjdohNZgyz=9ny@z}gO6Vn$GZE=5wHd^|5Dm{J_TBhxV!PD04!{*%YL?{Onjq6|L z?^m}~yM;o~*J5L04ChLg3&lg};w^OAwq3dL&m%ZfY}POU7!6}d{nM5$d^g(FHh}f_ zds6rPZ5GZnrZWJkNjqim+TNxBTO@H8Z(IgCMeRo8;~xAV18CS|jBWUhJR9Qj<3Tol z@cSbp7U>zeKJv*6qb?7Q(=Y5_B4KN!g^9o8(8aPOQAf7fIXDcc74I+Y1OxAH^J&o{ zB4^jK>90-Jr%rud#`b!!>i*|1tmujNTRHvlCS#%3?GAG$c?~9h)~Uv42J*udK>g|7 zh4}beL)Q2)al%4z{45f5`)!E&*1M6`yxTSUYojISsfox-`0-xFr@_fWSE#6r%GrEY z^f|{EU=sQ+Y;nZ>C(p`-W2Kxp& zr(RkQ)hD`m^8!S9VK4b_H6wn%!&?V`&mVW#p#rrR5N9aIDPT0cd{Gt* z@eyW6t5jVz8bIx};$O=sZ({L_aT{Vah5=hzG;`^-yJH-vxLPPX#`TyP$QFdN# zF=nqslq`(r&lcx7uc4r~O5)+Nf4pkM_6KkMZo)O3JU!cgyb_-cBwzo)yjl*s3Y;~R zycH*f_lMhjL4y7pTiX_wDQew;&|hLD^ZPqyd?z#&PtY9u{Z+HLt{*zbsfJ$Zcmmgu zjYN~s#mMvoUcQ2POVkj#zy+ZijMG@v9KL%ckP}V;`CnZ*wNcOwR}8`pI`f_gGSg%W9em@^H?a~TMMcMLJEl_%b=^Lt?# zuaa+VJoPAYs@BDK_3G6=g}m-Ay$f)jV%f8*p-%u6!GQ{F8r;O*yAlyvX;G6I1;E6w zuP$e(NIQNYRq#^sEkP+I!-Q~|Do1lcQqKRTI`0Ea0{?p)^prpPiqOI^{9l0i-`T-& z^ZzyYzxiM$+8}g0Mx5rm?(nq+a&T+{=vz+~(-Qp#&?#K5mcW*9U zzXK+QD|7(38ZLv0@8F&9j^+0e4t2 Ag8%>k literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..8d3521d08bcf5b7ed9b462a486a2111a6909f54b GIT binary patch literal 12625 zcmeHNi93}0+kdpLBnnY!IY;Fb5?LBlT7`;17>XpxZY*OxrT&3u=8{oJ4Xeo#;A9b3I(!wL+;R-ZVo zeHOzOr(@VciDgURlcfdkir~j0w?iikm%-z^>|!wdU&8%}iMxT*C3nvYt~QvhqmzTp zZZ~UJ8yiQr%TDgRg-i`tv>Pot>}qqt-OkBT+R)Cy1~a(eF1>f3w6=}4^j`VBd!^-- z)b`1%DalKp)|EbOU^xBgeiDXBV<)r^8hXX|wVOqddi^>F=S=N+s*W|k(;s?C393F8 z4DbJVG*0}t#Yet}t!cHp-8S+fU3$!KVgA>ksTH@B--VqKtq6IIJg1*40Qjw;HMpa#>3CN_=yh)2Y%w@&wBB* zEdKwa2Wp5gb@NhRcibwE*c?q0*siFqF8Tib`(#@TyM5PdH74~WJz$1AxOuiav1D#` zmV1Wf|8?bf@`aM~wu-autb+B-F(0NeWi^H^r?RlZJCi;8nG!;|D+kkOtC@0yAS*`Q zLX}|a*}bc$X2vI(GpxbhfZ1NQ5{BijO&18<_A)DLy{7MQYWC3mTm|*lrzwI*ma#bN zCfBZBG802^)3{N5;XY8f$Gx_muzGm3)u~)4VEm|gw#Q4pXgFzI5!p3}sZAsvp|`FB zEElh+u*W{VoBp&^Gx()gdMUduxz0ev=n?sfUBLljgRabyYX>=7Fl_sm&q|o{#e%VX zUMc%aoI<-gk+Pz^$d1cj>*r0Qg0d=X+rjohwR$f`DYo+GN4XKG&a!9Nj7$*NGvL%&e89=D9 zJVz}ZuDwSqx^vv(h+Szky*?q>pULC@W0S-Cr35vzd6VC&W8&}cCfq#C`ePU6&&;o5 z>tbt`%A}V9RC#Q(Scu&mm>KJgV9wUe-3no~dlD>64UgzQTJP_z*6#SsDTIY#eR?39 zV7tEmP^8yze_~l(<2|s(zTHnB$(&f3G-8l2pxxKkmp!%t3nY%E(2bcZ7 zTGCSL$wP479dpSAm=t4bj91?2I~I_`%V{io92%aq-ctBQT#Z~enb|2vSd0Z;2O|FI zbpAw7++e-utVRv#+#W)Z{?)3=uh|mCa=P`LV7M~$^T#!?%5QF}Gh(1bJ8$sy9AVa} zH(jUp_fg5>%QFp~%$e?H-=Xau9v&z41u*F?K=V}$J11&9+0~n;Nz*g6vzDBlTCVEq ztu8Z_$zR=l%@$yV<=!V3fhP9p`RhB!%l5O&HD_I5)k=3YR^D@Au*sWYbPOxqGUBGt z<|vxLD{Xo`HD}aeIfiZS-6;&;)e^?c+p=Af_^TfL5}Sg$jploFZIHFPy1JA%UBkNz zyRIX%g|hJtWKwy5>ua4_%=8eL@w})$6)SvbMk0lM`SPVsF>w7F6vU+lTQTRaQPI)X z*4F6g#jz<=2IFKc5LR3>cO4xa7j9E3+VIsQR}X^Wxh#HCTtY&ay5|78weX&-aXh-i z7_!ZIyJKg;M%V_4T|v$zfqeo=c*2^%-Hxx#C`)d~2k1ed-ogX*sd0J~4SN-cOTHD( zKgRBBc{y0LQVx#ZzF$G%3}pd^oxo3siH{G>_2hwlwzKx>a=3z~xJNBPk0bHV*(r9hUSIARgGtu9p>R8c!svK=4q3J)Uey4ULD*He18M%)$jInX z8o?FIb#qt-hmY&6#Z=Bl?;+iqn3w?B9{(%bSvNPg!H@Ro_du0>Qi2J{dfSoEXNg2e zSxrlH@V_P*Xi7E9$j&x_PjCOPPbWvX3dlf9ZdW<0xZ}xyqY!$1zL9{hZVlObm6%xqp7L)i5Lub(C<9I){i=(K4(0LdbOr(SqiKgO zh=_=|OpW%(#Keq$=aZuhnv1)>7YC3mEhBjTW4VJ~)tOt49Yj8pdlQj^Vb;m@oD$je zyt9-g%MVcaI}kY3HXD;z3aUyZN?G%_5eka7Z77H{a{Q(|;t4)k{8cMjTUw~8sX}|Y z|6=T_e!>m5Y_H#^>uyeVyEvcW^TIWL`*vm0CtQ_+gz!o@M`Bs*@WP!q%2q#yeKG-e zZ_!IoBT|+GDd~#D#l2?7hc<_xD!amQ63A9XMY z5|l{%Kz4X{#|1@DK1zuU23O6(Z=?~HJxk*nm2iWDsZ{D$JvBc+(byE1ur;8J_t`~Q zc>7AZyft+F#65&-e{9n9Z)127qy%+p#dX>pjq#XvN)&)qLqji~E(jhO8L1N)H6#+X zeb<;A&eK?n!-<&wEx1rUU^Xw3^*Dm1PtI+RiH+Yuu>S2I$q(#YfR#I5y7X(o(7p7# zw1XGsz6;H*nek`xL+>ycli!2F?iEtD$EHktzpjp92i_Z~V9tHta>Ao6C{K`;KYl#9 zF+K5{yPAUds*@laWTY#eEa>1P&(hM$!2KKvQU2EtNzE-SI|g7w3*49>JXYl;rDCyNlkL(U8Z(fWdBj0PqQtZ|_6BB3hyXCAXFM^WlIZD}&!~<{> zRU{BLrWZ_JL5dd?Y`XSV8n_Z?=e9=j#z=$uWZTeR@M9oww9dSC>fA&4{f~k6T(N6! zAMis7uxR6*k-s7EAo{UBly0|7EiA%UitPbcZh>gcc=^(RQbzlhv;JH}aP7MJ5Y#%; zTbmfcu`}KqZy2$OaOFtefVPJ>fh=~z>E)<85+XC|9L3U$fql#P4DbVn5j=QOF zdTzVpxB4!JFKX*CY!p?cQc{llPJ}qu7Gz~*S^4?p!$DNmv0`9>KUVDCy}Ojnwt_QG zv&i$#lpCD zqUwZfK>y$Vmp-*5YKX6UuQ_V4Vs1QZE}IRyZKMhmZ~pnAFU+Aph}&|9fFb!YW9<)+Q$&oy`K?PdKN zL81^rRxU0I9X`B#plz4*X$PoX&M|7|JE6=$BpvV`dd$B|AW-%RJ2iT1mPY`f;khfL zAA)JE5R`?Dsq~bDg@nI=DNiZp%!hxZutC+rW;; zyBeMt48E!LO9Qui@xPwA>I8^ANF@^QZCxMWek@9NDWU|p=Z`~KV&niC<09wY9d;yGZ>k&t=`4mGi~lmqxOB(Brd`QteX zGJgm%>9CfTmhmLb$)q|(l}lt&20qPmr;>^dleN$Jb4|vky!t@=9Pt65&1e&ZWbwbkHKIA9Z9xzuG&PPh?<$1A@5}^ z>PxYEl;)=n9QZPAy+ELQ8Yy=@WVRd})xaBrgse|sF&{D*rXttX6T%VX^*!0BpM@1t zT?g5vF^Tv}Lsn)a>*j;q^t5dn!7PX6{TOz)3qt>RFYu2+Gv)|f<{XM3)X_1D_Th@Y zWm9vMbRV7~-DsWdl`FfzM?lxgFWcMe+OC7gP?PVJ`{ zZ~>PKih%SbT!_i0tBbJ*tB{>IA2aRe`BwQw2Z!nk?%}^vB3bQ@EF1$qql-9idTGOT zWg?e=PAhdOz0C5x0bC-rWd)`sf#R)>&3*)xK^|^UK!x&he%7Kc$Jec~si4vph=85C zr%si;mt&fPs9!tseLnEUU{xj<$Zg$C+&uJo^SpZTVl9wJQ2bQZ>(`}Rk;kq5K%_A( zS9DJk#B~yjdk;M7J^uXyo+KYA4yx6CLC?U%=p{wnP+eJMawo4G$_hs!)rd$uRb3TD zbCE_=1&{n!J-xl%iir~di&BvcM)*Dpp_|2rC${Bg#pnH-AFsgfc|(=5NRt1!fZ{-9 z;W?YA$APBW=Z*giC0>oZ3u42Nvf^0ZV0Zaozf*uZ^oxnUkLEgF(jOixZ7o(&BB-iL zB+l~bAbfI2iN5{~ef^$@qI?TH8b?uTKOyB)lt402`8-gV4zKPo__zP0zQhw2w-m^| z?|@sPEWrj?O>)RL#PG4c^2{xm))i<_;Lj6yG%%1LV-434+c^oE0UbJ$ zC%RFi{r3E6O4)JbhHlil)p18CgG&RxFHN@9($+@F2@=(yDsA<|>gt-BqxAZngoRk(XH)>! zA&{%DhhnaEz<*i=C$QpARu8HnrLjJJ5W}>q?Ty=yz6^+zx#Wfa|kUsq*9J*ln28{Lczh41L^pRE0K9d!UuX@{=6NwV=!l0uOBs70vH zy=UU=V#{!hLGhpJHr%6YeDibzv86j(mE1O?=goWZPFp_@E&b9O*M0C5k;DqomJdS%mEG@JfQBuUkvI zRZ~*(a4x5oHa7FP_v+2!3kUqaTYQWx)~~MFtv$0`uXkXS*&|;gtvM4z+jP0ZYiPV! zO!-p7jYt-XfO#JQ=h6eF9YxEa4}8Ap+ofX}w(|Xc>+!1P?fRkwp^U>u%;{3vGeW4- zOs^*O=2(D>7jt||Tuf|uz^vS?O}y!WsXMjlr+)KSwwU{#+ZjIPG?NIi{DJe?Cq9j_ z%fTd|5?M%aYiGSORww|Il5I$lx7U^?)B70A{Eg{zjh1CXb3JiqcIAZEb+%B$Wb7e0 zo-p&*1x#7a8tl`RdEw^|tt5~Of93wnEe^p&y7jNDWH8JI&mjam&??uJaZw$oC(g;Z zaL~+8j-71CbRED z=ITL;mDQtl>H^pf9h8=KFtI3@{vub-O=nubIS_ShPgbV=6IvK58FkGXKSDf(}V9Q;Ga*SQPr1K;e&U%i@W_AUI z;rW87v>9Zfh*3i)30yL=w%=*G;;3;&2Fmr|e`!9Z|(CVATLg|rUv=99~vs(@f zPO=P$#Ea?k>5d~-BX@)yVmS?beOqdJdik}bLeL5~LVw4fx>lz+`YI#i&g?)>C``lV zBt>wo@ohvR5gBT%!-wh0R=oz}2bgszC@C6HDrsPW=%JwK7G$S($JhKt7-ovNrm2IC z%`Y^c0T@snrSv9vTG`st`JoZ_VVw2GfNtfg=2Y0x)^=*5#U^@U(|E9uzm?VAK|d~r zea5f!_)>YVfv&>qWG40u^e`A2(@79xMQu;M%7ho&+uP%Kca(cEf+dVqgL3NSS2jfEkVx4EOa zIr#1Q+p~+W-!~-Qo8@OBhJ5hzjNW4GlSnpb{V}?b;61ecU+eNkI?87nW~RoHI(gBw z54&25N2;nq(YD_|PYVjcwj6iP*{bU5do+c*DIPO%i0_=v(wTYj9$JmNa=BH%O;y-? zBEO#oG3D3mC2DM(FNWLlrv`F&ZiB)5Rc-d)Z?dwYhTPu&<~HBCsHgz>5MK#{4h#PY z7ziJw@bbsb7Wg_t5X3%arQ%EjCI-6*NyK>)^Fs+!7~__ff_**U?_sPR98ysvL#LU; zxl2^zP_+wwY?$4huR@IzBYK@M1 zKu4IYaUx!qNEBYqGY&wb|4$DNC@sLOyV<27z}wa^$4lh^Sz=`LX(iC8ilG6}U6kmT zV}H$|9=0N}20vFs8YY@~0QP)&e=u>Lo)A|nY67qGT>vpXJ03BoQZSkcbJrJ7_2?ps zii+UAs>w;px9F{Uy1J!McYzB7y*X=fX1D>md0(zs=PfZy-weJehW+{8rwYUpfow1` zHij{movp3Rm&PqEsK+WR0|qAbCxe0BM-={^4HJEFS;TvAHV`NbiL{Suxnnt|wF&vj z2o2p`U0?85oxh%9hb;U_3jq22Gjo{cId^csKFq_PINN*0ZbTbVPh$+pX{f=lPuBAjY{4Nbhj0eqMu zSZS~glaGQ@%dSaK2W4x)zi9FTTot{v2+SbEwe#(E;jsqtZWQ{cX_3!@F-G1i;Xm!5 zPe#y-$a7f}yqtJc53woWdwnWR08`na9}d=0gGoc)_o9_>{$>QS&ZlWoaI_M%A9$y` zaD3Uk?enF~+|cGdBc`+6JtLjxf2mkUBb18=!$IH>i$j=@=GwIh#-xVTET* z&CEhJ&wM?WY+FcWaYwAkWN_jSec;`>^hPl)cqIbw48tL%;L%b;_yiCB3JZQdMzaKX zelGYwVu$HDk3JLRk@L5iBl|o=kHW8g?|^>6O(APj!O#ZRb2aPYHC{GTb2i=K)o6kj4lTOhxj NIHISWcIf;c{{!Upqs{;T literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..f024f5cdb11e728b64ba00614fe25dfacb6e964e GIT binary patch literal 11275 zcmeHNcT`j9wm*)ej15FGQlyC@SV2SqX^s~}6k!l7AOg}1>gI@XWhRI@vdOw&4Bo1|+Bs%opY3266 zrM9?dBy|4z*!|?)sMRZerRI2ST6z7wvwXd?%1gQU$NeiEZkyQ3w!0=?40TmWKC@ZG zs477$uPtv#@up>dtV4!UNsRASi^`(JJr1!6&L?8^o(*O^VhJfe-IxB7XHIOT=meOU z#yX0F)fjg3xZ|63_`gDxHqlq0e;qd{LEgY*rMMF4Gt{TT1$|;~gug!U)em3s@HH>K z@&oX|SDyS@4*nC%LHePARd8+_8;jV+VLy8cMK>=mFSTV*pJ7c)P$O0yc&#N;S6646 z5pTKzvz>`(fiC0sE2@#k=_19&#o5`}!a_1SH(u+^?$P0oHwBEnQe%H$1bGGohzSd4 zzP5@QYL{J#mF@&(8L65 z>s!S3-V=r1W$Y1)W02V#IXpZZF#d;W2a`gP%%F@Vuqud@+4hI#yBY^4?1F-3BY1SH zkVEr)%FOHRf)a_Wq^v9M^y>L{B+2|ddw=h3f|LF|ebg=7IoL3hGVaw;8lvgWDwSeQ zH~9BV_ovWub92vMyhuHkw*T)r0$8Zh`m==%P)Y zppZqXUr}y!W>Lk)1=+}!p6C@Qxnb7)uw(GI81|Wp-0ShfFXfjcB_%6H zMn*);-V#fvor1?34fab=d#0w;{rvor0~Rb|npww838r)ZR&`6H`3qKrb*L@qRg=t2$bnKig>gzxF6n9=Zl#rd9`@N8~R=A1Iv_naA zH6w_Jz{Ow4|RxyK5Dg2?! z+y+gKUU6r%bFKu#S_u%kgW-T-@mqQhV%PzqvRFlT_a)VYSFJ1-&44CdSso_DPNV5W zYWj~<%yO7A!+x4z&=7}XLMrUH{{$O}BQupnj2}E~ANCs>9hcFn z*jf1>BF|<{+!i%TBw4D$E_%>M+dfJ(GUaeM)$p{dn_K0~3<+SVOP(J~sqQTHPiuD8 zr?*5oX2vR##OWL$vaVar7ZCAOo|` zy1c1_^i7sS&O13VMVP2LB%>f|6wIco^(6U!h=7L#1Oy$psxo^kL!P?ib2!QKCG;~dkD#Sj(7@+JB26ha zU}2tL;N5RrGCNWXz)odxs<~WGi2Es0jn@I&jc3!T@g;?!#&d?ZGItAA-kT zO?@sGVnZ}YjN~Z!QOy_NkS0S||G1N?gg%()lq25W-p+Q@8DTMHm{XNXJq+H5D8FJU zI2qbyXvm0E0~{-AjEuS&v-1;RpIfmCECXRDwfxZ30G8 zKYTb6mcMY}Li&S;oQ^^f*oZrjo^tH|=?H7ODEt*;tpZV6i;Q0-zJ4|L-31~72bD_o zyf;5Vr%1_!jGIC#{=u8ge)@Fd;NW14^}YkI-=agD0(%N%Whir=b6ozc1De*S@C}tX8JiH$4x;6EW<7aQbwyPoYyiA|j&4n@ ztgH+eDeSj%bd-;ci?a_5)QF9ZO>MkVae>JAzPW#1ai%jZfGsmmev=M+Cte48%FO&# z_Fts8*ei?aWu1-6JV!s4)+iP>5Tnj`{+tjM6_s63AVxj5?b8IweAjKoNI5yVV`-*( zdh5f=VQ15e=OdlnO3AMdqSW1Um(KbG#)VsEZKjuup3qvD8_R}OcJJN|ALI8g&3HOa z2j10=XolhujDl0(lTTztGv^1Hz{)dJ*6gb^->U>D;Z8?-g=8ST`^_@Ulj9$Pucmg* z3*chJqU*taJZ$iZha5g}|CZg~UFY@YWS0=vZTK?^OQSeeg`#MN7$P)K&I*7dzZ5v}+7F^$_jvpHo+`0@qjyQHsFS9xm$7Lm2K3@L?+7(fNZC-dD>sKK`EYu3!acbze z+)A8VQO{RcjU73wJIsOEn5Dyzd82i9=4wDf8P3pcVs@ zLdDJW2$lClxjqBG!&kD%%D(6}C`4qmwpo^9Bw%6J?OIMLnOtm?nVFegQX;u--8xf( z{+=wW!nY~@**}3@5B|1@>8R)VxzcIBbqPu57Zo9ffbQ?vo-}}_hx(21@Dukp0Z2?U zn^Udn)f0m)=*Ei|M^z=L2xOVfJ)b`-lcxt4wk-5bP(&97nf$mvctvp`B*;rObfO2A z++u!KRBCvf4OwHpn#I3p9x3<4s&ErnW}0<24VKMyX{qX-{x}(M!yVS7x4?G>fOtrq zX6J*>xorWDH04tF=%!dn3x;Jw;aT8}%yQqP@S`4{b!kj#P54pDdLn7BL_6NIezgFrW?c|VoM@w08`hzhK zwaJpgz2-)$Cgl<G4Xr=oJ4ivG5Yimu^Q6ov3$W&%9CoLMoa ze8`bya$sK~dP4@ZBNa?>28my z3U*AtG-den!!70A%C(J(tIL0w%j8gWtwSc73)i>ed&;96i*xt(K0uyh%b+d;`sT=< zy8Ky4Izd_ycXraM!sGXaHr>I(1T}W^IUB~yUXxXLu|f6EV*oh%xn)-RXwIjJ8MA2( z;M?9uMkMfi*>pTAX`4|z;9AC=@b_|Ztxrq^Xh9wCN)%)K4OdiPfPw0$AAxJJ zu=XV(Y;MA_nT)WBGdJy)tJ1)q|Kql5K^-Wm`-bxVgRw5bCu{Fzb zOcalp9Ew=|N)&;eW;m**oYBNf(7)B&Kaq{tvga*--gcvYFqxt%5pAQZ(Z{6=3wN@~ zgcMzIVPU>CZy*u!yQ~Fe`}RC2n{>vcWk-?k?%HQJ8m|xlEfCZj$I`go3O3Aug~u(% zA9SI^A1>xpc~eGTvt#lpQnT$_;{2pfU`k*3lH{fg5+7~p`|g$^BYX4mcW78VhMEp%Fn=AE+nJbVv37rHoQQfa|= zh^SNaHh|v~rBpRYXG%L-=m@tk_dHLTm>QHEV{k$^ZyM}(AvYWC1RvSfCi*{`+HRfl z(?>6#Y2O^+VCoh%CvUkTtpynZ@S{SZ?Kbe*u2WLrHN3A2B=o2J!X&&vB?eWwFM+ge zxV=B}>E$t3?&~#J*?K$}u_zez-z^Kq_joY2;=%X^1w$OA!WWR{71aq)G%a1v%p0;67TXx z6T>vn%F6kOm`0cWh7wwc6Hf>JBt8U8FSVh}3J04Hem-{`Y(h)VSI=%(wNMN0MFULe z$#du|%Kd1U2ef6jsp9m9reVw}7OyMFiUJ?UovA>0tQ+Xz12r;$w|IAS&Re_cMz8I- z8>JYzZtBo>yr1Da<1LWQ6Bu(~cHFafuRc^u5<@!Ia<6l)B{YMeo}kkDV~V&4YDqW1 zLPzXC3E;-0%x&Mid9xi%HZWrJ_Vzx6+ISN+nQm@-;RbZ1iABwW6HwdJU-;)*QlSQ? zH7eGs$knR28iok|T;mG?SdZ58;*)h}`14bq=}ptM>~rVNQA8iqakI|O&b5>-HVy1> zMvH4@-w1`qpc=%H1hw|guU+xy$lz3O;aS`=Naf_W51Y|*CwcoqPQik0!;|WFQ(Oa2 zXCy9c+9SeJ>3uONtxI1IehiITCUnrmwK zIeA(caCk!H$8tcb$5j;0eVx$K_!aXj-tq)wf;~QQYinyW_L?-bjeOkhV8bNyFNASo zruc4J`yG?yQWFpf<1@=Wn1A8vb8h~l`ZnK}#>K@|3(s<+tdf2IUZ?ayT8IVRiQFdF zGY~g#RQqjrBq{{cpFBBc5o;sPG)YDNgM2jrsSSp?uwkc?X_JKmtNH9G-JK9#d+;LW z%}KESI0}+lx-@G#jLA&=89XGZxzO8*SK_W~OF{ToBK*&405|^ca`FH8z($bsV0jzV zCzK^3yuCz&KU{wjeLN!iXE@OtuYu%SoD(HcOaF2ylXP8!$;D7c&51XAK&^s90)nicFJb!o!70sM~Dn?F4F{h%Y8fj!gr@PwWB;!AoqGEr} zF&LH4W_}MkABc(2z@kuqfOLv90OsvPt`&8FiEO@)IN$fy8+d$%PnMLD(g!Ju&*!(c zw?`pKq^%8!l;|U$Mb-q-=6U;YYiFmFzz9rJXyVq`>J*I*`5*$a*CXOlvoqLx-pcBR z3#g+fl^Ej{`yq))3~Nbg>1c&Ke}8}b%a;}M^7BnrjevO|U%|iaJw5kBTT*S-krFcb z{@aF|SMmM|O}(LOGR=3PaqZ;Eli7KBetr9C0`THN!45cC`Zqj~t4bE;XTMQP$BAhh zc+Pfha8lXDSVu`2ndR?nMtE9U+Lc_(Y$lVk%U!or2ww>aP#`9%N+jLw z+FXzxhI;}Hc&L^TBiOj=!Y$ApqgV2Pz>rb-iW*v4H@ZE< zV7Er+Tr_Ne?2|lthl>wZO=$M(S_9?t&*|CyLrEaGfoS%|IoFOzR07@94yj#gYV5`| z1GeD^O=m{2ZP;)9>gy;Fpp#9Iv>`CTqcsX9ypKR983?eSvw}Sneuc?ox}N>tu?I;; zQ&W?jWeCevi*zcOb>L%>guuwLG+~*8309z-tviqYF92nZYo=2?fJ<*z(HQEHVLVw= zN}Bi-=WeDVDQMzm=H+#%CdTcA@i)9;PN`<;?+zBR>66{_<4q1ChIRVDD{(=$&<=5pE~^{QawgJX5|MS;TW+0Da{C z{rgCjh4X=*$g}M%9li*3m&R!5i5J&5)k|wcH(zUw8G*n)ThCrpfdBA0oWAeS^we2- z(6DgjGUq*V_VwSo-X4+{PcOhH+mq;gea29vnwy)E7B6ZY&|Po;OL=#O+lfgYue$vf z!T_^$tU(h6A9PmG5#94O1v@}v40i7KN%qi|5M&nwL9g}s#XIO5`f*)A93l$kuky{C z(;Wq#?0S1>$UspvYrmCvu#!J@NFEe4P)XF2d#r72n9scx>rB_r^3_KsdJ2Lzc7l>W zq{@EUe53pwSu(NXZcz60dxqF}wB6OK18>kKte;RdW}W^k54b})udDo-vz=c&3C2Sz zUoH{lO-}liOuW0mUNbT=F#%Q8)5ixwLjK(K+Z`9N4Pn{&T*YXR?p@W`Px*m2bPB1_ zZH*sTRCc+sB(Er&G;t?jzRFT-Y$!Kix^JRjrdrKDaJu)3YN^bhz2@LE-9sR?Si)@X zMil@J`H-YwTFanz_Nkg00r3BAsHk{-V)B9sn0!cX&t?#1!n8P7WREnn{f1F z5BwhUI4i3Xrg(N6{6c{ab9?tWq)DNlXn-C~2{AfQ;MHT$xy?{Y7oMtt>AdGtbEao+ z?d|i{*3r;sG&}nuT{_hJ%SfD#D~3QEWupJ~^$M)~U*#$Ie))(6Uo0)E3rs0Z_&-1x zCiao2O_ll?#t{sAx()IJuGW7*-oZ8W5|9p;-$$tJaA~67nt*Ji0>Po+p6JEOXlt<8 z`+j}>tLqRhzQX2f${-wkrHijr2{8DNE)&MXrW&ipFH?R-31O&jtoP)o?X`aa6^c@p literal 0 HcmV?d00001