diff --git a/.gitignore b/.gitignore index e0aad645c..bf87286f8 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,8 @@ Python/app/handlers/__pycache__/ \.vscode/ +/.vs + backend/settings/ /.vscode/ # Compiled class file diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 2b94f3d26..be8311c5b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -26,12 +26,16 @@ package org.photonvision; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N5; import edu.wpi.first.wpilibj.DriverStation; import java.util.ArrayList; import java.util.HashSet; @@ -85,14 +89,14 @@ public class PhotonPoseEstimator { * Create a new PhotonPoseEstimator. * * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field + * with respect to the FIRST field using the Field * Coordinate System. * @param strategy The strategy it should use to determine the best pose. * @param camera PhotonCamera * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot + * robot ➔ camera) in the Robot * Coordinate System. */ public PhotonPoseEstimator( @@ -106,6 +110,14 @@ public class PhotonPoseEstimator { this.robotToCamera = robotToCamera; } + public PhotonPoseEstimator( + AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { + this.fieldTags = fieldTags; + this.primaryStrategy = strategy; + this.camera = null; + this.robotToCamera = robotToCamera; + } + /** Invalidates the pose cache. */ private void invalidatePoseCache() { poseCacheTimestampSeconds = -1; @@ -251,7 +263,7 @@ public class PhotonPoseEstimator { PhotonPipelineResult cameraResult = camera.getLatestResult(); - return update(cameraResult); + return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); } /** @@ -263,12 +275,33 @@ public class PhotonPoseEstimator { * pipeline results used to create the estimate */ public Optional update(PhotonPipelineResult cameraResult) { + if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty()); + return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); + } + + /** + * Updates the estimated position of the robot. Returns empty if there are no cameras set or no + * targets were found from the cameras. + * + * @param cameraResult The latest pipeline result from the camera + * @param cameraMatrixData Camera calibration data that can be used in the case of no assigned + * PhotonCamera. + * @param coeffsData Camera calibration data that can be used in the case of no assigned + * PhotonCamera + * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and + * pipeline results used to create the estimate + */ + public Optional update( + PhotonPipelineResult cameraResult, + Optional> cameraMatrixData, + Optional> coeffsData) { // Time in the past -- give up, since the following if expects times > 0 if (cameraResult.getTimestampSeconds() < 0) { return Optional.empty(); } - // If the pose cache timestamp was set, and the result is from the same timestamp, return an + // If the pose cache timestamp was set, and the result is from the same + // timestamp, return an // empty result if (poseCacheTimestampSeconds > 0 && Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) { @@ -283,11 +316,14 @@ public class PhotonPoseEstimator { return Optional.empty(); } - return update(cameraResult, this.primaryStrategy); + return update(cameraResult, cameraMatrixData, coeffsData, this.primaryStrategy); } private Optional update( - PhotonPipelineResult cameraResult, PoseStrategy strat) { + PhotonPipelineResult cameraResult, + Optional> cameraMatrixData, + Optional> coeffsData, + PoseStrategy strat) { Optional estimatedPose; switch (strat) { case LOWEST_AMBIGUITY: @@ -307,7 +343,7 @@ public class PhotonPoseEstimator { estimatedPose = averageBestTargetsStrategy(cameraResult); break; case MULTI_TAG_PNP: - estimatedPose = multiTagPNPStrategy(cameraResult); + estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrixData, coeffsData); break; default: DriverStation.reportError( @@ -322,7 +358,10 @@ public class PhotonPoseEstimator { return estimatedPose; } - private Optional multiTagPNPStrategy(PhotonPipelineResult result) { + private Optional multiTagPNPStrategy( + PhotonPipelineResult result, + Optional> cameraMatrixOpt, + Optional> distCoeffsOpt) { // Arrays we need declared up front var visCorners = new ArrayList(); var knownVisTags = new ArrayList(); @@ -331,7 +370,7 @@ public class PhotonPoseEstimator { if (result.getTargets().size() < 2) { // Run fallback strategy instead - return update(result, this.multiTagFallbackStrategy); + return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); } for (var target : result.getTargets()) { @@ -352,8 +391,6 @@ public class PhotonPoseEstimator { fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse())); } - var cameraMatrixOpt = camera.getCameraMatrix(); - var distCoeffsOpt = camera.getDistCoeffs(); boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); // multi-target solvePNP diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp index 5b7ff5485..155db6154 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp @@ -58,12 +58,23 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX, units::meter_t cornerY, frc::Pose3d tagPose); } // namespace detail +PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, + PoseStrategy strat, + frc::Transform3d robotToCamera) + : aprilTags(tags), + strategy(strat), + camera(nullptr), + m_robotToCamera(robotToCamera), + lastPose(frc::Pose3d()), + referencePose(frc::Pose3d()), + poseCacheTimestamp(-1_s) {} + PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, PoseStrategy strat, PhotonCamera&& cam, frc::Transform3d robotToCamera) : aprilTags(tags), strategy(strat), - camera(std::move(cam)), + camera(std::make_shared(std::move(cam))), m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), @@ -84,12 +95,27 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { } std::optional PhotonPoseEstimator::Update() { - auto result = camera.GetLatestResult(); - return Update(result); + if (!camera) { + FRC_ReportError(frc::warn::Warning, "[PhotonPoseEstimator] Missing camera!", + ""); + return std::nullopt; + } + auto result = camera->GetLatestResult(); + return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs()); } std::optional PhotonPoseEstimator::Update( const PhotonPipelineResult& result) { + // If camera is null, best we can do is pass null calibration data + if (!camera) { + return Update(result, std::nullopt, std::nullopt, this->strategy); + } + return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs()); +} + +std::optional PhotonPoseEstimator::Update( + const PhotonPipelineResult& result, std::optional cameraMatrixData, + std::optional coeffsData) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { return std::nullopt; @@ -110,11 +136,12 @@ std::optional PhotonPoseEstimator::Update( return std::nullopt; } - return Update(result, this->strategy); + return Update(result, cameraMatrixData, coeffsData, this->strategy); } std::optional PhotonPoseEstimator::Update( - PhotonPipelineResult result, PoseStrategy strategy) { + PhotonPipelineResult result, std::optional cameraMatrixData, + std::optional coeffsData, PoseStrategy strategy) { std::optional ret = std::nullopt; switch (strategy) { @@ -135,7 +162,7 @@ std::optional PhotonPoseEstimator::Update( ret = AverageBestTargetsStrategy(result); break; case ::photonlib::MULTI_TAG_PNP: - ret = MultiTagPnpStrategy(result); + ret = MultiTagPnpStrategy(result, coeffsData, cameraMatrixData); break; default: FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", @@ -328,11 +355,13 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { } std::optional PhotonPoseEstimator::MultiTagPnpStrategy( - PhotonPipelineResult result) { + PhotonPipelineResult result, std::optional camMat, + std::optional distCoeffs) { using namespace frc; if (!result.HasTargets() || result.GetTargets().size() < 2) { - return Update(result, this->multiTagFallbackStrategy); + return Update(result, std::nullopt, std::nullopt, + this->multiTagFallbackStrategy); } auto const targets = result.GetTargets(); @@ -364,8 +393,6 @@ std::optional PhotonPoseEstimator::MultiTagPnpStrategy( cv::Mat const rvec(3, 1, cv::DataType::type); cv::Mat const tvec(3, 1, cv::DataType::type); - auto const camMat = camera.GetCameraMatrix(); - auto const distCoeffs = camera.GetDistCoeffs(); if (!camMat || !distCoeffs) { return std::nullopt; } diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 5d76a851e..4ed212083 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -90,6 +90,23 @@ class PhotonPoseEstimator { PoseStrategy strategy, PhotonCamera&& camera, frc::Transform3d robotToCamera); + /** + * Create a new PhotonPoseEstimator. + * + *

Example: {@code

Map map = new HashMap<>(); + *

map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is + * at (1.0,2.0,3.0) } + * + * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with + * respect to the FIRST field. + * @param strategy The strategy it should use to determine the best pose. + * @param robotToCamera Transform3d from the center of the robot to the camera + * mount positions (ie, robot ➔ camera). + */ + explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, + PoseStrategy strategy, + frc::Transform3d robotToCamera); + /** * Get the AprilTagFieldLayout being used by the PositionEstimator. * @@ -181,14 +198,22 @@ class PhotonPoseEstimator { */ std::optional Update(const PhotonPipelineResult& result); - inline PhotonCamera& GetCamera() { return camera; } + /** + * Update the pose estimator. + */ + std::optional Update( + const PhotonPipelineResult& result, + std::optional cameraMatrixData, + std::optional coeffsData); + + inline std::shared_ptr GetCamera() { return camera; } private: frc::AprilTagFieldLayout aprilTags; PoseStrategy strategy; PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY; - PhotonCamera camera; + std::shared_ptr camera; frc::Transform3d m_robotToCamera; frc::Pose3d lastPose; @@ -198,8 +223,9 @@ class PhotonPoseEstimator { inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } - std::optional Update(PhotonPipelineResult result, - PoseStrategy strategy); + std::optional Update( + PhotonPipelineResult result, std::optional cameraMatrixData, + std::optional coeffsData, PoseStrategy strategy); /** * Return the estimated position of the robot with the lowest position @@ -242,7 +268,8 @@ class PhotonPoseEstimator { timestamp of this estimation. */ std::optional MultiTagPnpStrategy( - PhotonPipelineResult result); + PhotonPipelineResult result, std::optional camMat, + std::optional distCoeffs); /** * Return the average of the best target poses using ambiguity as weight. diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 7c67f02d1..42a6abf7d 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -257,8 +257,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; - estimator.GetCamera().testResult = {2_ms, targetsThree}; - estimator.GetCamera().testResult.SetTimestamp(units::second_t(21)); + estimator.GetCamera()->testResult = {2_ms, targetsThree}; + estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21)); estimatedPose = estimator.Update(); ASSERT_TRUE(estimatedPose); @@ -345,14 +345,14 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); // empty input, expect empty out - estimator.GetCamera().testResult = {2_ms, {}}; - estimator.GetCamera().testResult.SetTimestamp(units::second_t(1)); + estimator.GetCamera()->testResult = {2_ms, {}}; + estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1)); auto estimatedPose = estimator.Update(); EXPECT_FALSE(estimatedPose); // Set result, and update -- expect present and timestamp to be 15 - estimator.GetCamera().testResult = {3_ms, targets}; - estimator.GetCamera().testResult.SetTimestamp(units::second_t(15)); + estimator.GetCamera()->testResult = {3_ms, targets}; + estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15)); estimatedPose = estimator.Update(); EXPECT_TRUE(estimatedPose); EXPECT_NEAR(15, estimatedPose.value().timestamp.to(), 1e-6);