001/* 002 * Copyright (C) Photon Vision. 003 * 004 * This program is free software: you can redistribute it and/or modify 005 * it under the terms of the GNU General Public License as published by 006 * the Free Software Foundation, either version 3 of the License, or 007 * (at your option) any later version. 008 * 009 * This program is distributed in the hope that it will be useful, 010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 012 * GNU General Public License for more details. 013 * 014 * You should have received a copy of the GNU General Public License 015 * along with this program. If not, see <https://www.gnu.org/licenses/>. 016 */ 017 018package org.photonvision.vision.target; 019 020import com.fasterxml.jackson.annotation.JsonAlias; 021import com.fasterxml.jackson.annotation.JsonCreator; 022import com.fasterxml.jackson.annotation.JsonIgnore; 023import com.fasterxml.jackson.annotation.JsonProperty; 024import edu.wpi.first.math.util.Units; 025import java.util.ArrayList; 026import java.util.List; 027import org.opencv.core.MatOfPoint3f; 028import org.opencv.core.Point3; 029import org.photonvision.vision.opencv.Releasable; 030import org.photonvision.vision.pipe.impl.CornerDetectionPipe; 031import org.photonvision.vision.pipe.impl.SolvePNPPipe; 032 033/** 034 * A model representing the vertices of targets with known shapes. The vertices are in the EDN 035 * coordinate system. When creating a TargetModel, the vertices must be supplied in a certain order 036 * to ensure correct correspondence with corners detected in 2D for use with SolvePNP. For planar 037 * targets, we expect the target's Z-axis to point towards the camera. 038 * 039 * <p>{@link SolvePNPPipe} expects 3d object points to correspond to the {@link CornerDetectionPipe} 040 * implementation. The 2d corner detection finds the 4 extreme corners (bottom-left, bottom-right, 041 * top-right, top-left). To match our expectations, this means the model vertices would look like: 042 * 043 * <ul> 044 * <li>(+x, +y, 0) 045 * <li>(-x, +y, 0) 046 * <li>(-x, -y, 0) 047 * <li>(+x, -y, 0) 048 * </ul> 049 * 050 * <p>AprilTag models are currently only used for drawing on the output stream. 051 */ 052public enum TargetModel implements Releasable { 053 k2016HighGoal( 054 List.of( 055 new Point3(Units.inchesToMeters(10), Units.inchesToMeters(6), 0), 056 new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(6), 0), 057 new Point3(Units.inchesToMeters(-10), Units.inchesToMeters(-6), 0), 058 new Point3(Units.inchesToMeters(10), Units.inchesToMeters(-6), 0)), 059 Units.inchesToMeters(6)), 060 k2019DualTarget( 061 List.of( 062 new Point3(Units.inchesToMeters(7.313), Units.inchesToMeters(2.662), 0), 063 new Point3(Units.inchesToMeters(-7.313), Units.inchesToMeters(2.662), 0), 064 new Point3(Units.inchesToMeters(-5.936), Units.inchesToMeters(-2.662), 0), 065 new Point3(Units.inchesToMeters(5.936), Units.inchesToMeters(-2.662), 0)), 066 0.1), 067 k2020HighGoalOuter( 068 List.of( 069 new Point3(Units.inchesToMeters(9.819867), Units.inchesToMeters(8.5), 0), 070 new Point3(Units.inchesToMeters(-9.819867), Units.inchesToMeters(8.5), 0), 071 new Point3(Units.inchesToMeters(-19.625), Units.inchesToMeters(-8.5), 0), 072 new Point3(Units.inchesToMeters(19.625), Units.inchesToMeters(-8.5), 0)), 073 Units.inchesToMeters(12)), 074 kCircularPowerCell7in( 075 List.of( 076 new Point3( 077 -Units.inchesToMeters(7) / 2, 078 -Units.inchesToMeters(7) / 2, 079 -Units.inchesToMeters(7) / 2), 080 new Point3( 081 -Units.inchesToMeters(7) / 2, 082 Units.inchesToMeters(7) / 2, 083 -Units.inchesToMeters(7) / 2), 084 new Point3( 085 Units.inchesToMeters(7) / 2, 086 Units.inchesToMeters(7) / 2, 087 -Units.inchesToMeters(7) / 2), 088 new Point3( 089 Units.inchesToMeters(7) / 2, 090 -Units.inchesToMeters(7) / 2, 091 -Units.inchesToMeters(7) / 2)), 092 0), 093 k2022CircularCargoBall( 094 List.of( 095 new Point3( 096 -Units.inchesToMeters(9.5) / 2, 097 -Units.inchesToMeters(9.5) / 2, 098 -Units.inchesToMeters(9.5) / 2), 099 new Point3( 100 -Units.inchesToMeters(9.5) / 2, 101 Units.inchesToMeters(9.5) / 2, 102 -Units.inchesToMeters(9.5) / 2), 103 new Point3( 104 Units.inchesToMeters(9.5) / 2, 105 Units.inchesToMeters(9.5) / 2, 106 -Units.inchesToMeters(9.5) / 2), 107 new Point3( 108 Units.inchesToMeters(9.5) / 2, 109 -Units.inchesToMeters(9.5) / 2, 110 -Units.inchesToMeters(9.5) / 2)), 111 0), 112 k2025Algae( 113 List.of( 114 new Point3( 115 -Units.inchesToMeters(16.25) / 2, 116 -Units.inchesToMeters(16.25) / 2, 117 -Units.inchesToMeters(16.25) / 2), 118 new Point3( 119 -Units.inchesToMeters(16.25) / 2, 120 Units.inchesToMeters(16.25) / 2, 121 -Units.inchesToMeters(16.25) / 2), 122 new Point3( 123 Units.inchesToMeters(16.25) / 2, 124 Units.inchesToMeters(16.25) / 2, 125 -Units.inchesToMeters(16.25) / 2), 126 new Point3( 127 Units.inchesToMeters(16.25) / 2, 128 -Units.inchesToMeters(16.25) / 2, 129 -Units.inchesToMeters(16.25) / 2)), 130 0), 131 // 2023 AprilTag, with 6 inch marker width (inner black square). 132 @JsonAlias({"k6in_16h5"}) 133 kAprilTag6in_16h5( 134 // Corners of the tag's inner black square (excluding white border) 135 List.of( 136 new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0), 137 new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0), 138 new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0), 139 new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)), 140 Units.inchesToMeters(3 * 2)), 141 // 2024 AprilTag, with 6.5 inch marker width (inner black square). 142 @JsonAlias({"k6p5in_36h11", "k200mmAprilTag", "kAruco6p5in_36h11"}) 143 kAprilTag6p5in_36h11( 144 // Corners of the tag's inner black square (excluding white border) 145 List.of( 146 new Point3(-Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), 147 new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0), 148 new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0), 149 new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)), 150 Units.inchesToMeters(6.5)); 151 152 @JsonIgnore private MatOfPoint3f realWorldTargetCoordinates; 153 @JsonIgnore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); 154 @JsonIgnore private final MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); 155 156 @JsonProperty("realWorldCoordinatesArray") 157 private List<Point3> realWorldCoordinatesArray; 158 159 @JsonProperty("boxHeight") 160 private double boxHeight; 161 162 TargetModel() {} 163 164 TargetModel(MatOfPoint3f realWorldTargetCoordinates, double boxHeight) { 165 this.realWorldTargetCoordinates = realWorldTargetCoordinates; 166 this.realWorldCoordinatesArray = realWorldTargetCoordinates.toList(); 167 this.boxHeight = boxHeight; 168 169 var bottomList = realWorldTargetCoordinates.toList(); 170 var topList = new ArrayList<Point3>(); 171 for (var c : bottomList) { 172 topList.add(new Point3(c.x, c.y, c.z + boxHeight)); 173 } 174 175 this.visualizationBoxBottom.fromList(bottomList); 176 this.visualizationBoxTop.fromList(topList); 177 } 178 179 @JsonCreator 180 TargetModel( 181 @JsonProperty(value = "realWorldCoordinatesArray") List<Point3> points, 182 @JsonProperty(value = "boxHeight") double boxHeight) { 183 this(listToMat(points), boxHeight); 184 } 185 186 public List<Point3> getRealWorldCoordinatesArray() { 187 return this.realWorldCoordinatesArray; 188 } 189 190 public double getBoxHeight() { 191 return boxHeight; 192 } 193 194 public void setRealWorldCoordinatesArray(List<Point3> realWorldCoordinatesArray) { 195 this.realWorldCoordinatesArray = realWorldCoordinatesArray; 196 } 197 198 public void setBoxHeight(double boxHeight) { 199 this.boxHeight = boxHeight; 200 } 201 202 private static MatOfPoint3f listToMat(List<Point3> points) { 203 var mat = new MatOfPoint3f(); 204 mat.fromList(points); 205 return mat; 206 } 207 208 public MatOfPoint3f getRealWorldTargetCoordinates() { 209 return realWorldTargetCoordinates; 210 } 211 212 public MatOfPoint3f getVisualizationBoxBottom() { 213 return visualizationBoxBottom; 214 } 215 216 public MatOfPoint3f getVisualizationBoxTop() { 217 return visualizationBoxTop; 218 } 219 220 // public static TargetModel getCircleTarget(double Units.inchesToMeters(7)) { 221 // var corners = 222 // List.of( 223 // new Point3(-Units.inchesToMeters(7) / 2, -radius / 2, -radius / 2), 224 // new Point3(-Units.inchesToMeters(7) / 2, radius / 2, -radius / 2), 225 // new Point3(Units.inchesToMeters(7) / 2, radius / 2, -radius / 2), 226 // new Point3(Units.inchesToMeters(7) / 2, -radius / 2, -radius / 2)); 227 // return new TargetModel(corners, 0); 228 // } 229 230 @Override 231 public void release() { 232 realWorldTargetCoordinates.release(); 233 visualizationBoxBottom.release(); 234 visualizationBoxTop.release(); 235 } 236}