From 2ec688be156b4301d899ddfed92211b34f50c09f Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 4 Aug 2024 10:44:59 -0700 Subject: [PATCH] Fix tests --- .../cpp/photon/simulation/PhotonCameraSim.cpp | 31 ++++++------ .../native/cpp/PhotonPoseEstimatorTest.cpp | 49 ++++++++++++------- .../test/native/cpp/VisionSystemSimTest.cpp | 10 ++-- .../photon/dataflow/structures/Packet.h | 6 ++- .../include/photon/estimation/OpenCVHelp.h | 23 +++++++++ .../photon/estimation/VisionEstimation.h | 17 ++----- 6 files changed, 82 insertions(+), 54 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index 0153f9d3a4..96e641ce4b 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -210,18 +210,17 @@ PhotonPipelineResult PhotonCameraSim::Process( std::vector> tempCorners = OpenCVHelp::PointsToCorners(minAreaRectPts); - wpi::SmallVector, 4> smallVec; + std::vector smallVec; for (const auto& corner : tempCorners) { - smallVec.emplace_back(std::make_pair(static_cast(corner.first), - static_cast(corner.second))); + smallVec.emplace_back(static_cast(corner.first), + static_cast(corner.second)); } - std::vector> cornersFloat = - OpenCVHelp::PointsToCorners(noisyTargetCorners); + auto cornersFloat = OpenCVHelp::PointsToTargetCorners(noisyTargetCorners); - std::vector> cornersDouble{cornersFloat.begin(), - cornersFloat.end()}; + std::vector cornersDouble{cornersFloat.begin(), + cornersFloat.end()}; detectableTgts.emplace_back( -centerRot.Z().convert().to(), -centerRot.Y().convert().to(), areaPercent, @@ -277,17 +276,15 @@ PhotonPipelineResult PhotonCameraSim::Process( cv::LINE_AA); for (const auto& tgt : detectableTgts) { auto detectedCornersDouble = tgt.GetDetectedCorners(); - std::vector> detectedCornerFloat{ - detectedCornersDouble.begin(), detectedCornersDouble.end()}; if (tgt.GetFiducialId() >= 0) { VideoSimUtil::DrawTagDetection( tgt.GetFiducialId(), - OpenCVHelp::CornersToPoints(detectedCornerFloat), + OpenCVHelp::CornersToPoints(detectedCornersDouble), videoSimFrameProcessed); } else { cv::rectangle(videoSimFrameProcessed, OpenCVHelp::GetBoundingRect( - OpenCVHelp::CornersToPoints(detectedCornerFloat)), + OpenCVHelp::CornersToPoints(detectedCornersDouble)), cv::Scalar{0, 0, 255}, static_cast(VideoSimUtil::GetScaledThickness( 1, videoSimFrameProcessed)), @@ -316,7 +313,7 @@ PhotonPipelineResult PhotonCameraSim::Process( cs::VideoSource::ConnectionStrategy::kConnectionForceClose); } - MultiTargetPNPResult multiTagResults{}; + std::optional multiTagResults = std::nullopt; std::vector visibleLayoutTags = VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); @@ -330,12 +327,16 @@ PhotonPipelineResult PhotonCameraSim::Process( auto pnpResult = VisionEstimation::EstimateCamPosePNP( prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, kAprilTag36h11); - multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; + if (pnpResult) { + multiTagResults = MultiTargetPNPResult{*pnpResult, usedIds}; + } } heartbeatCounter++; - return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts, - multiTagResults}; + return PhotonPipelineResult{ + PhotonPipelineMetadata{heartbeatCounter, 0, + units::microsecond_t{latency}.to()}, + detectableTgts, multiTagResults}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 72d1ccf7d6..a1c901c15b 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -51,15 +51,17 @@ static std::vector tags = { static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; -static wpi::SmallVector, 4> corners{ - std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; -static std::vector> detectedCorners{ - std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; +static std::vector corners{ + photon::TargetCorner{1, 2}, photon::TargetCorner{3, 4}, + photon::TargetCorner{5, 6}, photon::TargetCorner{7, 8}}; +static std::vector detectedCorners{ + photon::TargetCorner{1, 2}, photon::TargetCorner{3, 4}, + photon::TargetCorner{5, 6}, photon::TargetCorner{7, 8}}; TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ + std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), @@ -83,7 +85,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {{0, 0_s, 2_ms, targets}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -118,7 +121,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { // ID 0 at 3,3,3 // ID 1 at 5,5,5 - wpi::SmallVector targets{ + std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), @@ -142,7 +145,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {{0, 0_s, 2_ms, targets}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -165,7 +169,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ + std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), @@ -189,7 +193,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {{0, 0_s, 2_ms, targets}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -214,7 +219,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ + std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), @@ -238,7 +243,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {{0, 0_s, 2_ms, targets}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -254,7 +260,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; - wpi::SmallVector targetsThree{ + std::vector targetsThree{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, -1, -1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), @@ -277,7 +283,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.4, corners, detectedCorners}}; - cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -298,7 +305,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { TEST(PhotonPoseEstimatorTest, AverageBestPoses) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ + std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), @@ -322,7 +329,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { 0.4, corners, detectedCorners}}; cameraOne.test = true; - cameraOne.testResult = {{0, 0_s, 2_ms, targets}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -345,7 +353,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { TEST(PhotonPoseEstimatorTest, PoseCache) { photon::PhotonCamera cameraOne = photon::PhotonCamera("test2"); - wpi::SmallVector targets{ + std::vector targets{ photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, -1, -1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), @@ -374,7 +382,9 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { {}); // empty input, expect empty out - cameraOne.testResult = {{0, 0_s, 2_ms, {}}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, + std::vector{}, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -385,7 +395,8 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { EXPECT_FALSE(estimatedPose); // Set result, and update -- expect present and timestamp to be 15 - cameraOne.testResult = {{0, 0_s, 3_ms, targets}}; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}}; cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index 8398a37a69..e57c91c4f0 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -439,9 +439,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { for (photon::PhotonTrackedTarget tar : targetSpan) { targets.push_back(tar); } - photon::PnpResult results = photon::VisionEstimation::EstimateCamPosePNP( + auto results = photon::VisionEstimation::EstimateCamPosePNP( camEigen, distEigen, targets, layout, photon::kAprilTag16h5); - frc::Pose3d pose = frc::Pose3d{} + results.best; + ASSERT_TRUE(results); + frc::Pose3d pose = frc::Pose3d{} + results->best; ASSERT_NEAR(5, pose.X().to(), 0.01); ASSERT_NEAR(1, pose.Y().to(), 0.01); ASSERT_NEAR(0, pose.Z().to(), 0.01); @@ -460,9 +461,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { for (photon::PhotonTrackedTarget tar : targetSpan2) { targets2.push_back(tar); } - photon::PnpResult results2 = photon::VisionEstimation::EstimateCamPosePNP( + auto results2 = photon::VisionEstimation::EstimateCamPosePNP( camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); - frc::Pose3d pose2 = frc::Pose3d{} + results2.best; + ASSERT_TRUE(results2); + frc::Pose3d pose2 = frc::Pose3d{} + results2->best; ASSERT_NEAR(5, pose2.X().to(), 0.01); ASSERT_NEAR(1, pose2.Y().to(), 0.01); ASSERT_NEAR(0, pose2.Z().to(), 0.01); diff --git a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h index 488ada0298..acd28a2caa 100644 --- a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h +++ b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h @@ -170,7 +170,8 @@ struct SerdeType> { static constexpr std::string_view GetSchema() { // TODO: this gets us the plain type name of T, but this is not schema JSON // compliant! - return wpi::Demangle(typeid(T).name()); + std::string name = wpi::Demangle(typeid(T).name()); + return name; } }; @@ -199,7 +200,8 @@ struct SerdeType> { static constexpr std::string_view GetSchema() { // TODO: this gets us the plain type name of T, but this is not schema JSON // compliant! - return wpi::Demangle(typeid(T).name()); + std::string name = wpi::Demangle(typeid(T).name()); + return name; } }; 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 794883e2c6..ebb6a9fffc 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -32,6 +32,8 @@ #include "photon/targeting/PnpResult.h" #include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/TargetCorner.h" + namespace photon { namespace OpenCVHelp { @@ -96,6 +98,16 @@ static std::vector RotationToRVec( return points[0]; } +[[maybe_unused]] static std::vector PointsToTargetCorners( + const std::vector& points) { + std::vector retVal; + retVal.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + retVal.emplace_back(photon::TargetCorner{points[i].x, points[i].y}); + } + return retVal; +} + [[maybe_unused]] static std::vector> PointsToCorners( const std::vector& points) { std::vector> retVal; @@ -116,6 +128,17 @@ static std::vector RotationToRVec( return retVal; } +[[maybe_unused]] static std::vector CornersToPoints( + const std::vector& corners) { + std::vector retVal; + retVal.reserve(corners.size()); + for (size_t i = 0; i < corners.size(); i++) { + retVal.emplace_back(cv::Point2f{static_cast(corners[i].x), + static_cast(corners[i].y)}); + } + return retVal; +} + [[maybe_unused]] static cv::Rect GetBoundingRect( const std::vector& points) { return cv::boundingRect(points); 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 53df691598..bb621f4f83 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -56,7 +56,7 @@ static std::optional EstimateCamPosePNP( return PnpResult(); } - std::vector> corners{}; + std::vector corners{}; std::vector knownTags{}; for (const auto& tgt : visTags) { @@ -101,19 +101,8 @@ static std::optional EstimateCamPosePNP( auto verts = tagModel.GetFieldVertices(tag.pose); objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); } - auto camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, - objectTrls, points); - if (!camToOrigin) { - return std::nullopt; - } else { - PnpResult ret{}; - ret.best = camToOrigin.best.Inverse(), - ret.alt = camToOrigin.alt.Inverse(), - ret.ambiguity = camToOrigin.ambiguity; - ret.bestReprojErr = camToOrigin.bestReprojErr; - ret.altReprojErr = camToOrigin.altReprojErr; - return ret; - } + return OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, + points); } }