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.targeting;
019
020import edu.wpi.first.math.geometry.Transform3d;
021import edu.wpi.first.util.protobuf.ProtobufSerializable;
022import java.util.List;
023import org.photonvision.common.dataflow.structures.PacketSerde;
024import org.photonvision.struct.PhotonTrackedTargetSerde;
025import org.photonvision.targeting.proto.PhotonTrackedTargetProto;
026import org.photonvision.targeting.serde.PhotonStructSerializable;
027
028public class PhotonTrackedTarget
029        implements ProtobufSerializable, PhotonStructSerializable<PhotonTrackedTarget> {
030    private static final int MAX_CORNERS = 8;
031
032    public double yaw;
033    public double pitch;
034    public double area;
035    public double skew;
036    public int fiducialId;
037    public int objDetectId;
038    public float objDetectConf;
039    public Transform3d bestCameraToTarget;
040    public Transform3d altCameraToTarget;
041    public double poseAmbiguity;
042
043    // Corners from the min-area rectangle bounding the target
044    public List<TargetCorner> minAreaRectCorners;
045
046    // Corners from whatever corner detection method was used
047    public List<TargetCorner> detectedCorners;
048
049    /** Construct a tracked target, given exactly 4 corners */
050    public PhotonTrackedTarget(
051            double yaw,
052            double pitch,
053            double area,
054            double skew,
055            int fiducialId,
056            int classId,
057            float objDetectConf,
058            Transform3d pose,
059            Transform3d altPose,
060            double ambiguity,
061            List<TargetCorner> minAreaRectCorners,
062            List<TargetCorner> detectedCorners) {
063        assert minAreaRectCorners.size() == 4;
064
065        if (detectedCorners.size() > MAX_CORNERS) {
066            detectedCorners = detectedCorners.subList(0, MAX_CORNERS);
067        }
068
069        this.yaw = yaw;
070        this.pitch = pitch;
071        this.area = area;
072        this.skew = skew;
073        this.fiducialId = fiducialId;
074        this.objDetectId = classId;
075        this.objDetectConf = objDetectConf;
076        this.bestCameraToTarget = pose;
077        this.altCameraToTarget = altPose;
078        this.minAreaRectCorners = minAreaRectCorners;
079        this.detectedCorners = detectedCorners;
080        this.poseAmbiguity = ambiguity;
081    }
082
083    public PhotonTrackedTarget() {
084        // TODO Auto-generated constructor stub
085    }
086
087    public double getYaw() {
088        return yaw;
089    }
090
091    public double getPitch() {
092        return pitch;
093    }
094
095    public double getArea() {
096        return area;
097    }
098
099    public double getSkew() {
100        return skew;
101    }
102
103    /** Get the fiducial ID, or -1 if not set. */
104    public int getFiducialId() {
105        return fiducialId;
106    }
107
108    /** Get the object detection class ID number, or -1 if not set. */
109    public int getDetectedObjectClassID() {
110        return objDetectId;
111    }
112
113    /**
114     * Get the object detection confidence, or -1 if not set. This will be between 0 and 1, with 1
115     * indicating most confidence, and 0 least.
116     */
117    public float getDetectedObjectConfidence() {
118        return objDetectConf;
119    }
120
121    /**
122     * Get the ratio of best:alternate pose reprojection errors, called ambiguity. This is between 0
123     * and 1 (0 being no ambiguity, and 1 meaning both have the same reprojection error). Numbers
124     * above 0.2 are likely to be ambiguous. -1 if invalid.
125     */
126    public double getPoseAmbiguity() {
127        return poseAmbiguity;
128    }
129
130    /**
131     * Return a list of the 4 corners in image space (origin top left, x right, y down), in no
132     * particular order, of the minimum area bounding rectangle of this target
133     */
134    public List<TargetCorner> getMinAreaRectCorners() {
135        return minAreaRectCorners;
136    }
137
138    /**
139     * Return a list of the n corners in image space (origin top left, x right, y down), in no
140     * particular order, detected for this target.
141     *
142     * <p>For fiducials, the order is known and is always counter-clock wise around the tag, like so:
143     *
144     * <pre>​
145     * ⟶ +X  3 ----- 2
146     * |      |       |
147     * V      |       |
148     * +Y     0 ----- 1
149     * </pre>
150     */
151    public List<TargetCorner> getDetectedCorners() {
152        return detectedCorners;
153    }
154
155    /**
156     * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag
157     * space (X forward, Y left, Z up) with the lowest reprojection error
158     */
159    public Transform3d getBestCameraToTarget() {
160        return bestCameraToTarget;
161    }
162
163    /**
164     * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag
165     * space (X forward, Y left, Z up) with the highest reprojection error
166     */
167    public Transform3d getAlternateCameraToTarget() {
168        return altCameraToTarget;
169    }
170
171    @Override
172    public int hashCode() {
173        final int prime = 31;
174        int result = 1;
175        long temp;
176        temp = Double.doubleToLongBits(yaw);
177        result = prime * result + (int) (temp ^ (temp >>> 32));
178        temp = Double.doubleToLongBits(pitch);
179        result = prime * result + (int) (temp ^ (temp >>> 32));
180        temp = Double.doubleToLongBits(area);
181        result = prime * result + (int) (temp ^ (temp >>> 32));
182        temp = Double.doubleToLongBits(skew);
183        result = prime * result + (int) (temp ^ (temp >>> 32));
184        result = prime * result + fiducialId;
185        result = prime * result + objDetectId;
186        result = prime * result + Float.floatToIntBits(objDetectConf);
187        result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode());
188        result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode());
189        temp = Double.doubleToLongBits(poseAmbiguity);
190        result = prime * result + (int) (temp ^ (temp >>> 32));
191        result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode());
192        result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode());
193        return result;
194    }
195
196    @Override
197    public boolean equals(Object obj) {
198        if (this == obj) return true;
199        if (obj == null) return false;
200        if (getClass() != obj.getClass()) return false;
201        PhotonTrackedTarget other = (PhotonTrackedTarget) obj;
202        if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false;
203        if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false;
204        if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false;
205        if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false;
206        if (fiducialId != other.fiducialId) return false;
207        if (objDetectId != other.objDetectId) return false;
208        if (Float.floatToIntBits(objDetectConf) != Float.floatToIntBits(other.objDetectConf))
209            return false;
210        if (bestCameraToTarget == null) {
211            if (other.bestCameraToTarget != null) return false;
212        } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false;
213        if (altCameraToTarget == null) {
214            if (other.altCameraToTarget != null) return false;
215        } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false;
216        if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity))
217            return false;
218        if (minAreaRectCorners == null) {
219            if (other.minAreaRectCorners != null) return false;
220        } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false;
221        if (detectedCorners == null) {
222            if (other.detectedCorners != null) return false;
223        } else if (!detectedCorners.equals(other.detectedCorners)) return false;
224        return true;
225    }
226
227    @Override
228    public String toString() {
229        return "PhotonTrackedTarget [yaw="
230                + yaw
231                + ", pitch="
232                + pitch
233                + ", area="
234                + area
235                + ", skew="
236                + skew
237                + ", fiducialId="
238                + fiducialId
239                + ", objDetectId="
240                + objDetectId
241                + ", objDetectConf="
242                + objDetectConf
243                + ", bestCameraToTarget="
244                + bestCameraToTarget
245                + ", altCameraToTarget="
246                + altCameraToTarget
247                + ", poseAmbiguity="
248                + poseAmbiguity
249                + ", minAreaRectCorners="
250                + minAreaRectCorners
251                + ", detectedCorners="
252                + detectedCorners
253                + "]";
254    }
255
256    public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto();
257    public static final PhotonTrackedTargetSerde photonStruct = new PhotonTrackedTargetSerde();
258
259    @Override
260    public PacketSerde<PhotonTrackedTarget> getSerde() {
261        return photonStruct;
262    }
263}