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

Only run Apriltag pose estimation when enabled (#657)

Browse Source 
Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
This commit is contained in:
Matt
2022-12-28 13:32:38 -08:00
committed by GitHub
parent 7ff630dc44
commit 3a10f49b54
2 changed files with 25 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -19,6 +19,7 @@ package org.photonvision.vision.pipeline;
import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.apriltag.AprilTagDetection;
import edu.wpi.first.apriltag.AprilTagDetector; import edu.wpi.first.apriltag.AprilTagDetector;
import edu.wpi.first.apriltag.AprilTagPoseEstimate;
import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
@@ -146,17 +147,19 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
if (detection.getDecisionMargin() < settings.decisionMargin) continue; if (detection.getDecisionMargin() < settings.decisionMargin) continue;
if (detection.getHamming() > settings.hammingDist) continue; if (detection.getHamming() > settings.hammingDist) continue;
// Do pose estimation for all the tags that make it thru AprilTagPoseEstimate tagPoseEstimate = null;
// TODO if (settings.solvePNPEnabled) {
var poseResult = poseEstimatorPipe.run(detection); var poseResult = poseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed; sumPipeNanosElapsed += poseResult.nanosElapsed;
tagPoseEstimate = poseResult.output;
}
// populate the target list // populate the target list
// Challenge here is that TrackedTarget functions with OpenCV Contour // Challenge here is that TrackedTarget functions with OpenCV Contour
TrackedTarget target = TrackedTarget target =
new TrackedTarget( new TrackedTarget(
detection, detection,
poseResult.output, tagPoseEstimate,
new TargetCalculationParameters( new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties)); false, null, null, null, null, frameStaticProperties));

31
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -80,20 +80,25 @@ public class TrackedTarget implements Releasable {
tagDetection.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength); tagDetection.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
var bestPose = new Transform3d(); var bestPose = new Transform3d();
var altPose = new Transform3d(); var altPose = new Transform3d();
if (tagPose.error1 <= tagPose.error2) {
bestPose = tagPose.pose1; if (tagPose != null) {
altPose = tagPose.pose2; if (tagPose.error1 <= tagPose.error2) {
} else { bestPose = tagPose.pose1;
bestPose = tagPose.pose2; altPose = tagPose.pose2;
altPose = tagPose.pose1; } else {
bestPose = tagPose.pose2;
altPose = tagPose.pose1;
}
bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
altPose = MathUtils.convertApriltagtoOpenCV(altPose);
m_bestCameraToTarget3d = bestPose;
m_altCameraToTarget3d = altPose;
m_poseAmbiguity = tagPose.getAmbiguity();
} }
bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
altPose = MathUtils.convertApriltagtoOpenCV(altPose);
m_bestCameraToTarget3d = bestPose;
m_altCameraToTarget3d = altPose;
double[] corners = tagDetection.getCorners(); double[] corners = tagDetection.getCorners();
Point[] cornerPoints = Point[] cornerPoints =
new Point[] { new Point[] {
@@ -128,8 +133,6 @@ public class TrackedTarget implements Releasable {
var rvec = new Mat(3, 1, CvType.CV_64FC1); var rvec = new Mat(3, 1, CvType.CV_64FC1);
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec); MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec); setCameraRelativeRvec(rvec);
m_poseAmbiguity = tagPose.getAmbiguity();
} }
public void setFiducialId(int id) { public void setFiducialId(int id) {