Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-04 03: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

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

66
photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java Normal file
View File

@@ -0,0 +1,66 @@
/*
* 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 static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import org.junit.jupiter.api.Test;
public class MultiTargetPNPResultTest {
@Test
public void equalityTest() {
var a = new MultiTargetPNPResult();
var b = new MultiTargetPNPResult();
assertEquals(a, b);
a =
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3));
b =
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3));
assertEquals(a, b);
}
@Test
public void inequalityTest() {
var a =
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(3, 4, 7));
var b =
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3));
assertNotEquals(a, b);
}
}

77
photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java Normal file
View File

@@ -0,0 +1,77 @@
/*
* 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 static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import org.junit.jupiter.api.Test;
public class PNPResultTest {
@Test
public void equalityTest() {
var a = new PNPResult();
var b = new PNPResult();
assertEquals(a, b);
a = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0);
b = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0);
assertEquals(a, b);
a =
new PNPResult(
new Transform3d(0, 1, 2, new Rotation3d()),
new Transform3d(3, 4, 5, new Rotation3d()),
0.5,
0.1,
0.1);
b =
new PNPResult(
new Transform3d(0, 1, 2, new Rotation3d()),
new Transform3d(3, 4, 5, new Rotation3d()),
0.5,
0.1,
0.1);
assertEquals(a, b);
}
@Test
public void inequalityTest() {
var a = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0);
var b = new PNPResult(new Transform3d(3, 4, 5, new Rotation3d()), 0.1);
assertNotEquals(a, b);
a =
new PNPResult(
new Transform3d(3, 4, 5, new Rotation3d()),
new Transform3d(0, 1, 2, new Rotation3d()),
0.5,
0.1,
0.1);
b =
new PNPResult(
new Transform3d(3, 4, 5, new Rotation3d()),
new Transform3d(0, 1, 2, new Rotation3d()),
0.5,
0.1,
0.2);
assertNotEquals(a, b);
}
}

399
photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java Normal file
View File

@@ -0,0 +1,399 @@
/*
* 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 static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import org.junit.jupiter.api.Test;
public class PhotonPipelineResultTest {
@Test
public void equalityTest() {
var a = new PhotonPipelineResult();
var b = new PhotonPipelineResult();
assertEquals(a, b);
a =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
b =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
assertEquals(a, b);
a =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))),
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3)));
b =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))),
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3)));
assertEquals(a, b);
}
@Test
public void inequalityTest() {
var a =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
7.0,
2.0,
1.0,
-9.0,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
var b =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
7.0,
2.0,
1.0,
-9.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
assertNotEquals(a, b);
a =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))),
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(3, 4, 7)));
b =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))),
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3)));
assertNotEquals(a, b);
}
}

119
photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java Normal file
View File

@@ -0,0 +1,119 @@
/*
* 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 static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import org.junit.jupiter.api.Test;
public class PhotonTrackedTargetTest {
@Test
public void equalityTest() {
var a =
new PhotonTrackedTarget(
3.0,
4.0,
9.0,
-5.0,
-1,
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)));
var b =
new PhotonTrackedTarget(
3.0,
4.0,
9.0,
-5.0,
-1,
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)));
assertEquals(a, b);
}
@Test
public void inequalityTest() {
var a =
new PhotonTrackedTarget(
3.0,
4.0,
9.0,
-5.0,
-1,
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)));
var b =
new PhotonTrackedTarget(
7.0,
2.0,
1.0,
-9.0,
-1,
new Transform3d(new Translation3d(), new Rotation3d()),
new Transform3d(new Translation3d(), new Rotation3d()),
0.25,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)));
assertNotEquals(a, b);
}
}

41
photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java Normal file
View File

@@ -0,0 +1,41 @@
/*
* 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 static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import org.junit.jupiter.api.Test;
public class TargetCornerTest {
@Test
public void equalityTest() {
var a = new TargetCorner(0, 1);
var b = new TargetCorner(0, 1);
assertEquals(a, b);
}
@Test
public void inequalityTest() {
var a = new TargetCorner(0, 1);
var b = new TargetCorner(2, 4);
assertNotEquals(a, b);
}
}