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