Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-02 02:51: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() {