Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +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.AprilTagDetector;
import edu.wpi.first.apriltag.AprilTagPoseEstimate;
import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
import edu.wpi.first.math.geometry.Transform3d;
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.getHamming() > settings.hammingDist) continue;
// Do pose estimation for all the tags that make it thru
// TODO
var poseResult = poseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed;
AprilTagPoseEstimate tagPoseEstimate = null;
if (settings.solvePNPEnabled) {
var poseResult = poseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed;
tagPoseEstimate = poseResult.output;
}
// populate the target list
// Challenge here is that TrackedTarget functions with OpenCV Contour
TrackedTarget target =
new TrackedTarget(
detection,
poseResult.output,
tagPoseEstimate,
new TargetCalculationParameters(
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);
var bestPose = new Transform3d();
var altPose = new Transform3d();
if (tagPose.error1 <= tagPose.error2) {
bestPose = tagPose.pose1;
altPose = tagPose.pose2;
} else {
bestPose = tagPose.pose2;
altPose = tagPose.pose1;
if (tagPose != null) {
if (tagPose.error1 <= tagPose.error2) {
bestPose = tagPose.pose1;
altPose = tagPose.pose2;
} 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();
Point[] cornerPoints =
new Point[] {
@@ -128,8 +133,6 @@ public class TrackedTarget implements Releasable {
var rvec = new Mat(3, 1, CvType.CV_64FC1);
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec);
m_poseAmbiguity = tagPose.getAmbiguity();
}
public void setFiducialId(int id) {