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