Switched to multi-tag pnp vision strategy

mpulte · mpulte · commit 77a335b7f92e · 2024-01-01T13:59:38.000-05:00
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -55,6 +55,7 @@
     "dtheta",
     "Holonomic",
     "Odometry",
+    "Reprojection",
     "Setpoint",
     "Setpoints",
     "teleop",
diff --git a/src/main/java/com/team1701/lib/drivers/cameras/AprilTagCamera.java b/src/main/java/com/team1701/lib/drivers/cameras/AprilTagCamera.java
@@ -1,12 +1,12 @@
-package com.team1701.lib.cameras;
+package com.team1701.lib.drivers.cameras;
 
 import java.util.ArrayList;
 import java.util.function.Consumer;
 import java.util.function.Predicate;
 import java.util.function.Supplier;
 
 import com.team1701.lib.alerts.Alert;
-import com.team1701.lib.cameras.AprilTagCameraIO.PhotonCameraInputs;
+import com.team1701.lib.drivers.cameras.AprilTagCameraIO.PhotonCameraInputs;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -38,13 +38,14 @@ public AprilTagCamera(
             AprilTagCameraIO cameraIO,
             Transform3d robotToCamPose,
             PoseStrategy poseStrategy,
+            PoseStrategy fallbackPoseStrategy,
             Supplier<AprilTagFieldLayout> fieldLayoutSupplier,
             Supplier<Pose3d> robotPoseSupplier) {
         mCameraIO = cameraIO;
         mCameraInputs = new PhotonCameraInputs();
         mLoggingPrefix = "Camera/" + cameraName + "/";
         mPoseEstimator = new PhotonPoseEstimator(fieldLayoutSupplier.get(), poseStrategy, null, robotToCamPose);
-        mPoseEstimator.setMultiTagFallbackStrategy(poseStrategy);
+        mPoseEstimator.setMultiTagFallbackStrategy(fallbackPoseStrategy);
         mRobotToCamPose = robotToCamPose;
         mFieldLayoutSupplier = fieldLayoutSupplier;
         mRobotPoseSupplier = robotPoseSupplier;
diff --git a/src/main/java/com/team1701/lib/drivers/cameras/AprilTagCameraIO.java b/src/main/java/com/team1701/lib/drivers/cameras/AprilTagCameraIO.java
@@ -1,13 +1,16 @@
-package com.team1701.lib.cameras;
+package com.team1701.lib.drivers.cameras;
 
 import java.util.ArrayList;
 import java.util.stream.DoubleStream;
+import java.util.stream.IntStream;
 
 import edu.wpi.first.math.geometry.Transform3d;
 import org.littletonrobotics.junction.LogTable;
 import org.littletonrobotics.junction.inputs.LoggableInputs;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.targeting.PNPResults;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
@@ -53,6 +56,19 @@ public void toLog(LogTable table) {
 
                 table.put(targetNamespace + "DetectedCorners", detectedCorners);
             }
+
+            var multiTagResult = pipelineResult.getMultiTagResult();
+            table.put("MultiTag/PnpResult/IsPresent", multiTagResult.estimatedPose.isPresent);
+            table.put("MultiTag/PnpResult/BestPose", multiTagResult.estimatedPose.best);
+            table.put("MultiTag/PnpResult/BestPoseReprojectionError", multiTagResult.estimatedPose.bestReprojErr);
+            table.put("MultiTag/PnpResult/AltPose", multiTagResult.estimatedPose.alt);
+            table.put("MultiTag/PnpResult/AltPoseReprojectionError", multiTagResult.estimatedPose.altReprojErr);
+            table.put("MultiTag/PnpResult/Ambiguity", multiTagResult.estimatedPose.ambiguity);
+            table.put(
+                    "MultiTag/TargetIdsUsed",
+                    multiTagResult.fiducialIDsUsed.stream()
+                            .mapToInt(Integer::intValue)
+                            .toArray());
         }
 
         @Override
@@ -95,7 +111,21 @@ public void fromLog(LogTable table) {
                 targets.add(trackedTarget);
             }
 
-            pipelineResult = new PhotonPipelineResult(latency, targets);
+            var multiTagPnpResultPresent = table.get("MultiTag/PnpResult/IsPresent", false);
+            var multiTagPnpResult = multiTagPnpResultPresent
+                    ? new PNPResults(
+                            table.get("MultiTag/PnpResult/BestPose", new Transform3d()),
+                            table.get("MultiTag/PnpResult/AltPose", new Transform3d()),
+                            table.get("MultiTag/PnpResult/Ambiguity", 0.0),
+                            table.get("MultiTag/PnpResult/BestPoseReprojectionError", 0.0),
+                            table.get("MultiTag/PnpResult/AltPoseReprojectionError", 0.0))
+                    : new PNPResults();
+            var multiTagTargetIdsUsed = IntStream.of(table.get("MultiTag/TargetIdsUsed", new int[] {}))
+                    .boxed()
+                    .toList();
+            var multiTargetResult = new MultiTargetPNPResults(multiTagPnpResult, multiTagTargetIdsUsed);
+
+            pipelineResult = new PhotonPipelineResult(latency, targets, multiTargetResult);
             pipelineResult.setTimestampSeconds(timestamp);
 
             isConnected = table.get("IsConnected", false);
diff --git a/src/main/java/com/team1701/lib/drivers/cameras/AprilTagCameraIOPhotonCamera.java b/src/main/java/com/team1701/lib/drivers/cameras/AprilTagCameraIOPhotonCamera.java
@@ -1,4 +1,4 @@
-package com.team1701.lib.cameras;
+package com.team1701.lib.drivers.cameras;
 
 import edu.wpi.first.math.geometry.Transform3d;
 import org.photonvision.PhotonCamera;
diff --git a/src/main/java/com/team1701/robot/Constants.java b/src/main/java/com/team1701/robot/Constants.java
@@ -166,6 +166,7 @@ public static final class Vision {
                 new Transform3d(new Translation3d(-0.3, -0.3, 0.2), new Rotation3d(0, 0, Units.degreesToRadians(-135)));
 
         public static final double kMaxPoseAmbiguity = 0.03;
-        public static final PoseStrategy kPoseStrategy = PoseStrategy.AVERAGE_BEST_TARGETS;
+        public static final PoseStrategy kPoseStrategy = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+        public static final PoseStrategy kFallbackPoseStrategy = PoseStrategy.LOWEST_AMBIGUITY;
     }
 }
diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java
@@ -6,8 +6,8 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.team1701.lib.alerts.TriggeredAlert;
-import com.team1701.lib.cameras.AprilTagCameraIO;
-import com.team1701.lib.cameras.AprilTagCameraIOPhotonCamera;
+import com.team1701.lib.drivers.cameras.AprilTagCameraIO;
+import com.team1701.lib.drivers.cameras.AprilTagCameraIOPhotonCamera;
 import com.team1701.lib.drivers.encoders.EncoderIO;
 import com.team1701.lib.drivers.encoders.EncoderIOAnalog;
 import com.team1701.lib.drivers.gyros.GyroIO;
diff --git a/src/main/java/com/team1701/robot/subsystems/vision/Vision.java b/src/main/java/com/team1701/robot/subsystems/vision/Vision.java
@@ -4,8 +4,8 @@
 import java.util.Optional;
 import java.util.function.Supplier;
 
-import com.team1701.lib.cameras.AprilTagCamera;
-import com.team1701.lib.cameras.AprilTagCameraIO;
+import com.team1701.lib.drivers.cameras.AprilTagCamera;
+import com.team1701.lib.drivers.cameras.AprilTagCameraIO;
 import com.team1701.robot.Constants;
 import com.team1701.robot.Robot;
 import com.team1701.robot.estimation.PoseEstimator;
@@ -35,27 +35,31 @@ public Vision(
                 cameraIOFrontLeft,
                 Constants.Vision.kRobotToFrontLeftCamPose,
                 Constants.Vision.kPoseStrategy,
+                Constants.Vision.kFallbackPoseStrategy,
                 fieldLayoutSupplier,
                 mPoseEstimator::getPose3d));
         mCameras.add(new AprilTagCamera(
                 Constants.Vision.kFrontRightCameraName,
                 cameraIOFrontRight,
                 Constants.Vision.kRobotToFrontRightCamPose,
                 Constants.Vision.kPoseStrategy,
+                Constants.Vision.kFallbackPoseStrategy,
                 fieldLayoutSupplier,
                 mPoseEstimator::getPose3d));
         mCameras.add(new AprilTagCamera(
                 Constants.Vision.kBackLeftCameraName,
                 cameraIOBackLeft,
                 Constants.Vision.kRobotToBackLeftCamPose,
                 Constants.Vision.kPoseStrategy,
+                Constants.Vision.kFallbackPoseStrategy,
                 fieldLayoutSupplier,
                 mPoseEstimator::getPose3d));
         mCameras.add(new AprilTagCamera(
                 Constants.Vision.kBackRightCameraName,
                 cameraIOBackRight,
                 Constants.Vision.kRobotToBackRightCamPose,
                 Constants.Vision.kPoseStrategy,
+                Constants.Vision.kFallbackPoseStrategy,
                 fieldLayoutSupplier,
                 mPoseEstimator::getPose3d));
 

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,4 @@`
`1`		`-package com.team1701.lib.cameras;`
	`1`	`+package com.team1701.lib.drivers.cameras;`
`2`	`2`
`3`	`3`	`import edu.wpi.first.math.geometry.Transform3d;`
`4`	`4`	`import org.photonvision.PhotonCamera;`
Original file line number	Diff line number	Diff line change
`@@ -166,6 +166,7 @@ public static final class Vision {`
`166`	`166`	`new Transform3d(new Translation3d(-0.3, -0.3, 0.2), new Rotation3d(0, 0, Units.degreesToRadians(-135)));`
`167`	`167`
`168`	`168`	`public static final double kMaxPoseAmbiguity = 0.03;`
`169`		`- public static final PoseStrategy kPoseStrategy = PoseStrategy.AVERAGE_BEST_TARGETS;`
	`169`	`+ public static final PoseStrategy kPoseStrategy = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;`
	`170`	`+ public static final PoseStrategy kFallbackPoseStrategy = PoseStrategy.LOWEST_AMBIGUITY;`
`170`	`171`	`}`
`171`	`172`	`}`