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

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