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

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

After changes:


![image](https://github.com/user-attachments/assets/0f58c2be-fc00-494c-af76-408c1ec438f9)

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
 ```

photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp

@@ -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);
+}

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::Vec3f>(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<photon::PnpResult> SolvePNP_Square(

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()