Moved Robot to camera transform to PhotonCamera

docs/source/docs/contributing/design-descriptions/index.md

@@ -6,4 +6,5 @@ image-rotation
 time-sync
 camera-matching
 e2e-latency
+robot-to-camera
 ```

docs/source/docs/contributing/design-descriptions/robot-to-camera.md

@@ -0,0 +1,14 @@
+# Robot to Camera
+
+## How 3D pose estimation works
+
+At its core, Photonvision's 3D pose estimation is built around solving the Perspective-n-Point (PnP) problem. The PnP problem is essentially, 'given a set of points in 3D space, and their projections on a 2D image, determine the pose of the camera'. In photonvision's case, the points are the corners of one or more apriltags.
+However, this leaves us with the camera's pose, *not* the robot's pose. The solution, of course, is to apply an offset. For 3D Pose estimation, Photonvision associates to each camera a `Transform3d` object, representing a vector encoding the 6DOF transformation from the robot (or, rather, the point on the robot considered to be its center) to the camera, and the final step of pose estimation is to transform the camera's pose (that got spit out of the PnP solver) by the inverse of this vector, yielding an estimate of the robot's pose.
+
+## What does the plumbing look like?
+
+The `PhotonCamera` object is (optionally) passed a `Transform3d` object by the Robot code, corresponding to the robot-to-camera transformation. This `Transform3d` (if passed) is transmitted to the coprocessor by the `PhotonCamera`. The coprocessor then appends that `Transform3d` to every result it sends back to the robot controller, where the `PhotonPoseEstimator` object then applies the transformation to the camera pose (If no `Transform3d` is passed to `PhotonCamera`, then no `Transform3d` is appended to the results, and `PhotonPoseEstimator` simply won't work. The result stores the `Transform3d` as an optional)
+
+## That seems complicated and silly. Why not just keep the robot to camera transform in PhotonLib?
+
+We used to! However, a new algorithm for fusing gyroscopic data to PnP pose observations has been added to PhotonVision, Constrained PnP, offering significantly improved accuracy and stability. Notably, this algorithm requires the robot-to-camera transform to run properly. In previous seasons, this wasn't a problem, as Constrained PnP ran entirely on the RoboRIO anyways, in order to access robot gyro data. However, Constrained PnP is very computationally expensive, and so work is being done to offload the work to the coprocessor, and expose gyro data to the coprocessor. The switch to having the robot-to-camera transform exposed to the coprocessor over NetworkTables and becoming an integrated part of Photonvision results is to facilitate the offloading of the Constrained PnP workload (and other estimation workloads that require Gyroscopic data) to the coprocessor

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -18,6 +18,7 @@
 package org.photonvision.common.dataflow.networktables;
 
 import java.util.List;
+import java.util.function.BiConsumer;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
@@ -35,6 +36,7 @@
 import org.wpilib.networktables.NetworkTable;
 import org.wpilib.networktables.NetworkTableEvent;
 import org.wpilib.networktables.NetworkTablesJNI;
+import org.wpilib.networktables.TimestampedObject;
 
 public class NTDataPublisher implements CVPipelineResultConsumer {
     private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General);
@@ -55,20 +57,25 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
     private final Consumer<Integer> fpsLimitConsumer;
     private final Supplier<Integer> fpsLimitSupplier;
 
+    NTDataChangeListener robotToCameraListener;
+    private final BiConsumer<Double, Transform3d> robotToCameraConsumer;
+
     public NTDataPublisher(
             String cameraNickname,
             Supplier<Integer> pipelineIndexSupplier,
             Consumer<Integer> pipelineIndexConsumer,
             BooleanSupplier driverModeSupplier,
             Consumer<Boolean> driverModeConsumer,
             Supplier<Integer> fpsLimitSupplier,
-            Consumer<Integer> fpsLimitConsumer) {
+            Consumer<Integer> fpsLimitConsumer,
+            BiConsumer<Double, Transform3d> robotToCameraConsumer) {
         this.pipelineIndexSupplier = pipelineIndexSupplier;
         this.pipelineIndexConsumer = pipelineIndexConsumer;
         this.driverModeSupplier = driverModeSupplier;
         this.driverModeConsumer = driverModeConsumer;
         this.fpsLimitSupplier = fpsLimitSupplier;
         this.fpsLimitConsumer = fpsLimitConsumer;
+        this.robotToCameraConsumer = robotToCameraConsumer;
 
         updateCameraNickname(cameraNickname);
         updateEntries();
@@ -98,6 +105,15 @@ private void onPipelineIndexChange(NetworkTableEvent entryNotification) {
         logger.debug("Set pipeline index to " + newIndex);
     }
 
+    private void onRobotToCameraChange(NetworkTableEvent entryNotification) {
+        // HACK: the entryNotification's value can't be cast to Transform3d, so we read directly from
+        // the subscriber
+        TimestampedObject<Transform3d> robotToCamera = ts.robotToCameraSubscriber.getAtomic(null);
+        var doubleTime = (robotToCamera.serverTime) * (double) 1e6;
+        robotToCameraConsumer.accept(doubleTime, robotToCamera.value);
+        logger.debug("Sampled robot to camera transform as " + robotToCamera + " at t=" + doubleTime);
+    }
+
     private void onDriverModeChange(NetworkTableEvent entryNotification) {
         var newDriverMode = entryNotification.valueData.value.getBoolean();
         var originalDriverMode = driverModeSupplier.getAsBoolean();
@@ -134,6 +150,7 @@ private void updateEntries() {
         if (pipelineIndexListener != null) pipelineIndexListener.remove();
         if (driverModeListener != null) driverModeListener.remove();
         if (fpsLimitListener != null) fpsLimitListener.remove();
+        if (robotToCameraListener != null) robotToCameraListener.remove();
 
         ts.updateEntries();
 
@@ -148,6 +165,10 @@ private void updateEntries() {
         fpsLimitListener =
                 new NTDataChangeListener(
                         ts.subTable.getInstance(), ts.fpsLimitSubscriber, this::onFPSLimitChange);
+
+        robotToCameraListener =
+                new NTDataChangeListener(
+                        ts.subTable.getInstance(), ts.robotToCameraSubscriber, this::onRobotToCameraChange);
     }
 
     public void updateCameraNickname(String newCameraNickname) {
@@ -187,7 +208,8 @@ public void accept(CVPipelineResult result) {
                         now + offset,
                         NetworkTablesManager.getInstance().getTimeSinceLastPong(),
                         TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
-                        acceptedResult.multiTagResult);
+                        acceptedResult.multiTagResult,
+                        acceptedResult.robotToCamera);
 
         // random guess at size of the array
         ts.resultPublisher.set(simplified, 1024);

photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java

@@ -57,10 +57,6 @@ public FileFrameProvider(Path path, double fov, int maxFPS) {
         this(path, fov, maxFPS, null);
     }
 
-    public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients calibration) {
-        this(path, fov, MAX_FPS, calibration);
-    }
-
     public FileFrameProvider(
             Path path, double fov, int maxFPS, CameraCalibrationCoefficients calibration) {
         if (!Files.exists(path))
@@ -97,6 +93,10 @@ public FileFrameProvider(Path path, double fov) {
         this(path, fov, MAX_FPS);
     }
 
+    public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients calibration) {
+        this(path, fov, MAX_FPS, calibration);
+    }
+
     @Override
     public CapturedFrame getInputMat() {
         var out = new CVMat();

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -20,6 +20,7 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import java.util.function.Function;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.logging.LogGroup;
@@ -63,12 +64,13 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
 
     public AprilTagPipeline() {
-        super(PROCESSING_TYPE);
+        super(PROCESSING_TYPE, (time) -> null);
         settings = new AprilTagPipelineSettings();
     }
 
-    public AprilTagPipeline(AprilTagPipelineSettings settings) {
-        super(PROCESSING_TYPE);
+    public AprilTagPipeline(
+            AprilTagPipelineSettings settings, Function<Long, Transform3d> robotToCameraSampler) {
+        super(PROCESSING_TYPE, robotToCameraSampler);
         this.settings = settings;
     }
 
@@ -247,7 +249,13 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         var fps = fpsResult.output;
 
         return new CVPipelineResult(
-                frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+                frame.sequenceID,
+                sumPipeNanosElapsed,
+                fps,
+                targetList,
+                multiTagResult,
+                frame,
+                Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos)));
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java

@@ -20,6 +20,7 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import java.util.function.Function;
 import org.opencv.core.Mat;
 import org.opencv.imgproc.Imgproc;
 import org.opencv.objdetect.Objdetect;
@@ -55,12 +56,13 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
 
     public ArucoPipeline() {
-        super(FrameThresholdType.GREYSCALE);
+        super(FrameThresholdType.GREYSCALE, (time) -> null);
         settings = new ArucoPipelineSettings();
     }
 
-    public ArucoPipeline(ArucoPipelineSettings settings) {
-        super(FrameThresholdType.GREYSCALE);
+    public ArucoPipeline(
+            ArucoPipelineSettings settings, Function<Long, Transform3d> robotToCameraSampler) {
+        super(FrameThresholdType.GREYSCALE, robotToCameraSampler);
         this.settings = settings;
     }
 
@@ -235,7 +237,13 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         var fps = fpsResult.output;
 
         return new CVPipelineResult(
-                frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+                frame.sequenceID,
+                sumPipeNanosElapsed,
+                fps,
+                targetList,
+                multiTagResult,
+                frame,
+                Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos)));
     }
 
     private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) {

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java

@@ -17,26 +17,31 @@
 
 package org.photonvision.vision.pipeline;
 
+import java.util.function.Function;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.wpilib.math.geometry.Transform3d;
 
 public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings>
         implements Releasable {
     protected S settings;
     protected FrameStaticProperties frameStaticProperties;
     protected QuirkyCamera cameraQuirks;
+    protected Function<Long, Transform3d> robotToCameraSampler;
 
     private final FrameThresholdType thresholdType;
 
     // So releaseable doesn't keep track of if we double-free something. so (ew) remember that here
     protected volatile boolean released = false;
 
-    public CVPipeline(FrameThresholdType thresholdType) {
+    public CVPipeline(
+            FrameThresholdType thresholdType, Function<Long, Transform3d> robotToCameraSampler) {
         this.thresholdType = thresholdType;
+        this.robotToCameraSampler = robotToCameraSampler;
     }
 
     public FrameThresholdType getThresholdType() {

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -74,7 +74,7 @@ public Calibrate3dPipeline() {
     }
 
     public Calibrate3dPipeline(int minSnapshots) {
-        super(PROCESSING_TYPE);
+        super(PROCESSING_TYPE, (time) -> null);
         this.settings = new Calibration3dPipelineSettings();
         this.foundCornersList = new ArrayList<>();
         this.minSnapshots = minSnapshots;

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -19,6 +19,8 @@
 
 import java.util.Arrays;
 import java.util.List;
+import java.util.Optional;
+import java.util.function.Function;
 import org.opencv.core.Point;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
@@ -31,6 +33,7 @@
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.PotentialTarget;
 import org.photonvision.vision.target.TrackedTarget;
+import org.wpilib.math.geometry.Transform3d;
 import org.wpilib.math.util.Pair;
 
 public class ColoredShapePipeline
@@ -54,12 +57,13 @@ public class ColoredShapePipeline
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.HSV;
 
     public ColoredShapePipeline() {
-        super(PROCESSING_TYPE);
+        super(PROCESSING_TYPE, (time) -> null);
         settings = new ColoredShapePipelineSettings();
     }
 
-    public ColoredShapePipeline(ColoredShapePipelineSettings settings) {
-        super(PROCESSING_TYPE);
+    public ColoredShapePipeline(
+            ColoredShapePipelineSettings settings, Function<Long, Transform3d> robotToCameraSampler) {
+        super(PROCESSING_TYPE, robotToCameraSampler);
         this.settings = settings;
     }
 
@@ -213,6 +217,12 @@ protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings set
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(frame.sequenceID, sumPipeNanosElapsed, fps, targetList, frame);
+        return new CVPipelineResult(
+                frame.sequenceID,
+                sumPipeNanosElapsed,
+                fps,
+                targetList,
+                frame,
+                Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos)));
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -18,13 +18,15 @@
 package org.photonvision.vision.pipeline;
 
 import java.util.List;
+import java.util.function.Function;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
 import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe;
 import org.photonvision.vision.pipe.impl.ResizeImagePipe;
 import org.photonvision.vision.pipeline.result.DriverModePipelineResult;
+import org.wpilib.math.geometry.Transform3d;
 import org.wpilib.math.util.Pair;
 
 public class DriverModePipeline
@@ -36,12 +38,13 @@ public class DriverModePipeline
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
 
     public DriverModePipeline() {
-        super(PROCESSING_TYPE);
+        super(PROCESSING_TYPE, (time) -> null);
         settings = new DriverModePipelineSettings();
     }
 
-    public DriverModePipeline(DriverModePipelineSettings settings) {
-        super(PROCESSING_TYPE);
+    public DriverModePipeline(
+            DriverModePipelineSettings settings, Function<Long, Transform3d> robotToCameraSampler) {
+        super(PROCESSING_TYPE, robotToCameraSampler);
         this.settings = settings;
     }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.pipeline;
 
+import java.util.function.Function;
 import org.opencv.core.Mat;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
@@ -26,6 +27,7 @@
 import org.photonvision.vision.pipe.impl.FocusPipe;
 import org.photonvision.vision.pipe.impl.ResizeImagePipe;
 import org.photonvision.vision.pipeline.result.FocusPipelineResult;
+import org.wpilib.math.geometry.Transform3d;
 
 public class FocusPipeline extends CVPipeline<FocusPipelineResult, FocusPipelineSettings> {
     private final FocusPipe focusPipe = new FocusPipe();
@@ -35,12 +37,13 @@ public class FocusPipeline extends CVPipeline<FocusPipelineResult, FocusPipeline
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
 
     public FocusPipeline() {
-        super(PROCESSING_TYPE);
+        super(PROCESSING_TYPE, (time) -> null);
         settings = new FocusPipelineSettings();
     }
 
-    public FocusPipeline(FocusPipelineSettings settings) {
-        super(PROCESSING_TYPE);
+    public FocusPipeline(
+            FocusPipelineSettings settings, Function<Long, Transform3d> robotToCameraSampler) {
+        super(PROCESSING_TYPE, robotToCameraSampler);
         this.settings = settings;
     }