Foobar

Author: mcm001

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -47,6 +47,7 @@
 import org.opencv.core.RotatedRect;
 import org.opencv.core.Scalar;
 import org.opencv.core.Size;
+import org.opencv.imgcodecs.Imgcodecs;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonTargetSortMode;
@@ -522,6 +523,7 @@ public PhotonPipelineResult process(
                 }
             }
             videoSimRaw.putFrame(videoSimFrameRaw);
+            Imgcodecs.imwrite("videoSimFrameRaw.png", videoSimFrameRaw);
         } else videoSimRaw.setConnectionStrategy(ConnectionStrategy.kForceClose);
         // draw/annotate target detection outline on processed view
         if (videoSimProcEnabled) {
@@ -558,6 +560,7 @@ public PhotonPipelineResult process(
                 }
             }
             videoSimProcessed.putFrame(videoSimFrameProcessed);
+            Imgcodecs.imwrite("videoSimFrameProcessed.png", videoSimFrameProcessed);
         } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
 
         // calculate multitag results

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -66,6 +66,9 @@
 import org.photonvision.targeting.PnpResult;
 import org.photonvision.targeting.TargetCorner;
 
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
+
 class PhotonPoseEstimatorTest {
     static AprilTagFieldLayout aprilTags;
 
@@ -931,42 +934,48 @@ public PhotonPipelineResult getLatestResult() {
 
 
     @Test
-    public void meme() {
+    public void meme() throws JsonProcessingException {
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
 
+        var layout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
         List<VisionTargetSim> simTargets =
-                aprilTags.getTags().stream()
+                layout.getTags().stream()
                         .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
                         .toList();
 
         /* Compound Rolled + Pitched + Yaw */
 
-        Transform3d compoundTestTransform =
+        Transform3d robot2camera =
                 new Transform3d(
                         -Units.inchesToMeters(12),
                         -Units.inchesToMeters(11),
-                        3,
+                        0.5,
                         new Rotation3d(
-                                Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60)));
+                                Units.degreesToRadians(0), Units.degreesToRadians(6), Units.degreesToRadians(0)));
 
         var estimator =
                 new PhotonPoseEstimator(
-                        aprilTags, PoseStrategy.CONSTRAINED_SOLVEPNP, compoundTestTransform);
+                        layout, PoseStrategy.CONSTRAINED_SOLVEPNP, robot2camera);
 
         /* this is the real pose of the robot base we test against */
-        var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
+        var realPose = new Pose3d(1, 4, 0, new Rotation3d(0, 0, 0));
 
         PhotonCameraSim cameraOneSim =
-                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG());
+                new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG(), layout);
+        cameraOneSim.setMinTargetAreaPercent(0.01);
+        cameraOneSim.enableDrawWireframe(true);
         PhotonPipelineResult result =
                 cameraOneSim.process(
                         1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
         cameraOneSim.submitProcessedFrame(result);
 
+        assertTrue(result.targets.size() > 0);
+
         estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
         Optional<EstimatedRobotPose> estimatedPose = estimator.update(result, cameraOne.getCameraMatrix(), cameraOne.getDistCoeffs(), Optional.of(new ConstrainedSolvepnpParams(true, 0)));
 
-        System.out.println("Ground truth: " + realPose);
+        System.out.println("Ground truth field2robot:\n" + new ObjectMapper().writeValueAsString(realPose.toMatrix().getStorage().getDDRM()));
+        System.out.println("Ground truth robot2camera:\n" + new ObjectMapper().writeValueAsString(robot2camera.toMatrix().getStorage().getDDRM()));
 
         System.out.println(estimatedPose);
     }

photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java

@@ -30,10 +30,13 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.*;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 import java.util.Objects;
 import java.util.Optional;
 import java.util.stream.Collectors;
+
+import org.ejml.data.DMatrixRMaj;
 import org.ejml.simple.SimpleMatrix;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.MatOfDouble;
@@ -150,13 +153,19 @@ public static Optional<PnpResult> estimateCamPosePNP(
 
     private static class DebugData {
         public double[] cameraCal;
-        public SimpleMatrix field2points;
-        public SimpleMatrix point_observations;
+        public DMatrixRMaj field2points;
+        public DMatrixRMaj point_observations;
 
         public DebugData(double[] cameraCal, SimpleMatrix field2points, SimpleMatrix point_observations) {
             this.cameraCal = cameraCal;
-            this.field2points = field2points;
-            this.point_observations = point_observations;
+            this.field2points = field2points.getDDRM();
+            this.point_observations = point_observations.getDDRM();
+        }
+
+        @Override
+        public String toString() {
+            return "DebugData [cameraCal=" + Arrays.toString(cameraCal) + ", field2points=" + field2points
+                    + ", point_observations=" + point_observations + "]";
         }
     }
 
@@ -282,6 +291,7 @@ public static Optional<PnpResult> estimateRobotPoseConstrainedSolvepnp(
         } catch (JsonProcessingException e) {
             System.err.println("Failed to serialize debug data: " + e.getMessage());
         }
+        System.out.println(debugData);
 
         var ret =
                 ConstrainedSolvepnpJni.do_optimization(