Add pose filtering to pose estimation examples

photonlib-cpp-examples/poseest/src/main/include/Vision.h

@@ -93,9 +93,20 @@ class Vision {
         }
       }
 
+      // Filter out poses that are likely to be incorrect (off the field, not flat on the ground, etc). 
+      // This is optional, but can help prevent bad vision estimates from hurting your pose estimator.
       if (visionEst) {
-        estConsumer(visionEst->estimatedPose.ToPose2d(), visionEst->timestamp,
-                    GetEstimationStdDevs(visionEst->estimatedPose.ToPose2d()));
+        auto estPose = visionEst->estimatedPose;
+        if (std::abs(estPose.Z().value()) < 0.2 && 
+            estPose.X().value() > -constants::Vision::kFieldLength / 2 && 
+            estPose.X().value() < constants::Vision::kFieldLength / 2 && 
+            estPose.Y().value() > -constants::Vision::kFieldWidth / 2 && 
+            estPose.Y().value() < constants::Vision::kFieldWidth / 2 && 
+            std::abs(estPose.Rotation().X().value()) < 0.2 && 
+            std::abs(estPose.Rotation().Y().value()) < 0.2) {
+          estConsumer(estPose.ToPose2d(), visionEst->timestamp,
+                      GetEstimationStdDevs(estPose.ToPose2d()));
+        }
       }
     }
   }

photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java

@@ -110,7 +110,17 @@ public void periodic() {
                         // Change our trust in the measurement based on the tags we can see
                         var estStdDevs = getEstimationStdDevs();
 
-                        estConsumer.accept(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
+                        // Filter out poses that are likely to be incorrect (off the field, not flat on the ground, etc). 
+                        // This is optional, but can help prevent bad vision estimates from hurting your pose estimator.
+                        if (Math.abs(estPose.getZ()) < 0.2
+                            && estPose.getX() > -kTagLayout.getFieldLength() / 2
+                            && estPose.getX() < kTagLayout.getFieldLength() / 2
+                            && estPose.getY() > -kTagLayout.getFieldWidth() / 2
+                            && estPose.getY() < kTagLayout.getFieldWidth() / 2
+                            && estPose.getRotation().getX() < 0.2
+                            && estPose.getRotation().getY() < 0.2) {
+                            estConsumer.accept(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
+                        }
                     });
         }
     }

photonlib-python-examples/poseest/robot.py

@@ -42,8 +42,9 @@ def robotInit(self) -> None:
         self.controller = wpilib.XboxController(0)
         self.swerve = drivetrain.Drivetrain()
         self.cam = PhotonCamera("YOUR CAMERA NAME")
+        self.aprilTagField = AprilTagFieldLayout.loadField(AprilTagField.kDefaultField)
         self.camPoseEst = PhotonPoseEstimator(
-            AprilTagFieldLayout.loadField(AprilTagField.kDefaultField),
+            self.aprilTagField,
             kRobotToCam,
         )
 
@@ -53,9 +54,19 @@ def robotPeriodic(self) -> None:
             if camEstPose is None:
                 camEstPose = self.camPoseEst.estimateLowestAmbiguityPose(result)
 
-            self.swerve.addVisionPoseEstimate(
-                camEstPose.estimatedPose, camEstPose.timestampSeconds
-            )
+            # Filter out poses that are likely to be incorrect (off the field, not flat on the ground, etc). 
+            # This is optional, but can help prevent bad vision estimates from hurting your pose estimator.
+            estPose = camEstPose.estimatedPose
+            if (
+                abs(estPose.Z()) < 0.2
+                and -self.aprilTagField.getFieldLength() / 2 < estPose.X() < self.aprilTagField.getFieldLength() / 2
+                and -self.aprilTagField.getFieldWidth() / 2 < estPose.Y() < self.aprilTagField.getFieldWidth() / 2
+                and abs(estPose.rotation().X()) < 0.2
+                and abs(estPose.rotation().Y()) < 0.2
+            ):
+                self.swerve.addVisionPoseEstimate(
+                    camEstPose.estimatedPose, camEstPose.timestampSeconds
+                )
 
         self.swerve.updateOdometry()
         self.swerve.log()