From 21aa96c002a9d9a3c5e7ee66f6b78045f1e5a4c1 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Sat, 11 Jan 2025 22:05:53 -0700 Subject: [PATCH] restore tests --- .../src/test/native/cpp/PhotonCameraTest.cpp | 54 ++ .../native/cpp/PhotonPoseEstimatorTest.cpp | 433 +++++++++++++ .../src/test/native/cpp/PhotonUtilsTest.cpp | 28 + .../src/test/native/cpp/VersionTest.cpp | 32 + .../test/native/cpp/VisionSystemSimTest.cpp | 576 ++++++++++++++++++ 5 files changed, 1123 insertions(+) create mode 100644 photon-lib/src/test/native/cpp/PhotonCameraTest.cpp create mode 100644 photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp create mode 100644 photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp create mode 100644 photon-lib/src/test/native/cpp/VersionTest.cpp create mode 100644 photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp new file mode 100644 index 0000000000..d795b653d0 --- /dev/null +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -0,0 +1,54 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include + +#include "photon/PhotonCamera.h" + +TEST(TimeSyncProtocolTest, Smoketest) { + using namespace wpi::tsp; + using namespace std::chrono_literals; + + // start a server implicitly + photon::PhotonCamera camera{"camera"}; + + TimeSyncClient client{"127.0.0.1", 5810, 100ms}; + client.Start(); + + for (int i = 0; i < 10; i++) { + std::this_thread::sleep_for(100ms); + TimeSyncClient::Metadata m = client.GetMetadata(); + + // give us time to warm up + if (i > 5) { + EXPECT_TRUE(m.rtt2 > 0); + EXPECT_TRUE(m.pongsReceived > 0); + } + } + + client.Stop(); +} diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp new file mode 100644 index 0000000000..0838c3e07a --- /dev/null +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -0,0 +1,433 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "photon/PhotonCamera.h" +#include "photon/PhotonPoseEstimator.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/PnpResult.h" + +static 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())}}; + +static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft}; + +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"); + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); + + photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, + frc::Transform3d{}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + ASSERT_TRUE(estimatedPose); + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), + .02); + EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); + EXPECT_NEAR(3, units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(2, units::unit_cast(pose.Z()), .01); +} + +TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { + 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())}, + }; + auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft); + + std::vector> cameras; + + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + // ID 0 at 3,3,3 + // ID 1 at 5,5,5 + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(17_s); + + photon::PhotonPoseEstimator estimator( + aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + ASSERT_TRUE(estimatedPose); + + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), + .02); + EXPECT_NEAR(4, units::unit_cast(pose.X()), .01); + EXPECT_NEAR(4, units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(0, units::unit_cast(pose.Z()), .01); +} + +TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); + + photon::PhotonPoseEstimator estimator(aprilTags, + photon::CLOSEST_TO_REFERENCE_POSE, {}); + estimator.SetReferencePose( + frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), + .01); + EXPECT_NEAR(1, units::unit_cast(pose.X()), .01); + EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(.9, units::unit_cast(pose.Z()), .01); +} + +TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); + + photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, + {}); + estimator.SetLastPose( + frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + std::vector targetsThree{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, + std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); + + // std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(21.0, units::unit_cast(estimatedPose.value().timestamp), + .01); + EXPECT_NEAR(.9, units::unit_cast(pose.X()), .01); + EXPECT_NEAR(1.1, units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(1, units::unit_cast(pose.Z()), .01); +} + +TEST(PhotonPoseEstimatorTest, AverageBestPoses) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); + + photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, + {}); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + frc::Pose3d pose = estimatedPose.value().estimatedPose; + + EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.value().timestamp), + .01); + EXPECT_NEAR(2.15, units::unit_cast(pose.X()), .01); + EXPECT_NEAR(2.15, units::unit_cast(pose.Y()), .01); + EXPECT_NEAR(2.15, units::unit_cast(pose.Z()), .01); +} + +TEST(PhotonPoseEstimatorTest, PoseCache) { + photon::PhotonCamera cameraOne = photon::PhotonCamera("test2"); + + std::vector targets{ + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.0, 4.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.7, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 3.0, -4.0, 9.1, 6.7, 1, -1, -1.f, + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.3, corners, detectedCorners}, + photon::PhotonTrackedTarget{ + 9.0, -2.0, 19.0, 3.0, 0, -1, -1.f, + frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m), + frc::Rotation3d(0_rad, 0_rad, 0_rad)), + 0.4, corners, detectedCorners}}; + + cameraOne.test = true; + + photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, + {}); + + // empty input, expect empty out + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, + std::vector{}, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); + + std::optional estimatedPose; + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + EXPECT_FALSE(estimatedPose); + + // Set result, and update -- expect present and timestamp to be 15 + cameraOne.testResult = {photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}}; + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); + + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + ASSERT_TRUE(estimatedPose); + EXPECT_NEAR((15_s - 3_ms).to(), + estimatedPose.value().timestamp.to(), 1e-6); + + // And again -- now pose cache should be empty + for (const auto& result : cameraOne.GetAllUnreadResults()) { + estimatedPose = estimator.Update(result); + } + + EXPECT_FALSE(estimatedPose); +} +TEST(PhotonPoseEstimatorTest, CopyResult) { + std::vector targets{}; + + auto testResult = photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}; + testResult.SetReceiveTimestamp(units::second_t(11)); + + auto test2 = testResult; + + EXPECT_NEAR(testResult.GetTimestamp().to(), + test2.GetTimestamp().to(), 0.001); +} diff --git a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp new file mode 100644 index 0000000000..95a14e259e --- /dev/null +++ b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp @@ -0,0 +1,28 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "gtest/gtest.h" +#include "photon/PhotonUtils.h" + +TEST(PhotonUtilsTest, Include) {} diff --git a/photon-lib/src/test/native/cpp/VersionTest.cpp b/photon-lib/src/test/native/cpp/VersionTest.cpp new file mode 100644 index 0000000000..4e547f6a37 --- /dev/null +++ b/photon-lib/src/test/native/cpp/VersionTest.cpp @@ -0,0 +1,32 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "PhotonVersion.h" +#include "gtest/gtest.h" + +TEST(VersionTest, PrintVersion) { + std::cout << photon::PhotonVersion::versionString << std::endl; +} diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp new file mode 100644 index 0000000000..88696414a2 --- /dev/null +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -0,0 +1,576 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include + +#include + +#include "gtest/gtest.h" +#include "photon/PhotonUtils.h" +#include "photon/simulation/VisionSystemSim.h" + +// Ignore GetLatestResult warnings +WPI_IGNORE_DEPRECATED + +class VisionSystemSimTest : public ::testing::Test { + void SetUp() override { + nt::NetworkTableInstance::GetDefault().StartServer(); + photon::PhotonCamera::SetVersionCheckEnabled(false); + } + + void TearDown() override {} +}; + +class VisionSystemSimTestWithParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface {}; +class VisionSystemSimTestDistanceParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface< + std::tuple> {}; + +TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); + + // To the right, to the right + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the right, to the right + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); + + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 5000_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + frc::Transform3d robotToCamera{ + frc::Translation3d{0_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); + + frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, + frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); + + frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(1.0); + cameraSim.SetMaxSightRange(10_m); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); + + frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+) + robotPose = + frc::Pose2d{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{GetParam()}}; + visionSysSim.Update(robotPose); + + const auto result = camera.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + ASSERT_NEAR(GetParam().to(), result.GetBestTarget().GetYaw(), 0.25); +} + +TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}}); + visionSysSim.Update(robotPose); + + const auto result = camera.GetLatestResult(); + ASSERT_TRUE(result.HasTargets()); + ASSERT_NEAR(GetParam().to(), result.GetBestTarget().GetPitch(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest, + testing::Values(-10_deg, -5_deg, -0_deg, -1_deg, + -2_deg, 5_deg, 7_deg, 10.23_deg)); + +TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { + units::foot_t distParam; + units::degree_t pitchParam; + units::foot_t heightParam; + std::tie(distParam, pitchParam, heightParam) = GetParam(); + + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}}; + frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; + frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, + frc::Rotation3d{0_deg, pitchParam, 0_deg}}; + photon::VisionSystemSim visionSysSim{ + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AdjustCamera(&cameraSim, robotToCamera); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}}); + visionSysSim.Update(robotPose); + + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + photon::PhotonTrackedTarget target = res.GetBestTarget(); + + ASSERT_NEAR(0.0, target.GetYaw(), 0.5); + + units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), -pitchParam, + units::degree_t{target.GetPitch()}); + ASSERT_NEAR(dist.to(), + distParam.convert().to(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceParamsTests, VisionSystemSimTestDistanceParamsTest, + testing::Values(std::make_tuple(5_ft, -15.98_deg, 0_ft), + std::make_tuple(6_ft, -15.98_deg, 1_ft), + std::make_tuple(10_ft, -15.98_deg, 0_ft), + std::make_tuple(15_ft, -15.98_deg, 2_ft), + std::make_tuple(19.95_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -15.98_deg, 0_ft), + std::make_tuple(5_ft, -42_deg, 1_ft), + std::make_tuple(6_ft, -42_deg, 0_ft), + std::make_tuple(10_ft, -42_deg, 2_ft), + std::make_tuple(15_ft, -42_deg, 0.5_ft), + std::make_tuple(19.42_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -42_deg, 0_ft), + std::make_tuple(5_ft, -55_deg, 2_ft), + std::make_tuple(6_ft, -55_deg, 0_ft), + std::make_tuple(10_ft, -54_deg, 2.2_ft), + std::make_tuple(15_ft, -53_deg, 0_ft), + std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft))); + +TEST_F(VisionSystemSimTest, TestMultipleTargets) { + frc::Pose3d targetPoseL{ + frc::Translation3d{15.98_m, 2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseC{ + frc::Translation3d{15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseR{ + frc::Translation3d{15.98_m, -2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 2}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 3}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 4}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 5}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 6}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 7}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 8}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 9}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 10}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 11}}); + + frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + visionSysSim.Update(robotPose); + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + std::span tgtList = res.GetTargets(); + ASSERT_EQ(static_cast(11), tgtList.size()); +} + +TEST_F(VisionSystemSimTest, TestPoseEstimation) { + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + std::vector tagList; + tagList.emplace_back(frc::AprilTag{ + 0, frc::Pose3d{12_m, 3_m, 1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 1, frc::Pose3d{12_m, 1_m, -1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 2, frc::Pose3d{11_m, 0_m, 2_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + units::meter_t fieldLength{54}; + units::meter_t fieldWidth{27}; + frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); + visionSysSim.Update(robotPose); + + Eigen::Matrix camEigen = camera.GetCameraMatrix().value(); + Eigen::Matrix distEigen = camera.GetDistCoeffs().value(); + + auto camResults = camera.GetLatestResult(); + auto targetSpan = camResults.GetTargets(); + std::vector targets; + for (photon::PhotonTrackedTarget tar : targetSpan) { + targets.push_back(tar); + } + auto results = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets, layout, photon::kAprilTag16h5); + 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); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose.Rotation().Z().to(), 0.01); + + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag16h5, 2}}); + visionSysSim.Update(robotPose); + + auto camResults2 = camera.GetLatestResult(); + auto targetSpan2 = camResults2.GetTargets(); + std::vector targets2; + for (photon::PhotonTrackedTarget tar : targetSpan2) { + targets2.push_back(tar); + } + auto results2 = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); + ASSERT_TRUE(results2); + frc::Pose3d pose2 = frc::Pose3d{} + results2->best; + ASSERT_NEAR(robotPose.X().to(), pose2.X().to(), 0.01); + ASSERT_NEAR(robotPose.Y().to(), pose2.Y().to(), 0.01); + ASSERT_NEAR(0, pose2.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); +} + +TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { + frc::Transform3d robotToCamera{frc::Translation3d{6_in, 6_in, 6_in}, + frc::Rotation3d{0_deg, -30_deg, 25.5_deg}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"cameraRotated"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + std::vector tagList; + tagList.emplace_back(frc::AprilTag{ + 0, frc::Pose3d{12_m, 3_m, 1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 1, frc::Pose3d{12_m, 1_m, -1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 2, frc::Pose3d{11_m, 0_m, 2_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + units::meter_t fieldLength{54}; + units::meter_t fieldWidth{27}; + frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{-5_deg}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag36h11, 0}}); + visionSysSim.Update(robotPose); + + Eigen::Matrix camEigen = camera.GetCameraMatrix().value(); + Eigen::Matrix distEigen = camera.GetDistCoeffs().value(); + + auto camResults = camera.GetLatestResult(); + auto targetSpan = camResults.GetTargets(); + std::vector targets; + for (photon::PhotonTrackedTarget tar : targetSpan) { + targets.push_back(tar); + } + auto results = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets, layout, photon::kAprilTag36h11); + ASSERT_TRUE(results); + frc::Pose3d pose = frc::Pose3d{} + results->best; + pose = pose.TransformBy(robotToCamera.Inverse()); + ASSERT_NEAR(5, pose.X().to(), 0.01); + ASSERT_NEAR(1, pose.Y().to(), 0.01); + ASSERT_NEAR(0, pose.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{-5}.convert().to(), + pose.Rotation().Z().to(), 0.01); + + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag36h11, 1}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag36h11, 2}}); + visionSysSim.Update(robotPose); + + auto camResults2 = camera.GetLatestResult(); + auto targetSpan2 = camResults2.GetTargets(); + std::vector targets2; + for (photon::PhotonTrackedTarget tar : targetSpan2) { + targets2.push_back(tar); + } + auto results2 = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets2, layout, photon::kAprilTag36h11); + ASSERT_TRUE(results2); + frc::Pose3d pose2 = frc::Pose3d{} + results2->best; + pose2 = pose2.TransformBy(robotToCamera.Inverse()); + ASSERT_NEAR(robotPose.X().to(), pose2.X().to(), 0.01); + ASSERT_NEAR(robotPose.Y().to(), pose2.Y().to(), 0.01); + ASSERT_NEAR(0, pose2.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{-5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); +} + +TEST_F(VisionSystemSimTest, TestTagAmbiguity) { + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + frc::Pose3d targetPose{ + frc::Translation3d{2_m, 0_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}}); + + frc::Pose2d robotPose{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{0_deg}}; + visionSysSim.Update(robotPose); + double ambiguity = + camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity(); + ASSERT_TRUE(ambiguity > 0.5); + + robotPose = + frc::Pose2d{frc::Translation2d{-2_m, -2_m}, frc::Rotation2d{30_deg}}; + visionSysSim.Update(robotPose); + ambiguity = camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity(); + ASSERT_TRUE(0 < ambiguity && ambiguity < 0.2); +}