diff --git a/docs/source/docs/contributing/building-photon.md b/docs/source/docs/contributing/building-photon.md index 3ae9e1989..ec2c92cbe 100644 --- a/docs/source/docs/contributing/building-photon.md +++ b/docs/source/docs/contributing/building-photon.md @@ -290,5 +290,5 @@ Then, run the examples: Using the [GitHub CLI](https://cli.github.com/), we can download artifacts from pipelines by run ID and name: ``` -~/photonvision$ gh run download 11759699679 -n jar-Linux +~/photonvision$ gh run download 11759699679 -n jar-Linux ``` diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index 07fa3b89a..06bf25e6d 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -471,3 +471,79 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) { 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); +} 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 09bb4a08b..37a5d962b 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -201,9 +201,8 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F}; rvecInput.convertTo(wrapped, CV_32F); data = wrapped.at(cv::Point{0, 0}); - return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]}, - units::radian_t{data[1]}, - units::radian_t{data[2]}}); + return RotationEDNToNWU( + frc::Rotation3d{Eigen::Vector3d{data[0], data[1], data[2]}}); } [[maybe_unused]] static std::optional SolvePNP_Square( diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h index 99e19bf2c..5c08f937f 100644 --- a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -26,11 +26,14 @@ namespace photon { class RotTrlTransform3d { public: - RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl) - : trl(trl), rot(rot) {} + RotTrlTransform3d(const frc::Rotation3d& newRot, + const frc::Translation3d& newTrl) + : trl{newTrl}, rot{newRot} {} + RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last) - : trl(last.Translation() - initial.Translation().RotateBy(rot)), - rot(last.Rotation() - initial.Rotation()) {} + : trl{last.Translation() - initial.Translation().RotateBy( + last.Rotation() - initial.Rotation())}, + rot{last.Rotation() - initial.Rotation()} {} explicit RotTrlTransform3d(const frc::Transform3d& trf) : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} RotTrlTransform3d()