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

[photon-targeting][photon-lib] Add tests to the targeting classes and cleanup photon-lib & photon-targeting (#1007)

Browse Source 
* Make MultiTagPNPResult and PNPResult singular

* add java tests

* Formatting fixes

* bring in the rest of the little stuff

* final things

* Formatting fixes

* add multisubscriber back

* Formatting fixes

* make comments better about x and y relationship
This commit is contained in:
Sriman Achanta
2023-11-15 18:28:26 -05:00
committed by GitHub
parent 524b135142
commit 308fd801d4
31 changed files with 833 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java → photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java
View File

@@ -21,30 +21,30 @@ import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
public class MultiTargetPNPResults {
public class MultiTargetPNPResult {
// 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 static final int PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
public PNPResults estimatedPose = new PNPResults();
public PNPResult estimatedPose = new PNPResult();
public List<Integer> fiducialIDsUsed = List.of();
public MultiTargetPNPResults() {}
public MultiTargetPNPResult() {}
public MultiTargetPNPResults(PNPResults results, List<Integer> ids) {
public MultiTargetPNPResult(PNPResult results, List<Integer> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}
public static MultiTargetPNPResults createFromPacket(Packet packet) {
var results = PNPResults.createFromPacket(packet);
public static MultiTargetPNPResult createFromPacket(Packet packet) {
var results = PNPResult.createFromPacket(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = (int) packet.decodeShort();
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResults(results, ids);
return new MultiTargetPNPResult(results, ids);
}
public void populatePacket(Packet packet) {
@@ -72,7 +72,7 @@ public class MultiTargetPNPResults {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
MultiTargetPNPResult other = (MultiTargetPNPResult) obj;
if (estimatedPose == null) {
if (other.estimatedPose != null) return false;
} else if (!estimatedPose.equals(other.estimatedPose)) return false;
@@ -84,7 +84,7 @@ public class MultiTargetPNPResults {
@Override
public String toString() {
return "MultiTargetPNPResults [estimatedPose="
return "MultiTargetPNPResult [estimatedPose="
+ estimatedPose
+ ", fiducialIDsUsed="
+ fiducialIDsUsed

21
photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java → photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java
View File

@@ -30,7 +30,7 @@ import org.photonvision.utils.PacketUtils;
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResults {
public class PNPResult {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
@@ -59,7 +59,7 @@ public class PNPResults {
public final double ambiguity;
/** An empty (invalid) result. */
public PNPResults() {
public PNPResult() {
this.isPresent = false;
this.best = new Transform3d();
this.alt = new Transform3d();
@@ -68,11 +68,11 @@ public class PNPResults {
this.altReprojErr = 0;
}
public PNPResults(Transform3d best, double bestReprojErr) {
public PNPResult(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResults(
public PNPResult(
Transform3d best,
Transform3d alt,
double ambiguity,
@@ -88,7 +88,7 @@ public class PNPResults {
public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
public static PNPResults createFromPacket(Packet packet) {
public static PNPResult createFromPacket(Packet packet) {
var present = packet.decodeBoolean();
var best = PacketUtils.decodeTransform(packet);
var alt = PacketUtils.decodeTransform(packet);
@@ -96,20 +96,19 @@ public class PNPResults {
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
if (present) {
return new PNPResults(best, alt, ambiguity, bestEr, altEr);
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
} else {
return new PNPResults();
return new PNPResult();
}
}
public Packet populatePacket(Packet packet) {
public void 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
@@ -134,7 +133,7 @@ public class PNPResults {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PNPResults other = (PNPResults) obj;
PNPResult other = (PNPResult) obj;
if (isPresent != other.isPresent) return false;
if (best == null) {
if (other.best != null) return false;
@@ -153,7 +152,7 @@ public class PNPResults {
@Override
public String toString() {
return "PNPResults [isPresent="
return "PNPResult [isPresent="
+ isPresent
+ ", best="
+ best

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

@@ -35,7 +35,7 @@ public class PhotonPipelineResult {
private double timestampSeconds = -1;
// Multi-tag result
private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}
@@ -59,7 +59,7 @@ public class PhotonPipelineResult {
* @param result Result from multi-target PNP.
*/
public PhotonPipelineResult(
double latencyMillis, List<PhotonTrackedTarget> targets, MultiTargetPNPResults result) {
double latencyMillis, List<PhotonTrackedTarget> targets, MultiTargetPNPResult result) {
this.latencyMillis = latencyMillis;
this.targets.addAll(targets);
this.multiTagResult = result;
@@ -73,7 +73,7 @@ public class PhotonPipelineResult {
public int getPacketSize() {
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES
+ 8 // latency
+ MultiTargetPNPResults.PACK_SIZE_BYTES
+ MultiTargetPNPResult.PACK_SIZE_BYTES
+ 1; // target count
}
@@ -130,7 +130,7 @@ public class PhotonPipelineResult {
* @return Whether the pipeline has targets.
*/
public boolean hasTargets() {
return targets.size() > 0;
return !targets.isEmpty();
}
/**
@@ -143,10 +143,10 @@ public class PhotonPipelineResult {
}
/**
* Return the latest mulit-target result. Be sure to check
* Return the latest multi-target result. Be sure to check
* getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
*/
public MultiTargetPNPResults getMultiTagResult() {
public MultiTargetPNPResult getMultiTagResult() {
return multiTagResult;
}
@@ -159,7 +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);
this.multiTagResult = MultiTargetPNPResult.createFromPacket(packet);
byte targetCount = packet.decodeByte();
targets.clear();
@@ -197,7 +197,7 @@ public class PhotonPipelineResult {
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + ((targets == null) ? 0 : targets.hashCode());
result = prime * result + targets.hashCode();
long temp;
temp = Double.doubleToLongBits(latencyMillis);
result = prime * result + (int) (temp ^ (temp >>> 32));
@@ -213,9 +213,7 @@ public class PhotonPipelineResult {
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineResult other = (PhotonPipelineResult) obj;
if (targets == null) {
if (other.targets != null) return false;
} else if (!targets.equals(other.targets)) return false;
if (!targets.equals(other.targets)) return false;
if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis))
return false;
if (Double.doubleToLongBits(timestampSeconds)

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

@@ -198,9 +198,9 @@ public class PhotonTrackedTarget {
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++) {
packet.encode(list.get(i).x);
packet.encode(list.get(i).y);
for (TargetCorner targetCorner : list) {
packet.encode(targetCorner.x);
packet.encode(targetCorner.y);
}
}