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    public Config() {}
061
062    Config(
063        int numThreads,
064        float quadDecimate,
065        float quadSigma,
066        boolean refineEdges,
067        double decodeSharpening,
068        boolean debug) {
069      this.numThreads = numThreads;
070      this.quadDecimate = quadDecimate;
071      this.quadSigma = quadSigma;
072      this.refineEdges = refineEdges;
073      this.decodeSharpening = decodeSharpening;
074      this.debug = debug;
075    }
076
077    @Override
078    public int hashCode() {
079      return numThreads
080          + Float.hashCode(quadDecimate)
081          + Float.hashCode(quadSigma)
082          + Boolean.hashCode(refineEdges)
083          + Double.hashCode(decodeSharpening)
084          + Boolean.hashCode(debug);
085    }
086
087    @Override
088    public boolean equals(Object obj) {
089      if (!(obj instanceof Config)) {
090        return false;
091      }
092
093      Config other = (Config) obj;
094      return numThreads == other.numThreads
095          && quadDecimate == other.quadDecimate
096          && quadSigma == other.quadSigma
097          && refineEdges == other.refineEdges
098          && decodeSharpening == other.decodeSharpening
099          && debug == other.debug;
100    }
101  }
102
103  /** Quad threshold parameters. */
104  @SuppressWarnings("MemberName")
105  public static class QuadThresholdParameters {
106    /** Threshold used to reject quads containing too few pixels. Default is 5 pixels. */
107    public int minClusterPixels = 5;
108
109    /**
110     * How many corner candidates to consider when segmenting a group of pixels into a quad. Default
111     * is 10.
112     */
113    public int maxNumMaxima = 10;
114
115    /**
116     * Critical angle, in radians. The detector will reject quads where pairs of edges have angles
117     * that are close to straight or close to 180 degrees. Zero means that no quads are rejected.
118     * Default is 10 degrees.
119     */
120    public double criticalAngle = 10 * Math.PI / 180.0;
121
122    /**
123     * When fitting lines to the contours, the maximum mean squared error allowed. This is useful in
124     * rejecting contours that are far from being quad shaped; rejecting these quads "early" saves
125     * expensive decoding processing. Default is 10.0.
126     */
127    public float maxLineFitMSE = 10.0f;
128
129    /**
130     * Minimum brightness offset. When we build our model of black &amp; white pixels, we add an
131     * extra check that the white model must be (overall) brighter than the black model. How much
132     * brighter? (in pixel values, [0,255]). Default is 5.
133     */
134    public int minWhiteBlackDiff = 5;
135
136    /**
137     * Whether the thresholded image be should be deglitched. Only useful for very noisy images.
138     * Default is disabled (false).
139     */
140    public boolean deglitch;
141
142    public QuadThresholdParameters() {}
143
144    QuadThresholdParameters(
145        int minClusterPixels,
146        int maxNumMaxima,
147        double criticalAngle,
148        float maxLineFitMSE,
149        int minWhiteBlackDiff,
150        boolean deglitch) {
151      this.minClusterPixels = minClusterPixels;
152      this.maxNumMaxima = maxNumMaxima;
153      this.criticalAngle = criticalAngle;
154      this.maxLineFitMSE = maxLineFitMSE;
155      this.minWhiteBlackDiff = minWhiteBlackDiff;
156      this.deglitch = deglitch;
157    }
158
159    @Override
160    public int hashCode() {
161      return minClusterPixels
162          + maxNumMaxima
163          + Double.hashCode(criticalAngle)
164          + Float.hashCode(maxLineFitMSE)
165          + minWhiteBlackDiff
166          + Boolean.hashCode(deglitch);
167    }
168
169    @Override
170    public boolean equals(Object obj) {
171      if (!(obj instanceof QuadThresholdParameters)) {
172        return false;
173      }
174
175      QuadThresholdParameters other = (QuadThresholdParameters) obj;
176      return minClusterPixels == other.minClusterPixels
177          && maxNumMaxima == other.maxNumMaxima
178          && criticalAngle == other.criticalAngle
179          && maxLineFitMSE == other.maxLineFitMSE
180          && minWhiteBlackDiff == other.minWhiteBlackDiff
181          && deglitch == other.deglitch;
182    }
183  }
184
185  public AprilTagDetector() {
186    m_native = AprilTagJNI.createDetector();
187  }
188
189  @Override
190  public void close() {
191    if (m_native != 0) {
192      AprilTagJNI.destroyDetector(m_native);
193    }
194    m_native = 0;
195  }
196
197  /**
198   * Sets detector configuration.
199   *
200   * @param config Configuration
201   */
202  public void setConfig(Config config) {
203    AprilTagJNI.setDetectorConfig(m_native, config);
204  }
205
206  /**
207   * Gets detector configuration.
208   *
209   * @return Configuration
210   */
211  public Config getConfig() {
212    return AprilTagJNI.getDetectorConfig(m_native);
213  }
214
215  /**
216   * Sets quad threshold parameters.
217   *
218   * @param params Parameters
219   */
220  public void setQuadThresholdParameters(QuadThresholdParameters params) {
221    AprilTagJNI.setDetectorQTP(m_native, params);
222  }
223
224  /**
225   * Gets quad threshold parameters.
226   *
227   * @return Parameters
228   */
229  public QuadThresholdParameters getQuadThresholdParameters() {
230    return AprilTagJNI.getDetectorQTP(m_native);
231  }
232
233  /**
234   * Adds a family of tags to be detected.
235   *
236   * @param fam Family name, e.g. "tag16h5"
237   * @throws IllegalArgumentException if family name not recognized
238   */
239  public void addFamily(String fam) {
240    addFamily(fam, 2);
241  }
242
243  /**
244   * Adds a family of tags to be detected.
245   *
246   * @param fam Family name, e.g. "tag16h5"
247   * @param bitsCorrected maximum number of bits to correct
248   * @throws IllegalArgumentException if family name not recognized
249   */
250  public void addFamily(String fam, int bitsCorrected) {
251    if (!AprilTagJNI.addFamily(m_native, fam, bitsCorrected)) {
252      throw new IllegalArgumentException("unknown family name '" + fam + "'");
253    }
254  }
255
256  /**
257   * Removes a family of tags from the detector.
258   *
259   * @param fam Family name, e.g. "tag16h5"
260   */
261  public void removeFamily(String fam) {
262    AprilTagJNI.removeFamily(m_native, fam);
263  }
264
265  /** Unregister all families. */
266  public void clearFamilies() {
267    AprilTagJNI.clearFamilies(m_native);
268  }
269
270  /**
271   * Detect tags from an 8-bit image.
272   *
273   * @param img 8-bit OpenCV Mat image
274   * @return Results (array of AprilTagDetection)
275   */
276  public AprilTagDetection[] detect(Mat img) {
277    return AprilTagJNI.detect(m_native, img.cols(), img.rows(), img.cols(), img.dataAddr());
278  }
279
280  private long m_native;
281}