diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 2b67dd2d57..95b563b97e 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -48,6 +48,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" @@ -97,7 +99,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, @@ -120,13 +123,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; @@ -159,6 +164,13 @@ std::optional PhotonPoseEstimator::Update( ""); } break; + case CONSTRAINED_SOLVEPNP: + ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs, + constrainedPnpParams); + break; + case PNP_DISTANCE_TRIG_SOLVE: + ret = PnpDistanceTrigSolveStrategy(result); + break; default: FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!", ""); @@ -475,4 +487,123 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS}; } + +std::optional +PhotonPoseEstimator::PnpDistanceTrigSolveStrategy( + photon::PhotonPipelineResult result) { + photon::PhotonTrackedTarget bestTarget = result.GetBestTarget(); + + if (bestTarget.fiducialId == -1) { + return {}; + } + + if (!headingBuffer.Sample(result.GetTimestamp()).has_value()) { + return {}; + } + + frc::Rotation2d headingSample{ + headingBuffer.Sample(result.GetTimestamp()).value()}; + + frc::Translation2d camToTagTranslation{ + frc::Translation3d{ + bestTarget.GetBestCameraToTarget().Translation().Norm(), + frc::Rotation3d{0_deg, -units::degree_t{bestTarget.GetPitch()}, + -units::degree_t{bestTarget.GetYaw()}}} + .RotateBy(m_robotToCamera.Rotation()) + .ToTranslation2d() + .RotateBy(headingSample)}; + + std::optional tagPose = + aprilTags.GetTagPose(bestTarget.GetFiducialId()); + + if (!tagPose.has_value()) { + return {}; + } + + frc::Pose2d tagPose2d{tagPose->ToPose2d()}; + + frc::Translation2d fieldToCameraTranslation{tagPose2d.Translation() + + (-camToTagTranslation)}; + + frc::Translation2d camToRobotTranslation{ + (-m_robotToCamera.Translation().ToTranslation2d()) + .RotateBy(headingSample)}; + + frc::Pose2d robotPose{fieldToCameraTranslation + camToRobotTranslation, + headingSample}; + + return EstimatedRobotPose{frc::Pose3d{robotPose}, result.GetTimestamp(), + result.GetTargets(), + PoseStrategy::PNP_DISTANCE_TRIG_SOLVE}; +} + +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 7b129a666e..96df4d4ee5 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -28,7 +28,9 @@ #include #include +#include #include +#include #include #include "photon/PhotonCamera.h" @@ -47,6 +49,13 @@ enum PoseStrategy { AVERAGE_BEST_TARGETS, MULTI_TAG_PNP_ON_COPROCESSOR, MULTI_TAG_PNP_ON_RIO, + CONSTRAINED_SOLVEPNP, + PNP_DISTANCE_TRIG_SOLVE +}; + +struct ConstrainedSolvepnpParams { + bool headingFree{false}; + double headingScalingFactor{0.0}; }; struct EstimatedRobotPose { @@ -182,11 +191,24 @@ 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); + + void AddHeadingData(units::second_t timestamp, frc::Rotation3d heading) { + AddHeadingData(timestamp, heading.ToRotation2d()); + } + + void AddHeadingData(units::second_t timestamp, frc::Rotation2d heading) { + headingBuffer.AddSample(timestamp, heading.Radians()); + } private: frc::AprilTagFieldLayout aprilTags; @@ -198,6 +220,8 @@ class PhotonPoseEstimator { frc::Pose3d lastPose; frc::Pose3d referencePose; + frc::TimeInterpolatableBuffer headingBuffer{1_s}; + units::second_t poseCacheTimestamp; inline static int InstanceCount = 0; @@ -216,13 +240,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); /** @@ -286,6 +311,15 @@ class PhotonPoseEstimator { */ std::optional AverageBestTargetsStrategy( PhotonPipelineResult result); + + std::optional PnpDistanceTrigSolveStrategy( + photon::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 c95fa57ed0..4be65e964c 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -38,6 +38,8 @@ #include "photon/PhotonCamera.h" #include "photon/PhotonPoseEstimator.h" #include "photon/dataflow/structures/Packet.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" @@ -474,3 +476,164 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { EXPECT_NEAR(testResult.GetTimestamp().to(), test2.GetTimestamp().to(), 0.001); } + +TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + photon::SimCameraProperties cameraSim = + photon::SimCameraProperties::PERFECT_90DEG(); + + std::vector simTargets; + for (const auto& tag : aprilTags.GetTags()) { + simTargets.push_back( + photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID)); + } + + frc::Transform3d compoundTestTransform( + frc::Translation3d(-12_in, -11_in, 3_m), + frc::Rotation3d(37_deg, 6_deg, 60_deg)); + + photon::PhotonPoseEstimator estimator( + aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); + + frc::Pose3d realPose(7.3_m, 4.42_m, 0_m, + frc::Rotation3d(0_rad, 0_rad, 2.197_rad)); + + std::vector targets{photon::PhotonTrackedTarget{ + -20.0, 15.0, 0.0, 0.1, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2_m, 1_m, 0.5_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1_m, 0.5_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.0, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 0, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(1_s); + + estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), + realPose.Rotation().ToRotation2d()); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(units::unit_cast(realPose.X()), + units::unit_cast(pose.X()), 0.1); + EXPECT_NEAR(units::unit_cast(realPose.Y()), + units::unit_cast(pose.Y()), 0.1); + EXPECT_NEAR(0.0, units::unit_cast(pose.Z()), 0.1); + + frc::Transform3d straightOnTestTransform( + frc::Translation3d(0_m, 0_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)); + + estimator.SetRobotToCameraTransform(straightOnTestTransform); + + realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m, + frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); + + targets = {photon::PhotonTrackedTarget{ + -15.0, 10.0, 0.0, 0.1, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(1.5_m, 0.8_m, 0.3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1.5_m, 0.8_m, 0.3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.0, corners, detectedCorners}}; + + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 0, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(2_s); + + estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), + realPose.Rotation().ToRotation2d()); + + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(units::unit_cast(realPose.X()), + units::unit_cast(pose.X()), 0.1); + EXPECT_NEAR(units::unit_cast(realPose.Y()), + units::unit_cast(pose.Y()), 0.1); + EXPECT_NEAR(0.0, units::unit_cast(pose.Z()), 0.1); +} + +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/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h index 37a5d962b5..fad60f2c81 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 7afdc13f3e..95eaea0654 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -26,6 +26,7 @@ #include "OpenCVHelp.h" #include "TargetModel.h" +#include "photon/constrained_solvepnp/wrap/casadi_wrapper.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonTrackedTarget.h" @@ -114,5 +115,109 @@ namespace VisionEstimation { } } +[[maybe_unused]] static 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