Casadi meme

.styleguide

@@ -21,6 +21,7 @@ modifiableFileExclude {
   \.rknn$
   gradlew
   photon-lib/py/photonlibpy/generated/
+  photon-targeting/src/main/native/cpp/photon/casadi_meme/generate/
   photon-targeting/src/generated/
 }
 

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -31,6 +31,7 @@
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -83,9 +84,32 @@ public enum PoseStrategy {
          * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can
          * take a lot of time.
          */
-        MULTI_TAG_PNP_ON_RIO
+        MULTI_TAG_PNP_ON_RIO,
+
+        /**
+         * Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase
+         * flat on the floor. This computation takes place on the RoboRIO, and typically takes not more
+         * than 2ms. See {@link PhotonPoseEstimator.ConstrainedSolvepnpParams} and {@link
+         * org.photonvision.jni.ConstrainedSolvepnpJni} for details and tuning handles this strategy
+         * exposes.
+         */
+        CONSTRAINED_SOLVEPNP
     }
 
+    /**
+     * Tuning handles we have over the CONSTRAINED_SOLVEPNP {@link PhotonPoseEstimator.PoseStrategy}.
+     * Internally, the cost function is a sum-squared of pixel reprojection error + (optionally)
+     * heading error * heading scale factor.
+     *
+     * @param headingFree If true, heading is completely free to vary. If false, heading excursions
+     *     from the provided heading measurement will be penalized
+     * @param headingMeasurement The current heading estimate
+     * @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
+     *     heading estimate against the tag corner reprojection error const.
+     */
+    public static final record ConstrainedSolvepnpParams(
+            boolean headingFree, Rotation2d headingMeasurement, double headingScaleFactor) {}
+
     private AprilTagFieldLayout fieldTags;
     private TargetModel tagModel = TargetModel.kAprilTag36h11;
     private PoseStrategy primaryStrategy;
@@ -304,6 +328,8 @@ public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
      *   <li>The timestamp of the provided pipeline result is the same as in the previous call to
      *       {@code update()}.
      *   <li>No targets were found in the pipeline results.
+     *   <li>The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the
+     *       other function overload).
      * </ul>
      *
      * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
@@ -317,6 +343,33 @@ public Optional<EstimatedRobotPose> update(
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
             Optional<Matrix<N8, N1>> distCoeffs) {
+        return update(cameraResult, cameraMatrix, distCoeffs, null);
+    }
+
+    /**
+     * Updates the estimated position of the robot. Returns empty if:
+     *
+     * <ul>
+     *   <li>The timestamp of the provided pipeline result is the same as in the previous call to
+     *       {@code update()}.
+     *   <li>No targets were found in the pipeline results.
+     *   <li>The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are null.
+     * </ul>
+     *
+     * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
+     *     otherwise
+     * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
+     *     otherwise
+     * @param constrainedPnpParams Constrained SolvePNP params, if needed. May be null if the strategy
+     *     is not CONSTRAINED_SOLVEPNP
+     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    public Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult,
+            Optional<Matrix<N3, N3>> cameraMatrix,
+            Optional<Matrix<N8, N1>> distCoeffs,
+            ConstrainedSolvepnpParams constrainedPnpParams) {
         // Time in the past -- give up, since the following if expects times > 0
         if (cameraResult.getTimestampSeconds() < 0) {
             return Optional.empty();
@@ -338,13 +391,15 @@ public Optional<EstimatedRobotPose> update(
             return Optional.empty();
         }
 
-        return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy);
+        return update(
+                cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, this.primaryStrategy);
     }
 
     private Optional<EstimatedRobotPose> update(
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
             Optional<Matrix<N8, N1>> distCoeffs,
+            ConstrainedSolvepnpParams meme,
             PoseStrategy strategy) {
         Optional<EstimatedRobotPose> estimatedPose =
                 switch (strategy) {
@@ -373,6 +428,26 @@ private Optional<EstimatedRobotPose> update(
                         }
                     }
                     case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
+                    case CONSTRAINED_SOLVEPNP -> {
+                        if (cameraMatrix.isEmpty()) {
+                            DriverStation.reportWarning(
+                                    "Camera matrix is empty for Constrained SolvePNP",
+                                    Thread.currentThread().getStackTrace());
+                            yield Optional.empty();
+                        } else if (distCoeffs.isEmpty()) {
+                            DriverStation.reportWarning(
+                                    "Camera matrix is empty for Constrained SolvePNP",
+                                    Thread.currentThread().getStackTrace());
+                            yield Optional.empty();
+                        } else if (meme == null) {
+                            DriverStation.reportWarning(
+                                    "Metadata is empty for Constrained SolvePNP",
+                                    Thread.currentThread().getStackTrace());
+                            yield Optional.empty();
+                        } else {
+                            yield constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, meme);
+                        }
+                    }
                 };
 
         if (estimatedPose.isPresent()) {
@@ -382,6 +457,78 @@ private Optional<EstimatedRobotPose> update(
         return estimatedPose;
     }
 
+    private Optional<EstimatedRobotPose> constrainedPnpStrategy(
+            PhotonPipelineResult result,
+            Optional<Matrix<N3, N3>> cameraMatrixOpt,
+            Optional<Matrix<N8, N1>> distCoeffsOpt,
+            ConstrainedSolvepnpParams meme) {
+        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
+        // cannot run multitagPNP, use fallback strategy
+        if (!hasCalibData) {
+            return update(result, cameraMatrixOpt, distCoeffsOpt, null, this.multiTagFallbackStrategy);
+        }
+
+        // Need heading if heading fixed
+        if (!meme.headingFree && meme.headingMeasurement == null) {
+            return update(result, cameraMatrixOpt, distCoeffsOpt, null, this.multiTagFallbackStrategy);
+        }
+
+        Pose3d fieldToRobotSeed;
+
+        // Attempt to use multi-tag to get a pose estimate seed
+        if (result.getMultiTagResult().isPresent()) {
+            fieldToRobotSeed =
+                    new Pose3d()
+                            .plus(
+                                    result
+                                            .getMultiTagResult()
+                                            .get()
+                                            .estimatedPose
+                                            .best
+                                            .plus(robotToCamera.inverse()));
+        } else {
+            // HACK - use fallback strategy to gimme a seed pose
+            // TODO - make sure nested update doesn't break state
+            var nestedUpdate =
+                    update(result, cameraMatrixOpt, distCoeffsOpt, null, this.multiTagFallbackStrategy);
+            if (nestedUpdate.isEmpty()) {
+                // best i can do is bail
+                return Optional.empty();
+            }
+            fieldToRobotSeed = nestedUpdate.get().estimatedPose;
+        }
+
+        if (!meme.headingFree) {
+            // If heading fixed, force rotation component
+            fieldToRobotSeed =
+                    new Pose3d(fieldToRobotSeed.getTranslation(), new Rotation3d(meme.headingMeasurement));
+        }
+
+        var pnpResult =
+                VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
+                        cameraMatrixOpt.get(),
+                        distCoeffsOpt.get(),
+                        result.getTargets(),
+                        robotToCamera,
+                        fieldToRobotSeed,
+                        fieldTags,
+                        tagModel,
+                        meme.headingFree,
+                        meme.headingMeasurement,
+                        meme.headingScaleFactor);
+        // try fallback strategy if solvePNP fails for some reason
+        if (!pnpResult.isPresent())
+            return update(result, cameraMatrixOpt, distCoeffsOpt, null, this.multiTagFallbackStrategy);
+        var best = new Pose3d().plus(pnpResult.get().best); // field-to-robot
+
+        return Optional.of(
+                new EstimatedRobotPose(
+                        best,
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP));
+    }
+
     private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
         if (result.getMultiTagResult().isPresent()) {
             var best_tf = result.getMultiTagResult().get().estimatedPose.best;
@@ -398,7 +545,8 @@ private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResu
                             PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
         } else {
             // We can never fall back on another multitag strategy
-            return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
+            return update(
+                    result, Optional.empty(), Optional.empty(), null, this.multiTagFallbackStrategy);
         }
     }
 
@@ -409,15 +557,15 @@ private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
         boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
         // cannot run multitagPNP, use fallback strategy
         if (!hasCalibData || result.getTargets().size() < 2) {
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+            return update(result, cameraMatrixOpt, distCoeffsOpt, null, this.multiTagFallbackStrategy);
         }
 
         var pnpResult =
                 VisionEstimation.estimateCamPosePNP(
                         cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
         // try fallback strategy if solvePNP fails for some reason
         if (!pnpResult.isPresent())
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+            return update(result, cameraMatrixOpt, distCoeffsOpt, null, this.multiTagFallbackStrategy);
         var best =
                 new Pose3d()
                         .plus(pnpResult.get().best) // field-to-camera