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;
006
007import edu.wpi.first.apriltag.jni.AprilTagJNI;
008import org.opencv.core.Mat;
009
010/**
011 * An AprilTag detector engine. This is expensive to set up and tear down, so most use cases should
012 * only create one of these, add a family to it, set up any other configuration, and repeatedly call
013 * Detect().
014 */
015public class AprilTagDetector implements AutoCloseable {
016  /** Detector configuration. */
017  @SuppressWarnings("MemberName")
018  public static class Config {
019    /**
020     * How many threads should be used for computation. Default is single-threaded operation (1
021     * thread).
022     */
023    public int numThreads = 1;
024
025    /**
026     * Quad decimation. Detection of quads can be done on a lower-resolution image, improving speed
027     * at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary
028     * payload is still done at full resolution. Default is 2.0.
029     */
030    public float quadDecimate = 2.0f;
031
032    /**
033     * What Gaussian blur should be applied to the segmented image (used for quad detection). Very
034     * noisy images benefit from non-zero values (e.g. 0.8). Default is 0.0.
035     */
036    public float quadSigma;
037
038    /**
039     * When true, the edges of the each quad are adjusted to "snap to" strong gradients nearby. This
040     * is useful when decimation is employed, as it can increase the quality of the initial quad
041     * estimate substantially. Generally recommended to be on (true). Default is true.
042     *
043     * <p>Very computationally inexpensive. Option is ignored if quad_decimate = 1.
044     */
045    public boolean refineEdges = true;
046
047    /**
048     * How much sharpening should be done to decoded images. This can help decode small tags but may
049     * or may not help in odd lighting conditions or low light conditions. Default is 0.25.
050     */
051    public double decodeSharpening = 0.25;
052
053    /**
054     * Debug mode. When true, the decoder writes a variety of debugging images to the current
055     * working directory at various stages through the detection process. This is slow and should
056     * *not* be used on space-limited systems such as the RoboRIO. Default is disabled (false).
057     */
058    public boolean debug;
059
060    /** Default constructor. */
061    public Config() {}
062
063    /**
064     * Constructs a detector configuration.
065     *
066     * @param numThreads How many threads should be used for computation.
067     * @param quadDecimate Quad decimation.
068     * @param quadSigma What Gaussian blur should be applied to the segmented image (used for quad
069     *     detection).
070     * @param refineEdges When true, the edges of the each quad are adjusted to "snap to" strong
071     *     gradients nearby.
072     * @param decodeSharpening How much sharpening should be done to decoded images.
073     * @param debug Debug mode.
074     */
075    Config(
076        int numThreads,
077        float quadDecimate,
078        float quadSigma,
079        boolean refineEdges,
080        double decodeSharpening,
081        boolean debug) {
082      this.numThreads = numThreads;
083      this.quadDecimate = quadDecimate;
084      this.quadSigma = quadSigma;
085      this.refineEdges = refineEdges;
086      this.decodeSharpening = decodeSharpening;
087      this.debug = debug;
088    }
089
090    @Override
091    public int hashCode() {
092      return numThreads
093          + Float.hashCode(quadDecimate)
094          + Float.hashCode(quadSigma)
095          + Boolean.hashCode(refineEdges)
096          + Double.hashCode(decodeSharpening)
097          + Boolean.hashCode(debug);
098    }
099
100    @Override
101    public boolean equals(Object obj) {
102      if (!(obj instanceof Config)) {
103        return false;
104      }
105
106      Config other = (Config) obj;
107      return numThreads == other.numThreads
108          && quadDecimate == other.quadDecimate
109          && quadSigma == other.quadSigma
110          && refineEdges == other.refineEdges
111          && decodeSharpening == other.decodeSharpening
112          && debug == other.debug;
113    }
114  }
115
116  /** Quad threshold parameters. */
117  @SuppressWarnings("MemberName")
118  public static class QuadThresholdParameters {
119    /** Threshold used to reject quads containing too few pixels. Default is 5 pixels. */
120    public int minClusterPixels = 5;
121
122    /**
123     * How many corner candidates to consider when segmenting a group of pixels into a quad. Default
124     * is 10.
125     */
126    public int maxNumMaxima = 10;
127
128    /**
129     * Critical angle, in radians. The detector will reject quads where pairs of edges have angles
130     * that are close to straight or close to 180 degrees. Zero means that no quads are rejected.
131     * Default is 10 degrees.
132     */
133    public double criticalAngle = 10 * Math.PI / 180.0;
134
135    /**
136     * When fitting lines to the contours, the maximum mean squared error allowed. This is useful in
137     * rejecting contours that are far from being quad shaped; rejecting these quads "early" saves
138     * expensive decoding processing. Default is 10.0.
139     */
140    public float maxLineFitMSE = 10.0f;
141
142    /**
143     * Minimum brightness offset. When we build our model of black &amp; white pixels, we add an
144     * extra check that the white model must be (overall) brighter than the black model. How much
145     * brighter? (in pixel values, [0,255]). Default is 5.
146     */
147    public int minWhiteBlackDiff = 5;
148
149    /**
150     * Whether the thresholded image be should be deglitched. Only useful for very noisy images.
151     * Default is disabled (false).
152     */
153    public boolean deglitch;
154
155    /** Default constructor. */
156    public QuadThresholdParameters() {}
157
158    /**
159     * Constructs quad threshold parameters.
160     *
161     * @param minClusterPixels Threshold used to reject quads containing too few pixels.
162     * @param maxNumMaxima How many corner candidates to consider when segmenting a group of pixels
163     *     into a quad.
164     * @param criticalAngle Critical angle, in radians.
165     * @param maxLineFitMSE When fitting lines to the contours, the maximum mean squared error
166     *     allowed.
167     * @param minWhiteBlackDiff Minimum brightness offset.
168     * @param deglitch Whether the thresholded image be should be deglitched.
169     */
170    QuadThresholdParameters(
171        int minClusterPixels,
172        int maxNumMaxima,
173        double criticalAngle,
174        float maxLineFitMSE,
175        int minWhiteBlackDiff,
176        boolean deglitch) {
177      this.minClusterPixels = minClusterPixels;
178      this.maxNumMaxima = maxNumMaxima;
179      this.criticalAngle = criticalAngle;
180      this.maxLineFitMSE = maxLineFitMSE;
181      this.minWhiteBlackDiff = minWhiteBlackDiff;
182      this.deglitch = deglitch;
183    }
184
185    @Override
186    public int hashCode() {
187      return minClusterPixels
188          + maxNumMaxima
189          + Double.hashCode(criticalAngle)
190          + Float.hashCode(maxLineFitMSE)
191          + minWhiteBlackDiff
192          + Boolean.hashCode(deglitch);
193    }
194
195    @Override
196    public boolean equals(Object obj) {
197      if (!(obj instanceof QuadThresholdParameters)) {
198        return false;
199      }
200
201      QuadThresholdParameters other = (QuadThresholdParameters) obj;
202      return minClusterPixels == other.minClusterPixels
203          && maxNumMaxima == other.maxNumMaxima
204          && criticalAngle == other.criticalAngle
205          && maxLineFitMSE == other.maxLineFitMSE
206          && minWhiteBlackDiff == other.minWhiteBlackDiff
207          && deglitch == other.deglitch;
208    }
209  }
210
211  /** Constructs an AprilTagDetector. */
212  public AprilTagDetector() {
213    m_native = AprilTagJNI.createDetector();
214  }
215
216  @Override
217  public void close() {
218    if (m_native != 0) {
219      AprilTagJNI.destroyDetector(m_native);
220    }
221    m_native = 0;
222  }
223
224  /**
225   * Sets detector configuration.
226   *
227   * @param config Configuration
228   */
229  public void setConfig(Config config) {
230    AprilTagJNI.setDetectorConfig(m_native, config);
231  }
232
233  /**
234   * Gets detector configuration.
235   *
236   * @return Configuration
237   */
238  public Config getConfig() {
239    return AprilTagJNI.getDetectorConfig(m_native);
240  }
241
242  /**
243   * Sets quad threshold parameters.
244   *
245   * @param params Parameters
246   */
247  public void setQuadThresholdParameters(QuadThresholdParameters params) {
248    AprilTagJNI.setDetectorQTP(m_native, params);
249  }
250
251  /**
252   * Gets quad threshold parameters.
253   *
254   * @return Parameters
255   */
256  public QuadThresholdParameters getQuadThresholdParameters() {
257    return AprilTagJNI.getDetectorQTP(m_native);
258  }
259
260  /**
261   * Adds a family of tags to be detected.
262   *
263   * @param fam Family name, e.g. "tag16h5"
264   * @throws IllegalArgumentException if family name not recognized
265   */
266  public void addFamily(String fam) {
267    addFamily(fam, 2);
268  }
269
270  /**
271   * Adds a family of tags to be detected.
272   *
273   * @param fam Family name, e.g. "tag16h5"
274   * @param bitsCorrected Maximum number of bits to correct
275   * @throws IllegalArgumentException if family name not recognized
276   */
277  public void addFamily(String fam, int bitsCorrected) {
278    if (!AprilTagJNI.addFamily(m_native, fam, bitsCorrected)) {
279      throw new IllegalArgumentException("unknown family name '" + fam + "'");
280    }
281  }
282
283  /**
284   * Removes a family of tags from the detector.
285   *
286   * @param fam Family name, e.g. "tag16h5"
287   */
288  public void removeFamily(String fam) {
289    AprilTagJNI.removeFamily(m_native, fam);
290  }
291
292  /** Unregister all families. */
293  public void clearFamilies() {
294    AprilTagJNI.clearFamilies(m_native);
295  }
296
297  /**
298   * Detect tags from an 8-bit image.
299   *
300   * <p>The image must be grayscale.
301   *
302   * @param img 8-bit OpenCV Mat image
303   * @return Results (array of AprilTagDetection)
304   */
305  public AprilTagDetection[] detect(Mat img) {
306    return AprilTagJNI.detect(m_native, img.cols(), img.rows(), img.cols(), img.dataAddr());
307  }
308
309  private long m_native;
310}