001/*
002 * MIT License
003 *
004 * Copyright (c) PhotonVision
005 *
006 * Permission is hereby granted, free of charge, to any person obtaining a copy
007 * of this software and associated documentation files (the "Software"), to deal
008 * in the Software without restriction, including without limitation the rights
009 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
010 * copies of the Software, and to permit persons to whom the Software is
011 * furnished to do so, subject to the following conditions:
012 *
013 * The above copyright notice and this permission notice shall be included in all
014 * copies or substantial portions of the Software.
015 *
016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
017 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
018 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
019 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
020 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
021 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
022 * SOFTWARE.
023 */
024
025package org.photonvision.simulation;
026
027import edu.wpi.first.math.geometry.Pose3d;
028import edu.wpi.first.math.geometry.Translation3d;
029import java.util.List;
030import org.photonvision.estimation.TargetModel;
031
032/** Describes a vision target located somewhere on the field that your vision system can detect. */
033public class VisionTargetSim {
034    private Pose3d pose;
035    private TargetModel model;
036
037    public final int fiducialID;
038
039    /** The object detection class ID, or -1 if not applicable. */
040    public final int objDetClassId;
041
042    /** The object detection confidence, or -1 if not applicable. */
043    public final float objDetConf;
044
045    /**
046     * Describes a retro-reflective/colored shape vision target located somewhere on the field that
047     * your vision system can detect.
048     *
049     * @param pose Pose3d of the tag in field-relative coordinates
050     * @param model TargetModel which describes the geometry of the target
051     */
052    public VisionTargetSim(Pose3d pose, TargetModel model) {
053        this(pose, model, -1, -1, -1);
054    }
055
056    /**
057     * Describes a fiducial tag located somewhere on the field that your vision system can detect.
058     *
059     * @param pose Pose3d of the tag in field-relative coordinates
060     * @param model TargetModel which describes the geometry of the target(tag)
061     * @param id The ID of this fiducial tag
062     */
063    public VisionTargetSim(Pose3d pose, TargetModel model, int id) {
064        this(pose, model, id, -1, -1);
065    }
066
067    /**
068     * Describes an object-detection vision target located somewhere on the field that your vision
069     * system can detect. Class ID is the (zero-indexed) index of the object's class ID in the list of
070     * all classes. Confidence can be specified, or pass -1 to estimate confidence based on 2 *
071     * sqrt(target area / total image area)
072     *
073     * @param pose Pose3d of the target in field-relative coordinates
074     * @param model TargetModel which describes the geometry of the target
075     * @param objDetClassId The object detection class ID, if -1 it will not be detected by object
076     *     detection
077     * @param objDetConf The object detection confidence, or -1 in which case the simulation will
078     *     compute a confidence based on the area of the target in the camera's field of view
079     */
080    public VisionTargetSim(Pose3d pose, TargetModel model, int objDetClassId, float objDetConf) {
081        this(pose, model, -1, objDetClassId, objDetConf);
082    }
083
084    private VisionTargetSim(
085            Pose3d pose, TargetModel model, int id, int objDetClassId, float objDetConf) {
086        this.pose = pose;
087        this.model = model;
088        this.fiducialID = id;
089        this.objDetClassId = objDetClassId;
090        this.objDetConf = objDetConf;
091    }
092
093    /**
094     * Sets the pose of this target on the field.
095     *
096     * @param pose The pose in field-relative coordinates
097     */
098    public void setPose(Pose3d pose) {
099        this.pose = pose;
100    }
101
102    /**
103     * Sets the model describing this target's geometry.
104     *
105     * @param model The model of the target
106     */
107    public void setModel(TargetModel model) {
108        this.model = model;
109    }
110
111    /**
112     * Returns the pose of this target on the field.
113     *
114     * @return The pose in field-relative coordinates
115     */
116    public Pose3d getPose() {
117        return pose;
118    }
119
120    /**
121     * Returns the model describing this target's geometry.
122     *
123     * @return The model of the target
124     */
125    public TargetModel getModel() {
126        return model;
127    }
128
129    /**
130     * This target's vertices offset from its field pose.
131     *
132     * @return A vector of Translation3d representing the vertices of the target
133     */
134    public List<Translation3d> getFieldVertices() {
135        return model.getFieldVertices(pose);
136    }
137
138    @Override
139    public boolean equals(Object obj) {
140        return this == obj
141                && obj instanceof VisionTargetSim o
142                && pose.equals(o.pose)
143                && model.equals(o.model);
144    }
145}