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 edu.wpi.first.math.geometry.Transform3d; 009 010/** Pose estimators for AprilTag tags. */ 011public class AprilTagPoseEstimator { 012 /** Configuration for the pose estimator. */ 013 @SuppressWarnings("MemberName") 014 public static class Config { 015 /** 016 * Creates a pose estimator configuration. 017 * 018 * @param tagSize tag size, in meters 019 * @param fx camera horizontal focal length, in pixels 020 * @param fy camera vertical focal length, in pixels 021 * @param cx camera horizontal focal center, in pixels 022 * @param cy camera vertical focal center, in pixels 023 */ 024 public Config(double tagSize, double fx, double fy, double cx, double cy) { 025 this.tagSize = tagSize; 026 this.fx = fx; 027 this.fy = fy; 028 this.cx = cx; 029 this.cy = cy; 030 } 031 032 /** Tag size, in meters. */ 033 public double tagSize; 034 035 /** Camera horizontal focal length, in pixels. */ 036 public double fx; 037 038 /** Camera vertical focal length, in pixels. */ 039 public double fy; 040 041 /** Camera horizontal focal center, in pixels. */ 042 public double cx; 043 044 /** Camera vertical focal center, in pixels. */ 045 public double cy; 046 047 @Override 048 public int hashCode() { 049 return Double.hashCode(tagSize) 050 + Double.hashCode(fx) 051 + Double.hashCode(fy) 052 + Double.hashCode(cx) 053 + Double.hashCode(cy); 054 } 055 056 @Override 057 public boolean equals(Object obj) { 058 if (!(obj instanceof Config)) { 059 return false; 060 } 061 062 Config other = (Config) obj; 063 return tagSize == other.tagSize 064 && fx == other.fx 065 && fy == other.fy 066 && cx == other.cx 067 && cy == other.cy; 068 } 069 } 070 071 /** 072 * Creates estimator. 073 * 074 * @param config Configuration 075 */ 076 public AprilTagPoseEstimator(Config config) { 077 m_config = new Config(config.tagSize, config.fx, config.fy, config.cx, config.cy); 078 } 079 080 /** 081 * Sets estimator configuration. 082 * 083 * @param config Configuration 084 */ 085 public void setConfig(Config config) { 086 m_config.tagSize = config.tagSize; 087 m_config.fx = config.fx; 088 m_config.fy = config.fy; 089 m_config.cx = config.cx; 090 m_config.cy = config.cy; 091 } 092 093 /** 094 * Gets estimator configuration. 095 * 096 * @return Configuration 097 */ 098 public Config getConfig() { 099 return new Config(m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy); 100 } 101 102 /** 103 * Estimates the pose of the tag using the homography method described in [1]. 104 * 105 * @param detection Tag detection 106 * @return Pose estimate 107 */ 108 public Transform3d estimateHomography(AprilTagDetection detection) { 109 return estimateHomography(detection.getHomography()); 110 } 111 112 /** 113 * Estimates the pose of the tag using the homography method described in [1]. 114 * 115 * @param homography Homography 3x3 matrix data 116 * @return Pose estimate 117 */ 118 public Transform3d estimateHomography(double[] homography) { 119 return AprilTagJNI.estimatePoseHomography( 120 homography, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy); 121 } 122 123 /** 124 * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with 125 * the object-space error of each. 126 * 127 * <p>This uses the homography method described in [1] for the initial estimate. Then Orthogonal 128 * Iteration [2] is used to refine this estimate. Then [3] is used to find a potential second 129 * local minima and Orthogonal Iteration is used to refine this second estimate. 130 * 131 * <p>[1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in 2011 IEEE 132 * International Conference on Robotics and Automation, May 2011, pp. 3400–3407. 133 * 134 * <p>[2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose estimation from 135 * video images," in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no. 136 * 6, pp. 610-622, June 2000. doi: 10.1109/34.862199 137 * 138 * <p>[3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar Target," in IEEE 139 * Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 12, pp. 2024-2030, Dec. 140 * 2006. doi: 10.1109/TPAMI.2006.252 141 * 142 * @param detection Tag detection 143 * @param nIters Number of iterations 144 * @return Initial and (possibly) second pose estimates 145 */ 146 public AprilTagPoseEstimate estimateOrthogonalIteration(AprilTagDetection detection, int nIters) { 147 return estimateOrthogonalIteration(detection.getHomography(), detection.getCorners(), nIters); 148 } 149 150 /** 151 * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with 152 * the object-space error of each. 153 * 154 * @param homography Homography 3x3 matrix data 155 * @param corners Corner point array (X and Y for each corner in order) 156 * @param nIters Number of iterations 157 * @return Initial and (possibly) second pose estimates 158 */ 159 public AprilTagPoseEstimate estimateOrthogonalIteration( 160 double[] homography, double[] corners, int nIters) { 161 return AprilTagJNI.estimatePoseOrthogonalIteration( 162 homography, 163 corners, 164 m_config.tagSize, 165 m_config.fx, 166 m_config.fy, 167 m_config.cx, 168 m_config.cy, 169 nIters); 170 } 171 172 /** 173 * Estimates tag pose. This method is an easier to use interface to 174 * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower 175 * object-space error. 176 * 177 * @param detection Tag detection 178 * @return Pose estimate 179 */ 180 public Transform3d estimate(AprilTagDetection detection) { 181 return estimate(detection.getHomography(), detection.getCorners()); 182 } 183 184 /** 185 * Estimates tag pose. This method is an easier to use interface to 186 * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower 187 * object-space error. 188 * 189 * @param homography Homography 3x3 matrix data 190 * @param corners Corner point array (X and Y for each corner in order) 191 * @return Pose estimate 192 */ 193 public Transform3d estimate(double[] homography, double[] corners) { 194 return AprilTagJNI.estimatePose( 195 homography, corners, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy); 196 } 197 198 private final Config m_config; 199}