diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 28d40a41a..6903240c9 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -49,6 +49,8 @@ #include #include "photon/PhotonCamera.h" +#include "photon/estimation/TargetModel.h" +#include "photon/estimation/VisionEstimation.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" @@ -99,7 +101,8 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { std::optional PhotonPoseEstimator::Update( const PhotonPipelineResult& result, std::optional cameraMatrixData, - std::optional cameraDistCoeffs) { + std::optional cameraDistCoeffs, + std::optional constrainedPnpParams) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { FRC_ReportError(frc::warn::Warning, @@ -122,13 +125,15 @@ std::optional PhotonPoseEstimator::Update( return std::nullopt; } - return Update(result, cameraMatrixData, cameraDistCoeffs, this->strategy); + return Update(result, cameraMatrixData, cameraDistCoeffs, + constrainedPnpParams, this->strategy); } std::optional PhotonPoseEstimator::Update( const PhotonPipelineResult& result, std::optional cameraMatrixData, std::optional cameraDistCoeffs, + std::optional constrainedPnpParams, PoseStrategy strategy) { std::optional ret = std::nullopt; @@ -161,6 +166,10 @@ std::optional PhotonPoseEstimator::Update( ""); } break; + case CONSTRAINED_SOLVEPNP: + ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs, + constrainedPnpParams); + break; case PNP_DISTANCE_TRIG_SOLVE: ret = PnpDistanceTrigSolveStrategy(result); break; @@ -527,4 +536,74 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS}; } + +std::optional PhotonPoseEstimator::ConstrainedPnpStrategy( + photon::PhotonPipelineResult result, + std::optional camMat, + std::optional distCoeffs, + std::optional constrainedPnpParams) { + using namespace frc; + + if (!camMat || !distCoeffs) { + FRC_ReportError(frc::warn::Warning, + "No camera calibration data provided to " + "StrPoseEstimator::MultiTagOnRioStrategy!", + ""); + return Update(result, this->multiTagFallbackStrategy); + } + + if (!constrainedPnpParams) { + return {}; + } + + if (!constrainedPnpParams->headingFree && + !headingBuffer.Sample(result.GetTimestamp()).has_value()) { + return Update(result, camMat, distCoeffs, {}, + this->multiTagFallbackStrategy); + } + + frc::Pose3d fieldToRobotSeed; + + if (result.MultiTagResult().has_value()) { + fieldToRobotSeed = + frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best + + m_robotToCamera.Inverse()); + } else { + std::optional nestedUpdate = + Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy); + + if (!nestedUpdate.has_value()) { + return {}; + } + + fieldToRobotSeed = nestedUpdate->estimatedPose; + } + + if (!constrainedPnpParams.value().headingFree) { + fieldToRobotSeed = frc::Pose3d{ + fieldToRobotSeed.Translation(), + frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}}; + } + + std::vector targets{result.GetTargets().begin(), + result.GetTargets().end()}; + + std::optional pnpResult = + VisionEstimation::EstimateRobotPoseConstrainedSolvePNP( + camMat.value(), distCoeffs.value(), targets, m_robotToCamera, + fieldToRobotSeed, aprilTags, photon::kAprilTag36h11, + constrainedPnpParams->headingFree, + frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()}, + constrainedPnpParams->headingScalingFactor); + + if (!pnpResult) { + return Update(result, camMat, distCoeffs, {}, + this->multiTagFallbackStrategy); + } + + frc::Pose3d best = frc::Pose3d{} + pnpResult->best; + + return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(), + PoseStrategy::CONSTRAINED_SOLVEPNP}; +} } // namespace photon diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 7998ab718..f14865a15 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -48,7 +49,13 @@ enum PoseStrategy { AVERAGE_BEST_TARGETS, MULTI_TAG_PNP_ON_COPROCESSOR, MULTI_TAG_PNP_ON_RIO, - PNP_DISTANCE_TRIG_SOLVE, + CONSTRAINED_SOLVEPNP, + PNP_DISTANCE_TRIG_SOLVE +}; + +struct ConstrainedSolvepnpParams { + bool headingFree{false}; + double headingScalingFactor{0.0}; }; struct EstimatedRobotPose { @@ -239,11 +246,16 @@ class PhotonPoseEstimator { * Only required if doing multitag-on-rio, and may be nullopt otherwise. * @param distCoeffsData The camera calibration distortion coefficients. Only * required if doing multitag-on-rio, and may be nullopt otherwise. + * @param constrainedPnpParams Constrained SolvePNP params, if needed. */ std::optional Update( - const PhotonPipelineResult& result, - std::optional cameraMatrixData = std::nullopt, - std::optional coeffsData = std::nullopt); + const photon::PhotonPipelineResult& result, + std::optional cameraMatrixData = + std::nullopt, + std::optional coeffsData = + std::nullopt, + std::optional constrainedPnpParams = + std::nullopt); private: frc::AprilTagFieldLayout aprilTags; @@ -275,13 +287,14 @@ class PhotonPoseEstimator { */ std::optional Update(const PhotonPipelineResult& result, PoseStrategy strategy) { - return Update(result, std::nullopt, std::nullopt, strategy); + return Update(result, std::nullopt, std::nullopt, std::nullopt, strategy); } std::optional Update( const PhotonPipelineResult& result, std::optional cameraMatrixData, std::optional coeffsData, + std::optional constrainedPnpParams, PoseStrategy strategy); /** @@ -355,6 +368,12 @@ class PhotonPoseEstimator { */ std::optional AverageBestTargetsStrategy( PhotonPipelineResult result); + + std::optional ConstrainedPnpStrategy( + photon::PhotonPipelineResult result, + std::optional camMat, + std::optional distCoeffs, + std::optional constrainedPnpParams); }; } // namespace photon diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 74eac6e43..5f1e95703 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -40,6 +40,7 @@ #include "photon/dataflow/structures/Packet.h" #include "photon/simulation/PhotonCameraSim.h" #include "photon/simulation/SimCameraProperties.h" +#include "photon/simulation/VisionTargetSim.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" @@ -583,3 +584,76 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { EXPECT_NEAR(testResult.GetTimestamp().to(), test2.GetTimestamp().to(), 0.001); } + +TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { + photon::PhotonPoseEstimator estimator( + frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), + photon::CONSTRAINED_SOLVEPNP, frc::Transform3d()); + + photon::PhotonPipelineResult result; + auto estimate = estimator.Update(result); + EXPECT_FALSE(estimate.has_value()); +} + +TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + auto distortion = Eigen::VectorXd::Zero(8); + auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5}, + {0, 399.16666666666674, 239.5}, + {0, 0, 1}}; + + // Create corners data matching the Java test + std::vector corners8{ + photon::TargetCorner{98.09875447066685, 331.0093220119495}, + photon::TargetCorner{122.20226758624413, 335.50083894738486}, + photon::TargetCorner{127.17118732489361, 313.81406314178633}, + photon::TargetCorner{104.28543773760417, 309.6516557438994}}; + + frc::Transform3d poseTransform( + frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m, + 0.48678786477534686_m), + frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333, + -0.08413452932300695, + 0.9130568172784148))); + + std::vector targets{ + photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform, + poseTransform, 0.0, corners8, corners8}}; + + auto multiTagResult = std::make_optional( + photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0}, + std::vector{8}); + + photon::PhotonPipelineResult result{ + photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, + multiTagResult}; + + cameraOne.test = true; + cameraOne.testResult = {result}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); + + const units::radian_t camPitch = 30_deg; + const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), + frc::Rotation3d(0_rad, -camPitch, 0_rad)}; + + photon::PhotonPoseEstimator estimator( + frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), + photon::CONSTRAINED_SOLVEPNP, kRobotToCam); + + estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), + frc::Rotation2d()); + + auto estimatedPose = + estimator.Update(cameraOne.testResult[0], cameraMat, distortion, + photon::ConstrainedSolvepnpParams{true, 0}); + + ASSERT_TRUE(estimatedPose.has_value()); + + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(3.58, units::unit_cast(pose.X()), 0.01); + EXPECT_NEAR(4.13, units::unit_cast(pose.Y()), 0.01); + EXPECT_NEAR(0.0, units::unit_cast(pose.Z()), 0.01); + + EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy); +} diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index dd2f628d1..1bf977f79 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -72,6 +72,8 @@ model { nativeUtils.useRequiredLibrary(it, "wpimath_shared") nativeUtils.useRequiredLibrary(it, "wpinet_shared") nativeUtils.useRequiredLibrary(it, "ntcore_shared") + nativeUtils.useRequiredLibrary(it, "apriltag_shared") + nativeUtils.useRequiredLibrary(it, "opencv_shared") } "${nativeName}JNI"(JniNativeLibrarySpec) { diff --git a/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp b/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp new file mode 100644 index 000000000..2349a5831 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/estimation/VisionEstimation.cpp @@ -0,0 +1,215 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "photon/estimation/VisionEstimation.h" + +#include +#include +#include + +#include "photon/constrained_solvepnp/wrap/casadi_wrapper.h" +#include "photon/estimation/OpenCVHelp.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +namespace photon { +namespace VisionEstimation { + +std::vector GetVisibleLayoutTags( + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout) { + std::vector retVal{}; + for (const auto& tag : visTags) { + int id = tag.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); + } + } + return retVal; +} + +std::optional EstimateCamPosePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { + if (visTags.size() == 0) { + return PnpResult(); + } + + std::vector corners{}; + std::vector knownTags{}; + + for (const auto& tgt : visTags) { + int id = tgt.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + auto currentCorners = tgt.GetDetectedCorners(); + corners.insert(corners.end(), currentCorners.begin(), + currentCorners.end()); + } + } + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return PnpResult{}; + } + + std::vector points = OpenCVHelp::CornersToPoints(corners); + + if (knownTags.size() == 1) { + auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs, + tagModel.GetVertices(), points); + if (!camToTag) { + return PnpResult{}; + } + frc::Pose3d bestPose = + knownTags[0].pose.TransformBy(camToTag->best.Inverse()); + frc::Pose3d altPose{}; + if (camToTag->ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse()); + } + frc::Pose3d o{}; + PnpResult result{}; + result.best = frc::Transform3d{o, bestPose}; + result.alt = frc::Transform3d{o, altPose}; + result.ambiguity = camToTag->ambiguity; + result.bestReprojErr = camToTag->bestReprojErr; + result.altReprojErr = camToTag->altReprojErr; + return result; + } else { + std::vector objectTrls{}; + for (const auto& tag : knownTags) { + auto verts = tagModel.GetFieldVertices(tag.pose); + objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); + } + auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, + points); + if (ret) { + // Invert best/alt transforms + ret->best = ret->best.Inverse(); + ret->alt = ret->alt.Inverse(); + } + + return ret; + } +} + +std::optional EstimateRobotPoseConstrainedSolvePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed, + const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel, + bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac) { + if (visTags.size() == 0) { + return photon::PnpResult(); + } + + std::vector corners{}; + std::vector knownTags{}; + + for (const auto& tgt : visTags) { + int id = tgt.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + auto currentCorners = tgt.GetDetectedCorners(); + corners.insert(corners.end(), currentCorners.begin(), + currentCorners.end()); + } + } + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return photon::PnpResult{}; + } + + std::vector points = + photon::OpenCVHelp::CornersToPoints(corners); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + cv::undistortImagePoints(points, points, cameraMat, distCoeffsMat); + + Eigen::Matrix4d robotToCameraBase{ + (Eigen::Matrix4d() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1) + .finished()}; + Eigen::Matrix + robotToCamera{robot2Camera.ToMatrix() * robotToCameraBase}; + + Eigen::Matrix + pointObservations{}; + pointObservations.resize(2, points.size()); + for (size_t i = 0; i < points.size(); i++) { + pointObservations(0, i) = points[i].x; + pointObservations(1, i) = points[i].y; + } + + std::vector objectTrls{}; + for (const auto& tag : knownTags) { + auto verts = tagModel.GetFieldVertices(tag.pose); + objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); + } + + Eigen::Matrix + field2points{}; + field2points.resize(4, objectTrls.size()); + for (size_t i = 0; i < objectTrls.size(); i++) { + field2points(0, i) = objectTrls[i].X().value(); + field2points(1, i) = objectTrls[i].Y().value(); + field2points(2, i) = objectTrls[i].Z().value(); + field2points(3, i) = 1; + } + + frc::Pose2d guess2 = robotPoseSeed.ToPose2d(); + + constrained_solvepnp::CameraCalibration cameraCal{ + cameraMatrix(0, 0), + cameraMatrix(1, 1), + cameraMatrix(0, 2), + cameraMatrix(1, 2), + }; + + Eigen::Matrix guessMat{ + guess2.X().value(), guess2.Y().value(), + guess2.Rotation().Radians().value()}; + + wpi::expected + result = constrained_solvepnp::do_optimization( + headingFree, knownTags.size(), cameraCal, robotToCamera, guessMat, + field2points, pointObservations, gyroTheta.Radians().value(), + gyroErrorScaleFac); + + if (!result.has_value()) { + return {}; + } else { + photon::PnpResult res{}; + + res.best = frc::Transform3d{frc::Transform2d{ + units::meter_t{result.value()[0]}, units::meter_t{result.value()[1]}, + frc::Rotation2d{units::radian_t{result.value()[2]}}}}; + + return res; + } +} + +} // namespace VisionEstimation +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h index 37a5d962b..fad60f2c8 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -42,7 +42,7 @@ static frc::Rotation3d NWU_TO_EDN{ static frc::Rotation3d EDN_TO_NWU{ (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()}; -static std::vector GetConvexHull( +[[maybe_unused]] static std::vector GetConvexHull( const std::vector& points) { std::vector outputHull{}; cv::convexHull(points, outputHull); diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h index 7afdc13f3..e9445ed8f 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -17,102 +17,36 @@ #pragma once -#include #include #include #include #include -#include "OpenCVHelp.h" #include "TargetModel.h" -#include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/PnpResult.h" namespace photon { namespace VisionEstimation { -[[maybe_unused]] static std::vector GetVisibleLayoutTags( +std::vector GetVisibleLayoutTags( const std::vector& visTags, - const frc::AprilTagFieldLayout& layout) { - std::vector retVal{}; - for (const auto& tag : visTags) { - int id = tag.GetFiducialId(); - auto maybePose = layout.GetTagPose(id); - if (maybePose) { - retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); - } - } - return retVal; -} + const frc::AprilTagFieldLayout& layout); -#include - -[[maybe_unused]] static std::optional EstimateCamPosePNP( +std::optional EstimateCamPosePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, - const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { - if (visTags.size() == 0) { - return PnpResult(); - } + const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel); - std::vector corners{}; - std::vector knownTags{}; - - for (const auto& tgt : visTags) { - int id = tgt.GetFiducialId(); - auto maybePose = layout.GetTagPose(id); - if (maybePose) { - knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); - auto currentCorners = tgt.GetDetectedCorners(); - corners.insert(corners.end(), currentCorners.begin(), - currentCorners.end()); - } - } - if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return PnpResult{}; - } - - std::vector points = OpenCVHelp::CornersToPoints(corners); - - if (knownTags.size() == 1) { - auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs, - tagModel.GetVertices(), points); - if (!camToTag) { - return PnpResult{}; - } - frc::Pose3d bestPose = - knownTags[0].pose.TransformBy(camToTag->best.Inverse()); - frc::Pose3d altPose{}; - if (camToTag->ambiguity != 0) { - altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse()); - } - frc::Pose3d o{}; - PnpResult result{}; - result.best = frc::Transform3d{o, bestPose}; - result.alt = frc::Transform3d{o, altPose}; - result.ambiguity = camToTag->ambiguity; - result.bestReprojErr = camToTag->bestReprojErr; - result.altReprojErr = camToTag->altReprojErr; - return result; - } else { - std::vector objectTrls{}; - for (const auto& tag : knownTags) { - auto verts = tagModel.GetFieldVertices(tag.pose); - objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); - } - auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, - points); - if (ret) { - // Invert best/alt transforms - ret->best = ret->best.Inverse(); - ret->alt = ret->alt.Inverse(); - } - - return ret; - } -} +std::optional EstimateRobotPoseConstrainedSolvePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed, + const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel, + bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac); } // namespace VisionEstimation } // namespace photon