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

[photonli?b C++] Fix rotation3d constructor (#1553)

Browse Source 
Fixes #1552 -- mixed up rpy and a rotation vector 

After changes:


![image](https://github.com/user-attachments/assets/0f58c2be-fc00-494c-af76-408c1ec438f9)
This commit is contained in:
Drew Williams
2024-11-11 11:06:00 -05:00
committed by GitHub
parent 5bee683661
commit 1fc93bd05d
4 changed files with 86 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -471,3 +471,79 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 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<frc::AprilTag> 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<double, 3, 3> camEigen = camera.GetCameraMatrix().value();
Eigen::Matrix<double, 8, 1> distEigen = camera.GetDistCoeffs().value();
auto camResults = camera.GetLatestResult();
auto targetSpan = camResults.GetTargets();
std::vector<photon::PhotonTrackedTarget> 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<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 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<photon::PhotonTrackedTarget> 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<double>(), pose2.X().to<double>(), 0.01);
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
}