Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-29 02:21:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix fallback for the multitag on rio pose strategy (#1755)

Browse Source
This commit is contained in:
Joseph Eng
2025-02-01 14:09:43 -08:00
committed by GitHub
parent 8ec2da952f
commit 99427d888a
5 changed files with 184 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -24,6 +24,7 @@
package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -645,6 +646,72 @@ class PhotonPoseEstimatorTest {
assertEquals(2.15, pose.getZ(), .01);
}
@Test
void testMultiTagOnRioFallback() {
PhotonCameraInjector camera = new PhotonCameraInjector();
camera.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
0,
-1,
-1,
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
0.7,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
1,
-1,
-1,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.3,
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)),
List.of(
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(camera.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(
() -> assertEquals(11, estimatedPose.get().timestampSeconds),
() -> assertEquals(1, pose.getX(), 1e-9),
() -> assertEquals(3, pose.getY(), 1e-9),
() -> assertEquals(2, pose.getZ(), 1e-9));
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");

43
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -419,6 +419,49 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
std::vector<photon::PhotonTrackedTarget> 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}};
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<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};