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