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 org.photonvision.common.dataflow.structures.PacketSerde;
023import org.photonvision.struct.PnpResultSerde;
024import org.photonvision.targeting.proto.PNPResultProto;
025import org.photonvision.targeting.serde.PhotonStructSerializable;
026
027/**
028 * The best estimated transformation from solvePnP, and possibly an alternate transformation
029 * depending on the solvePNP method. If an alternate solution is present, the ambiguity value
030 * represents the ratio of reprojection error in the best solution to the alternate (best /
031 * alternate).
032 *
033 * <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
034 * method.
035 */
036public class PnpResult implements ProtobufSerializable, PhotonStructSerializable<PnpResult> {
037    /**
038     * The best-fit transform. The coordinate frame of this transform depends on the method which gave
039     * this result.
040     */
041    public Transform3d best;
042
043    /** Reprojection error of the best solution, in pixels */
044    public double bestReprojErr;
045
046    /**
047     * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
048     * to the best solution.
049     */
050    public Transform3d alt;
051
052    /** If no alternate solution is found, this is bestReprojErr */
053    public double altReprojErr;
054
055    /** If no alternate solution is found, this is 0 */
056    public double ambiguity;
057
058    /** An empty (invalid) result. */
059    public PnpResult() {
060        this.best = new Transform3d();
061        this.alt = new Transform3d();
062        this.ambiguity = 0;
063        this.bestReprojErr = 0;
064        this.altReprojErr = 0;
065    }
066
067    public PnpResult(Transform3d best, double bestReprojErr) {
068        this(best, best, 0, bestReprojErr, bestReprojErr);
069    }
070
071    public PnpResult(
072            Transform3d best,
073            Transform3d alt,
074            double ambiguity,
075            double bestReprojErr,
076            double altReprojErr) {
077        this.best = best;
078        this.alt = alt;
079        this.ambiguity = ambiguity;
080        this.bestReprojErr = bestReprojErr;
081        this.altReprojErr = altReprojErr;
082    }
083
084    @Override
085    public int hashCode() {
086        final int prime = 31;
087        int result = 1;
088        result = prime * result + ((best == null) ? 0 : best.hashCode());
089        long temp;
090        temp = Double.doubleToLongBits(bestReprojErr);
091        result = prime * result + (int) (temp ^ (temp >>> 32));
092        result = prime * result + ((alt == null) ? 0 : alt.hashCode());
093        temp = Double.doubleToLongBits(altReprojErr);
094        result = prime * result + (int) (temp ^ (temp >>> 32));
095        temp = Double.doubleToLongBits(ambiguity);
096        result = prime * result + (int) (temp ^ (temp >>> 32));
097        return result;
098    }
099
100    @Override
101    public boolean equals(Object obj) {
102        if (this == obj) return true;
103        if (obj == null) return false;
104        if (getClass() != obj.getClass()) return false;
105        PnpResult other = (PnpResult) obj;
106        if (best == null) {
107            if (other.best != null) return false;
108        } else if (!best.equals(other.best)) return false;
109        if (Double.doubleToLongBits(bestReprojErr) != Double.doubleToLongBits(other.bestReprojErr))
110            return false;
111        if (alt == null) {
112            if (other.alt != null) return false;
113        } else if (!alt.equals(other.alt)) return false;
114        if (Double.doubleToLongBits(altReprojErr) != Double.doubleToLongBits(other.altReprojErr))
115            return false;
116        if (Double.doubleToLongBits(ambiguity) != Double.doubleToLongBits(other.ambiguity))
117            return false;
118        return true;
119    }
120
121    @Override
122    public String toString() {
123        return "PnpResult [best="
124                + best
125                + ", bestReprojErr="
126                + bestReprojErr
127                + ", alt="
128                + alt
129                + ", altReprojErr="
130                + altReprojErr
131                + ", ambiguity="
132                + ambiguity
133                + "]";
134    }
135
136    public static final PNPResultProto proto = new PNPResultProto();
137    public static final PnpResultSerde photonStruct = new PnpResultSerde();
138
139    @Override
140    public PacketSerde<PnpResult> getSerde() {
141        return photonStruct;
142    }
143}