Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +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

27
photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -45,12 +45,12 @@ import org.opencv.core.Point3;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
private static Rotation3d NWU_TO_EDN;
private static Rotation3d EDN_TO_NWU;
private static final Rotation3d NWU_TO_EDN;
private static final Rotation3d EDN_TO_NWU;
// Creating a cscore object is sufficient to load opencv, per
// https://www.chiefdelphi.com/t/unsatisfied-link-error-when-simulating-java-robot-code-using-opencv/426731/4
@@ -170,8 +170,8 @@ public final class OpenCVHelp {
public static List<TargetCorner> pointsToCorners(Point... points) {
var corners = new ArrayList<TargetCorner>(points.length);
for (int i = 0; i < points.length; i++) {
corners.add(new TargetCorner(points[i].x, points[i].y));
for (Point point : points) {
corners.add(new TargetCorner(point.x, point.y));
}
return corners;
}
@@ -199,7 +199,7 @@ public final class OpenCVHelp {
* <p>({1,2,3}, true, 1) == {3,2,1}
*
* @param <T> Element type
* @param elements
* @param elements list elements
* @param backwards If indexing should happen in reverse (0, size-1, size-2, ...)
* @param shiftStart How much the inital index should be shifted (instead of starting at index 0,
* start at shiftStart, negated if backwards)
@@ -401,7 +401,7 @@ public final class OpenCVHelp {
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
public static PNPResults solvePNP_SQUARE(
public static PNPResult solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> modelTrls,
@@ -466,15 +466,14 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");
if (alt != null)
return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
if (alt != null) return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResult(best, errors[0]);
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return new PNPResults();
return new PNPResult();
} finally {
// release our Mats from native memory
objectMat.release();
@@ -509,7 +508,7 @@ public final class OpenCVHelp {
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static PNPResults solvePNP_SQPNP(
public static PNPResult solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> objectTrls,
@@ -558,11 +557,11 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
return new PNPResults(best, error[0]);
return new PNPResult(best, error[0]);
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return new PNPResults();
return new PNPResult();
}
}
}

2
photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
View File

@@ -151,7 +151,7 @@ public class TargetModel {
*/
public List<Translation3d> getFieldVertices(Pose3d targetPose) {
var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation());
return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList());
return vertices.stream().map(basisChange::apply).collect(Collectors.toList());
}
/**

24
photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -29,7 +29,7 @@ import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -64,9 +64,9 @@ public class VisionEstimation {
* @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
* @param tagLayout The known tag layout on the field
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResults} are present before utilizing them.
* PNPResult} are present before utilizing them.
*/
public static PNPResults estimateCamPosePNP(
public static PNPResult estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<PhotonTrackedTarget> visTags,
@@ -74,9 +74,9 @@ public class VisionEstimation {
TargetModel tagModel) {
if (tagLayout == null
|| visTags == null
|| tagLayout.getTags().size() == 0
|| visTags.size() == 0) {
return new PNPResults();
|| tagLayout.getTags().isEmpty()
|| visTags.isEmpty()) {
return new PNPResult();
}
var corners = new ArrayList<TargetCorner>();
@@ -92,8 +92,8 @@ public class VisionEstimation {
corners.addAll(tgt.getDetectedCorners());
});
}
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
return new PNPResults();
if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
return new PNPResult();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);
@@ -101,14 +101,14 @@ public class VisionEstimation {
if (knownTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points);
if (!camToTag.isPresent) return new PNPResults();
if (!camToTag.isPresent) return new PNPResult();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
var altPose = new Pose3d();
if (camToTag.ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
var o = new Pose3d();
return new PNPResults(
return new PNPResult(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.ambiguity,
@@ -120,8 +120,8 @@ public class VisionEstimation {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResults();
return new PNPResults(
if (!camToOrigin.isPresent) return new PNPResult();
return new PNPResult(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,

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);
}
}