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