This... code was actually important?

PhotonVision · Aug 18, 2024 · b05387f · b05387f
1 parent 49af6b9
commit b05387f
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 10 deletions.
diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -423,7 +423,7 @@ public PhotonPipelineResult process(
             var pnpSim = new PnpResult();
             if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
                 pnpSim =
-                        OpenCVHelp.solvePNP_SQUARE(
+                        OpenCVHelp.solvePNP_SQPNP(
                                         prop.getIntrinsics(),
                                         prop.getDistCoeffs(),
                                         tgt.getModel().vertices,

diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
@@ -210,9 +210,11 @@ std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
   auto camCoeffs = cameraIntrinsicsSubscriber.Get();
   if (camCoeffs.size() == 9) {
     PhotonCamera::CameraMatrix retVal =
-        Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
+        Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
+            camCoeffs.data());
     return retVal;
   }
+
   return std::nullopt;
 }
 

diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
@@ -203,7 +203,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
 
     std::optional<photon::PnpResult> pnpSim = std::nullopt;
     if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
-      pnpSim = OpenCVHelp::SolvePNP_Square(
+      pnpSim = OpenCVHelp::SolvePNP_SQPNP(
           prop.GetIntrinsics(), prop.GetDistCoeffs(),
           tgt.GetModel().GetVertices(), noisyTargetCorners);
     }
@@ -371,9 +371,10 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
     ts.targetPoseEntry.Set(poseData, recieveTimestamp);
   }
 
-  auto intrinsics = prop.GetIntrinsics();
-  std::vector<double> intrinsicsView{intrinsics.data(),
-                                     intrinsics.data() + intrinsics.size()};
+  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> intrinsics =
+      prop.GetIntrinsics();
+  std::span<double> intrinsicsView{intrinsics.data(),
+                                   intrinsics.data() + intrinsics.size()};
   ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
 
   auto distortion = prop.GetDistCoeffs();

diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
@@ -465,8 +465,8 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
       camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
   ASSERT_TRUE(results2);
   frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
-  ASSERT_NEAR(5, pose2.X().to<double>(), 0.01);
-  ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
+  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);

diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
@@ -47,6 +47,8 @@ static std::vector<frc::AprilTag> GetVisibleLayoutTags(
   return retVal;
 }
 
+#include <iostream>
+
 static std::optional<PnpResult> EstimateCamPosePNP(
     const Eigen::Matrix<double, 3, 3>& cameraMatrix,
     const Eigen::Matrix<double, 8, 1>& distCoeffs,
@@ -101,8 +103,15 @@ static std::optional<PnpResult> EstimateCamPosePNP(
       auto verts = tagModel.GetFieldVertices(tag.pose);
       objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
     }
-    return OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
-                                      points);
+    auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
+                                          points);
+    if (ret) {
+      // Invert best/alt transforms
+      ret->best = ret->best.Inverse();
+      ret->alt = ret->alt.Inverse();
+    }
+
+    return ret;
   }
 }