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

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