Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-24 01:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Run multitag on coprocessor (#816)

Browse Source
This commit is contained in:
Matt
2023-10-17 10:20:00 -04:00
committed by GitHub
parent ededc4f130
commit 47bd077bbb
72 changed files with 1708 additions and 1801 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java Normal file
View File

@@ -0,0 +1,93 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.targeting;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
public class MultiTargetPNPResults {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
// pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
public PNPResults estimatedPose = new PNPResults();
public List<Integer> fiducialIDsUsed = List.of();
public MultiTargetPNPResults() {}
public MultiTargetPNPResults(PNPResults results, List<Integer> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}
public static MultiTargetPNPResults createFromPacket(Packet packet) {
var results = PNPResults.createFromPacket(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = (int) packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResults(results, ids);
}
public void populatePacket(Packet packet) {
estimatedPose.populatePacket(packet);
for (int i = 0; i < MAX_IDS; i++) {
if (i < fiducialIDsUsed.size()) {
packet.encode((short) fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
if (estimatedPose == null) {
if (other.estimatedPose != null) return false;
} else if (!estimatedPose.equals(other.estimatedPose)) return false;
if (fiducialIDsUsed == null) {
if (other.fiducialIDsUsed != null) return false;
} else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
return true;
}
@Override
public String toString() {
return "MultiTargetPNPResults [estimatedPose="
+ estimatedPose
+ ", fiducialIDsUsed="
+ fiducialIDsUsed
+ "]";
}
}

170
photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java Normal file
View File

@@ -0,0 +1,170 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.utils.PacketUtils;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
* depending on the solvePNP method. If an alternate solution is present, the ambiguity value
* represents the ratio of reprojection error in the best solution to the alternate (best /
* alternate).
*
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResults {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
/** An empty (invalid) result. */
public PNPResults() {
this.isPresent = false;
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
this.bestReprojErr = 0;
this.altReprojErr = 0;
}
public PNPResults(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResults(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
this.bestReprojErr = bestReprojErr;
this.altReprojErr = altReprojErr;
}
public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
public static PNPResults createFromPacket(Packet packet) {
var present = packet.decodeBoolean();
var best = PacketUtils.decodeTransform(packet);
var alt = PacketUtils.decodeTransform(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
if (present) {
return new PNPResults(best, alt, ambiguity, bestEr, altEr);
} else {
return new PNPResults();
}
}
public Packet populatePacket(Packet packet) {
packet.encode(isPresent);
PacketUtils.encodeTransform(packet, best);
PacketUtils.encodeTransform(packet, alt);
packet.encode(bestReprojErr);
packet.encode(altReprojErr);
packet.encode(ambiguity);
return packet;
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (isPresent ? 1231 : 1237);
result = prime * result + ((best == null) ? 0 : best.hashCode());
long temp;
temp = Double.doubleToLongBits(bestReprojErr);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + ((alt == null) ? 0 : alt.hashCode());
temp = Double.doubleToLongBits(altReprojErr);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(ambiguity);
result = prime * result + (int) (temp ^ (temp >>> 32));
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PNPResults other = (PNPResults) obj;
if (isPresent != other.isPresent) return false;
if (best == null) {
if (other.best != null) return false;
} else if (!best.equals(other.best)) return false;
if (Double.doubleToLongBits(bestReprojErr) != Double.doubleToLongBits(other.bestReprojErr))
return false;
if (alt == null) {
if (other.alt != null) return false;
} else if (!alt.equals(other.alt)) return false;
if (Double.doubleToLongBits(altReprojErr) != Double.doubleToLongBits(other.altReprojErr))
return false;
if (Double.doubleToLongBits(ambiguity) != Double.doubleToLongBits(other.ambiguity))
return false;
return true;
}
@Override
public String toString() {
return "PNPResults [isPresent="
+ isPresent
+ ", best="
+ best
+ ", bestReprojErr="
+ bestReprojErr
+ ", alt="
+ alt
+ ", altReprojErr="
+ altReprojErr
+ ", ambiguity="
+ ambiguity
+ "]";
}
}

49
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -34,6 +34,9 @@ public class PhotonPipelineResult {
// Timestamp in milliseconds.
private double timestampSeconds = -1;
// Multi-tag result
private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}
@@ -48,13 +51,30 @@ public class PhotonPipelineResult {
this.targets.addAll(targets);
}
/**
* Constructs a pipeline result.
*
* @param latencyMillis The latency in the pipeline.
* @param targets The list of targets identified by the pipeline.
* @param result Result from multi-target PNP.
*/
public PhotonPipelineResult(
double latencyMillis, List<PhotonTrackedTarget> targets, MultiTargetPNPResults result) {
this.latencyMillis = latencyMillis;
this.targets.addAll(targets);
this.multiTagResult = result;
}
/**
* Returns the size of the packet needed to store this pipeline result.
*
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES
+ 8 // latency
+ MultiTargetPNPResults.PACK_SIZE_BYTES
+ 1; // target count
}
/**
@@ -122,6 +142,14 @@ public class PhotonPipelineResult {
return new ArrayList<>(targets);
}
/**
* Return the latest mulit-target result. Be sure to check
* getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
*/
public MultiTargetPNPResults getMultiTagResult() {
return multiTagResult;
}
/**
* Populates the fields of the pipeline result from the packet.
*
@@ -131,6 +159,7 @@ public class PhotonPipelineResult {
public Packet createFromPacket(Packet packet) {
// Decode latency, existence of targets, and number of targets.
latencyMillis = packet.decodeDouble();
this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet);
byte targetCount = packet.decodeByte();
targets.clear();
@@ -154,6 +183,7 @@ public class PhotonPipelineResult {
public Packet populatePacket(Packet packet) {
// Encode latency, existence of targets, and number of targets.
packet.encode(latencyMillis);
multiTagResult.populatePacket(packet);
packet.encode((byte) targets.size());
// Encode the information of each target.
@@ -173,6 +203,7 @@ public class PhotonPipelineResult {
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(timestampSeconds);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode());
return result;
}
@@ -189,6 +220,22 @@ public class PhotonPipelineResult {
return false;
if (Double.doubleToLongBits(timestampSeconds)
!= Double.doubleToLongBits(other.timestampSeconds)) return false;
if (multiTagResult == null) {
if (other.multiTagResult != null) return false;
} else if (!multiTagResult.equals(other.multiTagResult)) return false;
return true;
}
@Override
public String toString() {
return "PhotonPipelineResult [targets="
+ targets
+ ", latencyMillis="
+ latencyMillis
+ ", timestampSeconds="
+ timestampSeconds
+ ", multiTagResult="
+ multiTagResult
+ "]";
}
}

35
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -17,13 +17,11 @@
package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.utils.PacketUtils;
public class PhotonTrackedTarget {
private static final int MAX_CORNERS = 8;
@@ -198,29 +196,6 @@ public class PhotonTrackedTarget {
return true;
}
private static Transform3d decodeTransform(Packet packet) {
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
return new Transform3d(translation, rotation);
}
private static void encodeTransform(Packet packet, Transform3d transform) {
packet.encode(transform.getTranslation().getX());
packet.encode(transform.getTranslation().getY());
packet.encode(transform.getTranslation().getZ());
packet.encode(transform.getRotation().getQuaternion().getW());
packet.encode(transform.getRotation().getQuaternion().getX());
packet.encode(transform.getRotation().getQuaternion().getY());
packet.encode(transform.getRotation().getQuaternion().getZ());
}
private static void encodeList(Packet packet, List<TargetCorner> list) {
packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
for (int i = 0; i < list.size(); i++) {
@@ -253,8 +228,8 @@ public class PhotonTrackedTarget {
this.skew = packet.decodeDouble();
this.fiducialId = packet.decodeInt();
this.bestCameraToTarget = decodeTransform(packet);
this.altCameraToTarget = decodeTransform(packet);
this.bestCameraToTarget = PacketUtils.decodeTransform(packet);
this.altCameraToTarget = PacketUtils.decodeTransform(packet);
this.poseAmbiguity = packet.decodeDouble();
@@ -282,8 +257,8 @@ public class PhotonTrackedTarget {
packet.encode(area);
packet.encode(skew);
packet.encode(fiducialId);
encodeTransform(packet, bestCameraToTarget);
encodeTransform(packet, altCameraToTarget);
PacketUtils.encodeTransform(packet, bestCameraToTarget);
PacketUtils.encodeTransform(packet, altCameraToTarget);
packet.encode(poseAmbiguity);
for (int i = 0; i < 4; i++) {