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 & 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}