From 0b5256df12fb751a1bc150450eaa98391b057ba9 Mon Sep 17 00:00:00 2001 From: Nick Hadley Date: Mon, 2 Jan 2023 19:22:39 -0500 Subject: [PATCH] RobotPoseEstimator Enhancements (#677) * Use List for RobotPoseEstimator constructor * Update `RobotPoseEstimator` constructor to accept wpilib `AprilTagFieldLayout` java * Initial cpp changes * Java return optional from update * Fix java test * Clean up strategy switch * small lint * Actually link to vision_shared * Fix auto optimized imports * format * report error * small method changes * format and clean up Co-authored-by: Matt --- photon-lib/build.gradle | 3 +- .../org/photonvision/RobotPoseEstimator.java | 183 ++++++++---------- .../org/photonvision/SimVisionSystem.java | 9 +- .../cpp/photonlib/RobotPoseEstimator.cpp | 27 ++- .../include/photonlib/RobotPoseEstimator.h | 16 +- .../photonvision/RobotPoseEstimatorTest.java | 33 ++-- .../native/cpp/RobotPoseEstimatorTest.cpp | 64 +++--- 7 files changed, 171 insertions(+), 164 deletions(-) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 6bedea719..ce84db080 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -34,7 +34,6 @@ dependencies { implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") - // Junit testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2") @@ -66,6 +65,7 @@ model { } } nativeUtils.useRequiredLibrary(it, "wpilib_shared") + nativeUtils.useRequiredLibrary(it, "vision_shared") } } testSuites { @@ -80,6 +80,7 @@ model { } nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") + nativeUtils.useRequiredLibrary(it, "vision_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") } } diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java index daf2aefcd..728668c94 100644 --- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java @@ -35,6 +35,8 @@ import edu.wpi.first.wpilibj.DriverStation; import java.util.ArrayList; import java.util.HashSet; import java.util.List; +import java.util.Optional; +import java.util.Set; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -62,11 +64,11 @@ public class RobotPoseEstimator { private AprilTagFieldLayout aprilTags; private PoseStrategy strategy; - private ArrayList> cameras; + private List> cameras; private Pose3d lastPose; private Pose3d referencePose; - private HashSet reportedErrors; + private Set reportedErrors; /** * Create a new RobotPoseEstimator. @@ -83,7 +85,7 @@ public class RobotPoseEstimator { public RobotPoseEstimator( AprilTagFieldLayout aprilTags, PoseStrategy strategy, - ArrayList> cameras) { + List> cameras) { this.aprilTags = aprilTags; this.strategy = strategy; this.cameras = cameras; @@ -97,37 +99,36 @@ public class RobotPoseEstimator { * @return The updated estimated pose and the latency in milliseconds Estimated pose may be null * if no targets were seen */ - public Pair update() { + public Optional> update() { if (cameras.isEmpty()) { DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false); - return Pair.of(lastPose, 0.); + return Optional.empty(); } - Pair pair; + + Pair pair = getResultFromActiveStrategy(); + + if (pair != null) { + lastPose = pair.getFirst(); + } + + return Optional.ofNullable(pair); + } + + private Pair getResultFromActiveStrategy() { switch (strategy) { case LOWEST_AMBIGUITY: - pair = lowestAmbiguityStrategy(); - lastPose = pair.getFirst(); - return pair; + return lowestAmbiguityStrategy(); case CLOSEST_TO_CAMERA_HEIGHT: - pair = closestToCameraHeightStrategy(); - lastPose = pair.getFirst(); - return pair; + return closestToCameraHeightStrategy(); case CLOSEST_TO_REFERENCE_POSE: - pair = closestToReferencePoseStrategy(); - lastPose = pair.getFirst(); - return pair; + return closestToReferencePoseStrategy(); case CLOSEST_TO_LAST_POSE: - referencePose = lastPose; - pair = closestToReferencePoseStrategy(); - lastPose = pair.getFirst(); - return pair; + return closestToLastPoseStrategy(); case AVERAGE_BEST_TARGETS: - pair = averageBestTargetsStrategy(); - lastPose = pair.getFirst(); - return pair; + return averageBestTargetsStrategy(); default: DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false); - return Pair.of(null, 0.); + return null; } } @@ -157,37 +158,29 @@ public class RobotPoseEstimator { // No targets, return null if (lowestAI == -1 || lowestAJ == -1) { - return Pair.of(null, 0.); + return null; } // Pick the lowest and do the heavy calculations PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ); - // If the map doesn't contain the ID fail - var tmp = aprilTags.getTagPose(bestTarget.getFiducialId()); - if (tmp.isEmpty()) { - if (!reportedErrors.contains(bestTarget.getFiducialId())) { - DriverStation.reportError( - "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " - + bestTarget.getFiducialId(), - false); - reportedErrors.add(bestTarget.getFiducialId()); - } - return Pair.of(lastPose, 0.); + Optional fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId()); + if (fiducialPose.isEmpty()) { + reportFiducialPoseError(bestTarget.getFiducialId()); + return null; } - var tagPose = tmp.get(); - return Pair.of( - tagPose + fiducialPose + .get() .transformBy(bestTarget.getBestCameraToTarget().inverse()) .transformBy(cameras.get(lowestAI).getSecond().inverse()), results.get(lowestAI).getLatencyMillis()); } private Pair closestToCameraHeightStrategy() { - double smallestHeightDifference = 10e9; - double mili = 0; + double smallestHeightDifference = Double.MAX_VALUE; + double latency = 0; Pose3d pose = null; for (int i = 0; i < cameras.size(); i++) { @@ -196,18 +189,12 @@ public class RobotPoseEstimator { List targets = result.targets; for (int j = 0; j < targets.size(); j++) { PhotonTrackedTarget target = targets.get(j); - var tmp = aprilTags.getTagPose(target.getFiducialId()); - if (tmp.isEmpty()) { - if (!reportedErrors.contains(target.getFiducialId())) { - DriverStation.reportWarning( - "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " - + target.getFiducialId(), - false); - reportedErrors.add(target.getFiducialId()); - } + Optional fiducialPose = aprilTags.getTagPose(target.getFiducialId()); + if (fiducialPose.isEmpty()) { + reportFiducialPoseError(target.getFiducialId()); continue; } - Pose3d targetPose = tmp.get(); + Pose3d targetPose = fiducialPose.get(); double alternativeDifference = Math.abs( p.getSecond().getZ() @@ -222,7 +209,7 @@ public class RobotPoseEstimator { targetPose .transformBy(target.getAlternateCameraToTarget().inverse()) .transformBy(p.getSecond().inverse()); - mili = result.getLatencyMillis(); + latency = result.getLatencyMillis(); } if (bestDifference < smallestHeightDifference) { smallestHeightDifference = bestDifference; @@ -230,12 +217,12 @@ public class RobotPoseEstimator { targetPose .transformBy(target.getBestCameraToTarget().inverse()) .transformBy(p.getSecond().inverse()); - mili = result.getLatencyMillis(); + latency = result.getLatencyMillis(); } } } - return Pair.of(pose, mili); + return Pair.of(pose, latency); } private Pair closestToReferencePoseStrategy() { @@ -243,10 +230,10 @@ public class RobotPoseEstimator { DriverStation.reportError( "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!", false); - return Pair.of(null, 0.); + return null; } double smallestDifference = 10e9; - double mili = 0; + double latency = 0; Pose3d pose = null; for (int i = 0; i < cameras.size(); i++) { Pair p = cameras.get(i); @@ -254,19 +241,12 @@ public class RobotPoseEstimator { List targets = result.targets; for (int j = 0; j < targets.size(); j++) { PhotonTrackedTarget target = targets.get(j); - // If the map doesn't contain the ID fail - var tmp = aprilTags.getTagPose(target.getFiducialId()); - if (tmp.isEmpty()) { - if (!reportedErrors.contains(target.getFiducialId())) { - DriverStation.reportWarning( - "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " - + target.getFiducialId(), - false); - reportedErrors.add(target.getFiducialId()); - } + Optional fiducialPose = aprilTags.getTagPose(target.getFiducialId()); + if (fiducialPose.isEmpty()) { + reportFiducialPoseError(target.getFiducialId()); continue; } - Pose3d targetPose = tmp.get(); + Pose3d targetPose = fiducialPose.get(); Pose3d botBestPose = targetPose .transformBy(target.getAlternateCameraToTarget().inverse()) @@ -280,16 +260,21 @@ public class RobotPoseEstimator { if (alternativeDifference < smallestDifference) { smallestDifference = alternativeDifference; pose = botAltPose; - mili = result.getLatencyMillis(); + latency = result.getLatencyMillis(); } if (bestDifference < smallestDifference) { smallestDifference = bestDifference; pose = botBestPose; - mili = result.getLatencyMillis(); + latency = result.getLatencyMillis(); } } } - return Pair.of(pose, mili); + return Pair.of(pose, latency); + } + + private Pair closestToLastPoseStrategy() { + setReferencePose(lastPose); + return closestToReferencePoseStrategy(); } /** Return the average of the best target poses using ambiguity as weight */ @@ -303,19 +288,12 @@ public class RobotPoseEstimator { List targets = result.targets; for (int j = 0; j < targets.size(); j++) { PhotonTrackedTarget target = targets.get(j); - // If the map doesn't contain the ID fail - var tmp = aprilTags.getTagPose(target.getFiducialId()); - if (tmp.isEmpty()) { - if (!reportedErrors.contains(target.getFiducialId())) { - DriverStation.reportWarning( - "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " - + target.getFiducialId(), - false); - reportedErrors.add(target.getFiducialId()); - } + Optional fiducialPose = aprilTags.getTagPose(target.getFiducialId()); + if (fiducialPose.isEmpty()) { + reportFiducialPoseError(target.getFiducialId()); continue; } - Pose3d targetPose = tmp.get(); + Pose3d targetPose = fiducialPose.get(); try { totalAmbiguity += 1. / target.getPoseAmbiguity(); } catch (ArithmeticException e) { @@ -339,25 +317,24 @@ public class RobotPoseEstimator { Rotation3d rotation = new Rotation3d(); double latency = 0; - if (tempPoses.size() > 0) { - for (Pair> pair : tempPoses) { - try { - double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity; - transform = transform.plus(pair.getFirst().getTranslation().times(weight)); - rotation = rotation.plus(pair.getFirst().getRotation().times(weight)); - latency += - pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well - } catch (ArithmeticException e) { - DriverStation.reportWarning( - "[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!", - false); - return Pair.of(pair.getFirst(), pair.getSecond().getSecond()); - } - } - return Pair.of(new Pose3d(transform, rotation), latency); - } else { - return new Pair(null, -1.0); + if (tempPoses.isEmpty()) { + return null; } + + if (totalAmbiguity == 0) { + Pose3d p = tempPoses.get(0).getFirst(); + double l = tempPoses.get(0).getSecond().getSecond(); + return Pair.of(p, l); + } + + for (Pair> pair : tempPoses) { + double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity; + transform = transform.plus(pair.getFirst().getTranslation().times(weight)); + rotation = rotation.plus(pair.getFirst().getRotation().times(weight)); + latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well + } + + return Pair.of(new Pose3d(transform, rotation), latency); } /** @@ -409,7 +386,7 @@ public class RobotPoseEstimator { * @param referencePose the referencePose to set */ public void setReferencePose(Pose2d referencePose) { - this.referencePose = new Pose3d(referencePose); + setReferencePose(new Pose3d(referencePose)); } /** @@ -420,4 +397,12 @@ public class RobotPoseEstimator { public void setLastPose(Pose3d lastPose) { this.lastPose = lastPose; } + + private void reportFiducialPoseError(int fiducialId) { + if (!reportedErrors.contains(fiducialId)) { + DriverStation.reportError( + "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " + fiducialId, false); + reportedErrors.add(fiducialId); + } + } } diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index 269232a67..e73d08430 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -27,7 +27,6 @@ package org.photonvision; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -139,13 +138,7 @@ public class SimVisionSystem { * PhotonVision parameters. */ public void processFrame(Pose2d robotPoseMeters) { - var robotPose3d = - new Pose3d( - robotPoseMeters.getX(), - robotPoseMeters.getY(), - 0.0, - new Rotation3d(0, 0, robotPoseMeters.getRotation().getRadians())); - processFrame(robotPose3d); + processFrame(new Pose3d(robotPoseMeters)); } /** diff --git a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp index 73d504c3e..47cfd48f1 100644 --- a/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -44,7 +45,7 @@ namespace photonlib { RobotPoseEstimator::RobotPoseEstimator( - std::map tags, PoseStrategy strat, + std::shared_ptr tags, PoseStrategy strat, std::vector, frc::Transform3d>> cams) : aprilTags(tags), @@ -112,7 +113,9 @@ RobotPoseEstimator::LowestAmbiguityStrategy() { PhotonTrackedTarget bestTarget = cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ]; - if (aprilTags.count(bestTarget.GetFiducialId()) == 0) { + std::optional fiducialPose = + aprilTags->GetTagPose(bestTarget.GetFiducialId()); + if (!fiducialPose) { FRC_ReportError(frc::warn::Warning, "Tried to get pose of unknown April Tag: {}", bestTarget.GetFiducialId()); @@ -120,7 +123,7 @@ RobotPoseEstimator::LowestAmbiguityStrategy() { } return std::make_pair( - aprilTags[bestTarget.GetFiducialId()] + fiducialPose.value() .TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) .TransformBy(cameras[lowestAI].second.Inverse()), cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.); @@ -138,13 +141,15 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() { p.first->GetLatestResult().GetTargets(); for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { PhotonTrackedTarget target = targets[j]; - if (aprilTags.count(target.GetFiducialId()) == 0) { + std::optional fiducialPose = + aprilTags->GetTagPose(target.GetFiducialId()); + if (!fiducialPose) { FRC_ReportError(frc::warn::Warning, "Tried to get pose of unknown April Tag: {}", target.GetFiducialId()); continue; } - frc::Pose3d targetPose = aprilTags[target.GetFiducialId()]; + frc::Pose3d targetPose = fiducialPose.value(); units::meter_t alternativeDifference = units::math::abs( p.second.Z() - targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) @@ -180,13 +185,15 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() { p.first->GetLatestResult().GetTargets(); for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { PhotonTrackedTarget target = targets[j]; - if (aprilTags.count(target.GetFiducialId()) == 0) { + std::optional fiducialPose = + aprilTags->GetTagPose(target.GetFiducialId()); + if (!fiducialPose) { FRC_ReportError(frc::warn::Warning, "Tried to get pose of unknown April Tag: {}", target.GetFiducialId()); continue; } - frc::Pose3d targetPose = aprilTags[target.GetFiducialId()]; + frc::Pose3d targetPose = fiducialPose.value(); units::meter_t alternativeDifference = units::math::abs(referencePose.Translation().Distance( targetPose @@ -225,14 +232,16 @@ RobotPoseEstimator::AverageBestTargetsStrategy() { p.first->GetLatestResult().GetTargets(); for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) { PhotonTrackedTarget target = targets[j]; - if (aprilTags.count(target.GetFiducialId()) == 0) { + std::optional fiducialPose = + aprilTags->GetTagPose(target.GetFiducialId()); + if (!fiducialPose) { FRC_ReportError(frc::warn::Warning, "Tried to get pose of unknown April Tag: {}", target.GetFiducialId()); continue; } - frc::Pose3d targetPose = aprilTags[target.GetFiducialId()]; + frc::Pose3d targetPose = fiducialPose.value(); if (target.GetPoseAmbiguity() == 0) { FRC_ReportError(frc::warn::Warning, "Pose ambiguity of zero exists, using that instead!", diff --git a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h index c049b22e2..a5493234e 100644 --- a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h @@ -34,6 +34,10 @@ #include "photonlib/PhotonCamera.h" +namespace frc { +class AprilTagFieldLayout; +} // namespace frc + namespace photonlib { enum PoseStrategy : int { LOWEST_AMBIGUITY, @@ -47,18 +51,18 @@ enum PoseStrategy : int { * A managing class to determine how an estimated pose should be chosen. */ class RobotPoseEstimator { + public: using map_value_type = std::pair, frc::Transform3d>; using size_type = std::vector::size_type; - public: - explicit RobotPoseEstimator(std::map aprilTags, - PoseStrategy strategy, - std::vector); + explicit RobotPoseEstimator( + std::shared_ptr aprilTags, + PoseStrategy strategy, std::vector cameras); std::pair Update(); - void SetPoseStrategy(PoseStrategy strategy); + inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; } inline void SetReferencePose(frc::Pose3d referencePose) { this->referencePose = referencePose; @@ -79,7 +83,7 @@ class RobotPoseEstimator { frc::Pose3d GetReferencePose() const { return referencePose; } private: - std::map aprilTags; + std::shared_ptr aprilTags; PoseStrategy strategy; std::vector cameras; frc::Pose3d lastPose; diff --git a/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java index 4345a0be4..9f04cbd7a 100644 --- a/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java @@ -42,6 +42,7 @@ import edu.wpi.first.util.WPIUtilJNI; import java.io.IOException; import java.util.ArrayList; import java.util.List; +import java.util.Optional; import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.photonvision.RobotPoseEstimator.PoseStrategy; @@ -138,10 +139,10 @@ class RobotPoseEstimatorTest { RobotPoseEstimator estimator = new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras); - Pair estimatedPose = estimator.update(); - Pose3d pose = estimatedPose.getFirst(); + Optional> estimatedPose = estimator.update(); + Pose3d pose = estimatedPose.get().getFirst(); - assertEquals(2, estimatedPose.getSecond()); + assertEquals(2, estimatedPose.get().getSecond()); assertEquals(1, pose.getX(), .01); assertEquals(3, pose.getY(), .01); assertEquals(2, pose.getZ(), .01); @@ -210,10 +211,10 @@ class RobotPoseEstimatorTest { RobotPoseEstimator estimator = new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras); - Pair estimatedPose = estimator.update(); - Pose3d pose = estimatedPose.getFirst(); + Optional> estimatedPose = estimator.update(); + Pose3d pose = estimatedPose.get().getFirst(); - assertEquals(2, estimatedPose.getSecond()); + assertEquals(2, estimatedPose.get().getSecond()); assertEquals(4, pose.getX(), .01); assertEquals(4, pose.getY(), .01); assertEquals(0, pose.getZ(), .01); @@ -283,10 +284,10 @@ class RobotPoseEstimatorTest { new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras); estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d())); - Pair estimatedPose = estimator.update(); - Pose3d pose = estimatedPose.getFirst(); + Optional> estimatedPose = estimator.update(); + Pose3d pose = estimatedPose.get().getFirst(); - assertEquals(4, estimatedPose.getSecond()); + assertEquals(4, estimatedPose.get().getSecond()); assertEquals(1, pose.getX(), .01); assertEquals(1.1, pose.getY(), .01); assertEquals(.9, pose.getZ(), .01); @@ -357,8 +358,8 @@ class RobotPoseEstimatorTest { estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d())); - Pair estimatedPose = estimator.update(); - Pose3d pose = estimatedPose.getFirst(); + Optional> estimatedPose = estimator.update(); + Pose3d pose = estimatedPose.get().getFirst(); cameraOne.result = new PhotonPipelineResult( @@ -412,9 +413,9 @@ class RobotPoseEstimatorTest { new TargetCorner(7, 8))))); estimatedPose = estimator.update(); - pose = estimatedPose.getFirst(); + pose = estimatedPose.get().getFirst(); - assertEquals(2, estimatedPose.getSecond()); + assertEquals(2, estimatedPose.get().getSecond()); assertEquals(.9, pose.getX(), .01); assertEquals(1.1, pose.getY(), .01); assertEquals(1, pose.getZ(), .01); @@ -483,9 +484,9 @@ class RobotPoseEstimatorTest { RobotPoseEstimator estimator = new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras); - Pair estimatedPose = estimator.update(); - Pose3d pose = estimatedPose.getFirst(); - assertEquals(2.6885245901639347, estimatedPose.getSecond(), .01); + Optional> estimatedPose = estimator.update(); + Pose3d pose = estimatedPose.get().getFirst(); + assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01); assertEquals(2.15, pose.getX(), .01); assertEquals(2.15, pose.getY(), .01); assertEquals(2.15, pose.getZ(), .01); diff --git a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp index 0b5c64476..8bd3f7d06 100644 --- a/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -40,11 +41,14 @@ #include "photonlib/RobotPoseEstimator.h" TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) { - std::map aprilTags; - aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), frc::Rotation3d())}); - aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), frc::Rotation3d())}); + std::vector tags = { + {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + frc::Rotation3d())}, + {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + frc::Rotation3d())}}; + + std::shared_ptr aprilTags = + std::make_shared(tags, 54_ft, 27_ft); std::vector< std::pair, frc::Transform3d>> @@ -122,11 +126,14 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) { } TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) { - std::map aprilTags; - aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), frc::Rotation3d())}); - aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), frc::Rotation3d())}); + std::vector tags = { + {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + frc::Rotation3d())}, + {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + frc::Rotation3d())}, + }; + std::shared_ptr aprilTags = + std::make_shared(tags, 54_ft, 27_ft); std::vector< std::pair, frc::Transform3d>> @@ -204,11 +211,14 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) { } TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) { - std::map aprilTags; - aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), frc::Rotation3d())}); - aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), frc::Rotation3d())}); + std::vector tags = { + {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + frc::Rotation3d())}, + {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + frc::Rotation3d())}, + }; + std::shared_ptr aprilTags = + std::make_shared(tags, 54_ft, 27_ft); std::vector< std::pair, frc::Transform3d>> @@ -288,11 +298,13 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) { } TEST(RobotPoseEstimatorTest, ClosestToLastPose) { - std::map aprilTags; - aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), frc::Rotation3d())}); - aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), frc::Rotation3d())}); + std::vector tags = { + {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + frc::Rotation3d())}, + {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + frc::Rotation3d())}}; + std::shared_ptr aprilTags = + std::make_shared(tags, 54_ft, 27_ft); std::vector< std::pair, frc::Transform3d>> @@ -431,11 +443,13 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) { } TEST(RobotPoseEstimatorTest, AverageBestPoses) { - std::map aprilTags; - aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3), - units::meter_t(3), frc::Rotation3d())}); - aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5), - units::meter_t(5), frc::Rotation3d())}); + std::vector tags = { + {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), + frc::Rotation3d())}, + {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5), + frc::Rotation3d())}}; + std::shared_ptr aprilTags = + std::make_shared(tags, 54_ft, 27_ft); std::vector< std::pair, frc::Transform3d>>