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

14
photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
View File

@@ -24,7 +24,7 @@ import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.target.TrackedTarget;
@@ -32,13 +32,13 @@ import org.photonvision.vision.target.TrackedTarget;
/** Estimate the camera pose given multiple Apriltag observations */
public class MultiTargetPNPPipe
extends CVPipe<
List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
List<TrackedTarget>, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
private boolean hasWarned = false;
@Override
protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
if (params == null
|| params.cameraCoefficients == null
|| params.cameraCoefficients.getCameraIntrinsicsMat() == null
@@ -48,13 +48,13 @@ public class MultiTargetPNPPipe
"Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
hasWarned = true;
}
return new MultiTargetPNPResults();
return new MultiTargetPNPResult();
}
return calculateCameraInField(targetList);
}
private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetList) {
private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetList) {
// Find tag IDs that exist in the tag layout
var tagIDsUsed = new ArrayList<Integer>();
for (var target : targetList) {
@@ -64,7 +64,7 @@ public class MultiTargetPNPPipe
// Only run with multiple targets
if (tagIDsUsed.size() < 2) {
return new MultiTargetPNPResults();
return new MultiTargetPNPResult();
}
var estimatedPose =
@@ -75,7 +75,7 @@ public class MultiTargetPNPPipe
params.atfl,
params.targetModel);
return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
return new MultiTargetPNPResult(estimatedPose, tagIDsUsed);
}
public static class MultiTargetPNPPipeParams {

4
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -31,7 +31,7 @@ import java.util.List;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
@@ -149,7 +149,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
}
// Do multi-tag pose estimation
MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;

4
photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
View File

@@ -47,7 +47,7 @@ import org.opencv.objdetect.Objdetect;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
@@ -170,7 +170,7 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
}
// Do multi-tag pose estimation
MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;

4
photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
View File

@@ -34,7 +34,7 @@ import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.file.FileUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
@@ -147,7 +147,7 @@ public class Calibrate3dPipeline
sumPipeNanosElapsed,
fps, // Unused but here in case
Collections.emptyList(),
new MultiTargetPNPResults(),
new MultiTargetPNPResult(),
new Frame(
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
}

14
photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
View File

@@ -20,7 +20,7 @@ package org.photonvision.vision.pipeline.result;
import java.util.Collections;
import java.util.List;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.target.TrackedTarget;
@@ -31,23 +31,23 @@ public class CVPipelineResult implements Releasable {
public final double fps;
public final List<TrackedTarget> targets;
public final Frame inputAndOutputFrame;
public MultiTargetPNPResults multiTagResult;
public MultiTargetPNPResult multiTagResult;
public CVPipelineResult(
double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame);
}
public CVPipelineResult(
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResults multiTagResults,
MultiTargetPNPResult multiTagResult,
Frame inputFrame) {
this.processingNanos = processingNanos;
this.fps = fps;
this.targets = targets != null ? targets : Collections.emptyList();
this.multiTagResult = multiTagResults;
this.multiTagResult = multiTagResult;
this.inputAndOutputFrame = inputFrame;
}
@@ -56,8 +56,8 @@ public class CVPipelineResult implements Releasable {
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResults multiTagResults) {
this(processingNanos, fps, targets, multiTagResults, null);
MultiTargetPNPResult multiTagResult) {
this(processingNanos, fps, targets, multiTagResult, null);
}
public boolean hasTargets() {

2
photon-lib/build.gradle
View File

@@ -46,7 +46,7 @@ dependencies {
implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get()
implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get();
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get()
// Junit
testImplementation wpilibTools.deps.wpilibJava("cscore")

2
photon-lib/publish.gradle
View File

@@ -152,7 +152,7 @@ publishing {
tasks.withType(PublishToMavenRepository) {
doFirst {
println("Publishing to " + repository.url);
println("Publishing to " + repository.url)
}
}

68
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -24,8 +24,6 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -43,7 +41,6 @@ import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
@@ -73,9 +70,8 @@ public class PhotonCamera implements AutoCloseable {
IntegerPublisher pipelineIndexRequest, ledModeRequest;
IntegerSubscriber pipelineIndexState, ledModeState;
IntegerSubscriber heartbeatEntry;
private DoubleArraySubscriber cameraIntrinsicsSubscriber;
private DoubleArraySubscriber cameraDistortionSubscriber;
private StringPublisher atflPublisher;
DoubleArraySubscriber cameraIntrinsicsSubscriber;
DoubleArraySubscriber cameraDistortionSubscriber;
@Override
public void close() {
@@ -99,7 +95,6 @@ public class PhotonCamera implements AutoCloseable {
pipelineIndexRequest.close();
cameraIntrinsicsSubscriber.close();
cameraDistortionSubscriber.close();
atflPublisher.close();
}
private final String path;
@@ -111,7 +106,7 @@ public class PhotonCamera implements AutoCloseable {
private long prevHeartbeatValue = -1;
private double prevHeartbeatChangeTime = 0;
private static final double HEARBEAT_DEBOUNCE_SEC = 0.5;
private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;
public static void setVersionCheckEnabled(boolean enabled) {
VERSION_CHECK_ENABLED = enabled;
@@ -119,12 +114,6 @@ public class PhotonCamera implements AutoCloseable {
Packet packet = new Packet(1);
private static AprilTagFieldLayout lastSetTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
// Existing is enough to make this multisubscriber do its thing
private final MultiSubscriber m_topicNameSubscriber;
/**
* Constructs a PhotonCamera from a root table.
*
@@ -159,11 +148,8 @@ public class PhotonCamera implements AutoCloseable {
ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
// Save the layout locally on Rio; on reboot, should be pushed out to NT clients
atflPublisher.getTopic().setPersistent(true);
m_topicNameSubscriber =
// Existing is enough to make this multisubscriber do its thing
MultiSubscriber m_topicNameSubscriber =
new MultiSubscriber(
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
}
@@ -329,49 +315,9 @@ public class PhotonCamera implements AutoCloseable {
prevHeartbeatValue = curHeartbeat;
}
return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
}
// TODO: Implement ATFL subscribing in backend
// /**
// * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all
// PhotonVision
// * coprocessors on the same NT instance-- this method returns the most recent layout set. If no
// * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}.
// *
// * @return The last set layout
// */
// public AprilTagFieldLayout getAprilTagFieldLayout() {
// return lastSetTagLayout;
// }
// /**
// * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might
// * later) connect to this robot. The topic is marked as persistent, so even if you only call
// this
// * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code
// reboots.
// * PhotonVision will also store a copy of this layout locally on the coprocessor, but
// subscribes
// * to this topic and the local copy will be updated when this function is called.
// *
// * @param layout The layout to use for *all* PhotonVision cameras
// * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
// * have updated their internal layouts.
// */
// public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) {
// try {
// var layout_json = new ObjectMapper().writeValueAsString(layout);
// atflPublisher.set(layout_json);
// lastSetTagLayout = layout;
// return true;
// } catch (JsonProcessingException e) {
// MathSharedStore.reportError("Error setting ATFL in " + this.getName(),
// e.getStackTrace());
// }
// return false;
// }
public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
if (cameraMatrix != null && cameraMatrix.length == 9) {
@@ -428,7 +374,7 @@ public class PhotonCamera implements AutoCloseable {
// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.equals("") && !PhotonVersion.versionMatches(versionString)) {
if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) {
// Error on a verified version mismatch
// But stay silent otherwise
DriverStation.reportWarning(

6
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -443,15 +443,15 @@ public class PhotonPoseEstimator {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
var pnpResults =
var pnpResult =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResults.isPresent)
if (!pnpResult.isPresent)
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
.plus(pnpResult.best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(

2
photon-lib/src/main/java/org/photonvision/PhotonUtils.java
View File

@@ -201,7 +201,7 @@ public final class PhotonUtils {
*
* @param robotPose Pose of the robot
* @param targetPose Pose of the target
* @return
* @return distance to the pose
*/
public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
return robotPose.getTranslation().getDistance(targetPose.getTranslation());

17
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -54,8 +54,8 @@ import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -80,7 +80,8 @@ public class PhotonCameraSim implements AutoCloseable {
private double minTargetAreaPercent;
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
private final AprilTagFieldLayout tagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
// video stream simulation
private final CvSource videoSimRaw;
@@ -419,7 +420,7 @@ public class PhotonCameraSim implements AutoCloseable {
// projected target can't be detected, skip to next
if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue;
var pnpSim = new PNPResults();
var pnpSim = new PNPResult();
if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
pnpSim =
OpenCVHelp.solvePNP_SQUARE(
@@ -516,21 +517,21 @@ public class PhotonCameraSim implements AutoCloseable {
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
// calculate multitag results
var multitagResults = new MultiTargetPNPResults();
var multitagResult = new MultiTargetPNPResult();
// TODO: Implement ATFL subscribing in backend
// var tagLayout = cam.getAprilTagFieldLayout();
var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
List<Integer> usedIDs =
visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
var pnpResults =
var pnpResult =
VisionEstimation.estimateCamPosePNP(
prop.getIntrinsics(),
prop.getDistCoeffs(),
detectableTgts,
tagLayout,
TargetModel.kAprilTag16h5);
multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs);
multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
}
// sort target order
@@ -539,7 +540,7 @@ public class PhotonCameraSim implements AutoCloseable {
}
// put this simulated data to NT
return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults);
return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResult);
}
/**

16
photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
View File

@@ -79,7 +79,7 @@ public class SimCameraProperties {
private double avgLatencyMs = 0;
private double latencyStdDevMs = 0;
// util
private List<DMatrix3> viewplanes = new ArrayList<>();
private final List<DMatrix3> viewplanes = new ArrayList<>();
/** Default constructor which is the same as {@link #PERFECT_90DEG} */
public SimCameraProperties() {
@@ -215,8 +215,9 @@ public class SimCameraProperties {
0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0))
};
viewplanes.clear();
for (int i = 0; i < p.length; i++) {
viewplanes.add(new DMatrix3(p[i].getX(), p[i].getY(), p[i].getZ()));
for (Translation3d translation3d : p) {
viewplanes.add(
new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ()));
}
}
@@ -457,8 +458,7 @@ public class SimCameraProperties {
// check if the ends of the line segment are visible
boolean aVisible = true;
boolean bVisible = true;
for (int i = 0; i < viewplanes.size(); i++) {
var normal = viewplanes.get(i);
for (DMatrix3 normal : viewplanes) {
double aVisibility = CommonOps_DDF3.dot(av, normal);
if (aVisibility < 0) aVisible = false;
double bVisibility = CommonOps_DDF3.dot(bv, normal);
@@ -467,7 +467,7 @@ public class SimCameraProperties {
if (aVisibility <= 0 && bVisibility <= 0) return new Pair<>(null, null);
}
// both ends are inside frustum
if (aVisible && bVisible) return new Pair<>(Double.valueOf(0), Double.valueOf(1));
if (aVisible && bVisible) return new Pair<>((double) 0, 1.0);
// parametrized (t=0 at a, t=1 at b) intersections with viewplanes
double[] intersections = {Double.NaN, Double.NaN, Double.NaN, Double.NaN};
@@ -546,8 +546,8 @@ public class SimCameraProperties {
}
// one viewplane intersection
else if (!Double.isNaN(inter1)) {
if (aVisible) return new Pair<>(Double.valueOf(0), inter1);
if (bVisible) return new Pair<>(inter1, Double.valueOf(1));
if (aVisible) return new Pair<>((double) 0, inter1);
if (bVisible) return new Pair<>(inter1, 1.0);
return new Pair<>(inter1, null);
}
// no intersections

4
photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
View File

@@ -35,7 +35,7 @@ import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -142,7 +142,7 @@ public class SimPhotonCamera {
}
PhotonPipelineResult newResult =
new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults());
new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResult());
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData());

4
photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java
View File

@@ -120,9 +120,7 @@ public class SimVisionSystem {
*/
public void addSimVisionTarget(SimVisionTarget target) {
tgtList.add(target);
dbgField
.getObject("Target " + Integer.toString(target.targetID))
.setPose(target.targetPose.toPose2d());
dbgField.getObject("Target " + target.targetID).setPose(target.targetPose.toPose2d());
}
/**

20
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View File

@@ -374,8 +374,8 @@ public class VideoSimUtil {
/**
* Set the field dimensions that are used for drawing the field wireframe.
*
* @param fieldLengthMeters
* @param fieldWidthMeters
* @param fieldLengthMeters field length in meters (x direction)
* @param fieldWidthMeters field width in meters (y direction)
*/
public static void setFieldDimensionsMeters(double fieldLengthMeters, double fieldWidthMeters) {
fieldLength = fieldLengthMeters;
@@ -501,8 +501,8 @@ public class VideoSimUtil {
if (inter.getSecond() == null) continue;
// cull line to the inside of the camera fulcrum
double inter1 = inter.getFirst().doubleValue();
double inter2 = inter.getSecond().doubleValue();
double inter1 = inter.getFirst();
double inter2 = inter.getSecond();
var baseDelta = ptb.minus(pta);
var old_pta = pta;
if (inter1 > 0) pta = old_pta.plus(baseDelta.times(inter1));
@@ -510,11 +510,11 @@ public class VideoSimUtil {
baseDelta = ptb.minus(pta);
// project points into 2d
var poly = new ArrayList<Point>();
poly.addAll(
Arrays.asList(
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb))));
var poly =
new ArrayList<>(
Arrays.asList(
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb))));
var pxa = poly.get(0);
var pxb = poly.get(1);
@@ -526,7 +526,7 @@ public class VideoSimUtil {
for (int j = 0; j < subdivisions; j++) {
subPts.add(pta.plus(subDelta.times(j + 1)));
}
if (subPts.size() > 0) {
if (!subPts.isEmpty()) {
poly.addAll(
1,
Arrays.asList(

5
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -176,8 +176,7 @@ public class VisionSystemSim {
*/
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim, double timeSeconds) {
var robotToCamera = getRobotToCamera(cameraSim, timeSeconds);
if (robotToCamera.isEmpty()) return Optional.empty();
return Optional.of(getRobotPose(timeSeconds).plus(robotToCamera.get()));
return robotToCamera.map(transform3d -> getRobotPose(timeSeconds).plus(transform3d));
}
/**
@@ -405,6 +404,6 @@ public class VisionSystemSim {
}
}
if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d);
if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d);
if (!cameraPoses2d.isEmpty()) dbgField.getObject("cameras").setPoses(cameraPoses2d);
}
}

8
photon-lib/src/test/java/org/photonvision/PacketTest.java
View File

@@ -30,8 +30,8 @@ import java.util.List;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -174,8 +174,8 @@ class PacketTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))),
new MultiTargetPNPResults(
new PNPResults(
new MultiTargetPNPResult(
new PNPResult(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3)));

4
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -69,7 +69,7 @@ class PhotonPoseEstimatorTest {
e.printStackTrace();
}
List<AprilTag> tagList = new ArrayList<AprilTag>(2);
List<AprilTag> tagList = new ArrayList<>(2);
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
double fieldLength = Units.feetToMeters(54.0);
@@ -619,7 +619,7 @@ class PhotonPoseEstimatorTest {
assertEquals(2.15, pose.getZ(), .01);
}
private class PhotonCameraInjector extends PhotonCamera {
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");
}

2
photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java
View File

@@ -30,7 +30,7 @@ import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
public class PhotonVersionTest {
public static final boolean versionMatches(String versionString, String other) {
public static boolean versionMatches(String versionString, String other) {
String c = versionString;
Pattern p = Pattern.compile("v[0-9]+.[0-9]+.[0-9]+");
Matcher m = p.matcher(c);

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