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