001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.apriltag.jni;
006
007import edu.wpi.first.apriltag.AprilTagDetection;
008import edu.wpi.first.apriltag.AprilTagDetector;
009import edu.wpi.first.apriltag.AprilTagPoseEstimate;
010import edu.wpi.first.math.geometry.Transform3d;
011import edu.wpi.first.util.RawFrame;
012import edu.wpi.first.util.RuntimeLoader;
013import java.io.IOException;
014import java.util.concurrent.atomic.AtomicBoolean;
015
016/** AprilTag JNI. */
017public class AprilTagJNI {
018  static boolean libraryLoaded = false;
019
020  /** Sets whether JNI should be loaded in the static block. */
021  public static class Helper {
022    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
023
024    /**
025     * Returns true if the JNI should be loaded in the static block.
026     *
027     * @return True if the JNI should be loaded in the static block.
028     */
029    public static boolean getExtractOnStaticLoad() {
030      return extractOnStaticLoad.get();
031    }
032
033    /**
034     * Sets whether the JNI should be loaded in the static block.
035     *
036     * @param load Whether the JNI should be loaded in the static block.
037     */
038    public static void setExtractOnStaticLoad(boolean load) {
039      extractOnStaticLoad.set(load);
040    }
041
042    /** Utility class. */
043    private Helper() {}
044  }
045
046  static {
047    if (Helper.getExtractOnStaticLoad()) {
048      try {
049        RuntimeLoader.loadLibrary("apriltagjni");
050      } catch (IOException ex) {
051        ex.printStackTrace();
052        System.exit(1);
053      }
054      libraryLoaded = true;
055    }
056  }
057
058  /**
059   * Constructs an AprilTag detector engine.
060   *
061   * @return The detector engine handle
062   */
063  public static native long createDetector();
064
065  /**
066   * Destroys an AprilTag detector engine.
067   *
068   * @param det The detector engine handle
069   */
070  public static native void destroyDetector(long det);
071
072  /**
073   * Sets the detector engine configuration.
074   *
075   * @param det The detector engine handle
076   * @param config A configuration
077   */
078  public static native void setDetectorConfig(long det, AprilTagDetector.Config config);
079
080  /**
081   * Gets the detector engine configuration.
082   *
083   * @param det The detector engine handle
084   * @return The configuration
085   */
086  public static native AprilTagDetector.Config getDetectorConfig(long det);
087
088  /**
089   * Sets the detector engine quad threshold parameters.
090   *
091   * @param det The detector engine handle
092   * @param params Quad threshold parameters
093   */
094  public static native void setDetectorQTP(
095      long det, AprilTagDetector.QuadThresholdParameters params);
096
097  /**
098   * Gets the detector engine quad threshold parameters.
099   *
100   * @param det The detector engine handle
101   * @return Quad threshold parameters
102   */
103  public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det);
104
105  /**
106   * Adds a family of tags to be detected by the detector engine.
107   *
108   * @param det The detector engine handle
109   * @param fam Family name, e.g. "tag16h5"
110   * @param bitsCorrected Maximum number of bits to correct
111   * @return False if family can't be found
112   */
113  public static native boolean addFamily(long det, String fam, int bitsCorrected);
114
115  /**
116   * Removes a family of tags from the detector.
117   *
118   * @param det The detector engine handle
119   * @param fam Family name, e.g. "tag16h5"
120   */
121  public static native void removeFamily(long det, String fam);
122
123  /**
124   * Unregister all families.
125   *
126   * @param det The detector engine handle
127   */
128  public static native void clearFamilies(long det);
129
130  /**
131   * Detect tags from an 8-bit image.
132   *
133   * @param det The detector engine handle
134   * @param width The width of the image
135   * @param height The height of the image
136   * @param stride The number of bytes between image rows (often the same as width)
137   * @param bufAddr The address of the image buffer
138   * @return The results (array of AprilTagDetection)
139   */
140  public static native AprilTagDetection[] detect(
141      long det, int width, int height, int stride, long bufAddr);
142
143  /**
144   * Estimates the pose of the tag using the homography method described in [1].
145   *
146   * @param homography Homography 3x3 matrix data
147   * @param tagSize The tag size, in meters
148   * @param fx The camera horizontal focal length, in pixels
149   * @param fy The camera vertical focal length, in pixels
150   * @param cx The camera horizontal focal center, in pixels
151   * @param cy The camera vertical focal center, in pixels
152   * @return Pose estimate
153   */
154  public static native Transform3d estimatePoseHomography(
155      double[] homography, double tagSize, double fx, double fy, double cx, double cy);
156
157  /**
158   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
159   * the object-space error of each.
160   *
161   * @param homography Homography 3x3 matrix data
162   * @param corners Corner point array (X and Y for each corner in order)
163   * @param tagSize The tag size, in meters
164   * @param fx The camera horizontal focal length, in pixels
165   * @param fy The camera vertical focal length, in pixels
166   * @param cx The camera horizontal focal center, in pixels
167   * @param cy The camera vertical focal center, in pixels
168   * @param nIters Number of iterations
169   * @return Initial and (possibly) second pose estimates
170   */
171  public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
172      double[] homography,
173      double[] corners,
174      double tagSize,
175      double fx,
176      double fy,
177      double cx,
178      double cy,
179      int nIters);
180
181  /**
182   * Estimates tag pose. This method is an easier to use interface to
183   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
184   * object-space error.
185   *
186   * @param homography Homography 3x3 matrix data
187   * @param corners Corner point array (X and Y for each corner in order)
188   * @param tagSize The tag size, in meters
189   * @param fx The camera horizontal focal length, in pixels
190   * @param fy The camera vertical focal length, in pixels
191   * @param cx The camera horizontal focal center, in pixels
192   * @param cy The camera vertical focal center, in pixels
193   * @return Pose estimate
194   */
195  public static native Transform3d estimatePose(
196      double[] homography,
197      double[] corners,
198      double tagSize,
199      double fx,
200      double fy,
201      double cx,
202      double cy);
203
204  /**
205   * Generates a RawFrame containing the apriltag with the id with family 16h5 passed in.
206   *
207   * @param frameObj generated frame (output parameter).
208   * @param frame raw frame handle
209   * @param id id
210   */
211  public static native void generate16h5AprilTagImage(RawFrame frameObj, long frame, int id);
212
213  /**
214   * Generates a RawFrame containing the apriltag with the id with family 36h11 passed in.
215   *
216   * @param frameObj generated frame (output parameter).
217   * @param frame raw frame handle
218   * @param id id
219   */
220  public static native void generate36h11AprilTagImage(RawFrame frameObj, long frame, int id);
221
222  /** Utility class. */
223  private AprilTagJNI() {}
224}