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.math.MatBuilder;
008import edu.wpi.first.math.Matrix;
009import edu.wpi.first.math.Nat;
010import edu.wpi.first.math.numbers.N3;
011import java.util.Arrays;
012
013/** A detection of an AprilTag tag. */
014public class AprilTagDetection {
015  /**
016   * Gets the decoded tag's family name.
017   *
018   * @return Decoded family name
019   */
020  public String getFamily() {
021    return m_family;
022  }
023
024  /**
025   * Gets the decoded ID of the tag.
026   *
027   * @return Decoded ID
028   */
029  public int getId() {
030    return m_id;
031  }
032
033  /**
034   * Gets how many error bits were corrected. Note: accepting large numbers of corrected errors
035   * leads to greatly increased false positive rates. NOTE: As of this implementation, the detector
036   * cannot detect tags with a hamming distance greater than 2.
037   *
038   * @return Hamming distance (number of corrected error bits)
039   */
040  public int getHamming() {
041    return m_hamming;
042  }
043
044  /**
045   * Gets a measure of the quality of the binary decoding process: the average difference between
046   * the intensity of a data bit versus the decision threshold. Higher numbers roughly indicate
047   * better decodes. This is a reasonable measure of detection accuracy only for very small tags--
048   * not effective for larger tags (where we could have sampled anywhere within a bit cell and still
049   * gotten a good detection.)
050   *
051   * @return Decision margin
052   */
053  public float getDecisionMargin() {
054    return m_decisionMargin;
055  }
056
057  /**
058   * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at
059   * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image.
060   *
061   * @return Homography matrix data
062   */
063  @SuppressWarnings("PMD.MethodReturnsInternalArray")
064  public double[] getHomography() {
065    return m_homography;
066  }
067
068  /**
069   * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at
070   * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image.
071   *
072   * @return Homography matrix
073   */
074  public Matrix<N3, N3> getHomographyMatrix() {
075    return MatBuilder.fill(Nat.N3(), Nat.N3(), m_homography);
076  }
077
078  /**
079   * Gets the center of the detection in image pixel coordinates.
080   *
081   * @return Center point X coordinate
082   */
083  public double getCenterX() {
084    return m_centerX;
085  }
086
087  /**
088   * Gets the center of the detection in image pixel coordinates.
089   *
090   * @return Center point Y coordinate
091   */
092  public double getCenterY() {
093    return m_centerY;
094  }
095
096  /**
097   * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
098   * around the tag. Index 0 is the bottom left corner.
099   *
100   * @param ndx Corner index (range is 0-3, inclusive)
101   * @return Corner point X coordinate
102   */
103  public double getCornerX(int ndx) {
104    return m_corners[ndx * 2];
105  }
106
107  /**
108   * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
109   * around the tag. Index 0 is the bottom left corner.
110   *
111   * @param ndx Corner index (range is 0-3, inclusive)
112   * @return Corner point Y coordinate
113   */
114  public double getCornerY(int ndx) {
115    return m_corners[ndx * 2 + 1];
116  }
117
118  /**
119   * Gets the corners of the tag in image pixel coordinates. These always wrap counter-clock wise
120   * around the tag. The first set of corner coordinates are the coordinates for the bottom left
121   * corner.
122   *
123   * @return Corner point array (X and Y for each corner in order)
124   */
125  @SuppressWarnings("PMD.MethodReturnsInternalArray")
126  public double[] getCorners() {
127    return m_corners;
128  }
129
130  private final String m_family;
131  private final int m_id;
132  private final int m_hamming;
133  private final float m_decisionMargin;
134  private final double[] m_homography;
135  private final double m_centerX;
136  private final double m_centerY;
137  private final double[] m_corners;
138
139  /**
140   * Constructs a new detection result. Used from JNI.
141   *
142   * @param family family
143   * @param id id
144   * @param hamming hamming
145   * @param decisionMargin dm
146   * @param homography homography
147   * @param centerX centerX
148   * @param centerY centerY
149   * @param corners corners
150   */
151  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
152  public AprilTagDetection(
153      String family,
154      int id,
155      int hamming,
156      float decisionMargin,
157      double[] homography,
158      double centerX,
159      double centerY,
160      double[] corners) {
161    m_family = family;
162    m_id = id;
163    m_hamming = hamming;
164    m_decisionMargin = decisionMargin;
165    m_homography = homography;
166    m_centerX = centerX;
167    m_centerY = centerY;
168    m_corners = corners;
169  }
170
171  @Override
172  public String toString() {
173    return "DetectionResult [centerX="
174        + m_centerX
175        + ", centerY="
176        + m_centerY
177        + ", corners="
178        + Arrays.toString(m_corners)
179        + ", decisionMargin="
180        + m_decisionMargin
181        + ", hamming="
182        + m_hamming
183        + ", homography="
184        + Arrays.toString(m_homography)
185        + ", family="
186        + m_family
187        + ", id="
188        + m_id
189        + "]";
190  }
191}