Fix large calibration datasets crashes (PhotonVision#1453)

The target list in networktables is limited to 127 items. When you capture more than 127 calibration images it breaks this limit and errors out and dies. Do not publish calibration targets to nt. And also move cal images into their own folder
Juniormunk · Oct 13, 2024 · 0766d0e · 0766d0e
1 parent d9b6199
commit 0766d0e
Show file tree

Hide file tree

Showing 10 changed files with 141 additions and 58 deletions.
diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
@@ -30,6 +30,7 @@
 import java.time.temporal.TemporalAccessor;
 import java.util.Date;
 import java.util.List;
+import org.opencv.core.Size;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.file.FileUtils;
@@ -60,8 +61,8 @@ enum ConfigSaveStrategy {
         ATOMIC_ZIP
     }
 
-    // This logic decides which kind of ConfigManager we load as the default. If we want
-    // to switch back to the legacy config manager, change this constant
+    // This logic decides which kind of ConfigManager we load as the default. If we want to switch
+    // back to the legacy config manager, change this constant
     private static final ConfigSaveStrategy m_saveStrat = ConfigSaveStrategy.SQL;
 
     public static ConfigManager getInstance() {
@@ -249,6 +250,19 @@ public Path getImageSavePath() {
         return imgFilePath.toPath();
     }
 
+    public Path getCalibrationImageSavePath(Size frameSize, String uniqueCameraName) {
+        var imgFilePath =
+                Path.of(
+                                configDirectoryFile.toString(),
+                                "calibration",
+                                uniqueCameraName,
+                                "imgs",
+                                frameSize.toString())
+                        .toFile();
+        if (!imgFilePath.exists()) imgFilePath.mkdirs();
+        return imgFilePath.toPath();
+    }
+
     public boolean saveUploadedHardwareConfig(Path uploadPath) {
         return m_provider.saveUploadedHardwareConfig(uploadPath);
     }

diff --git a/...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -21,6 +21,7 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
 import edu.wpi.first.util.WPIUtilJNI;
+import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
@@ -32,6 +33,7 @@
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 
 public class NTDataPublisher implements CVPipelineResultConsumer {
@@ -130,15 +132,29 @@ public void updateCameraNickname(String newCameraNickname) {
 
     @Override
     public void accept(CVPipelineResult result) {
+        CVPipelineResult acceptedResult;
+        if (result
+                instanceof
+                CalibrationPipelineResult) // If the data is from a calibration pipeline, override the list
+            // of targets to be null to prevent the data from being sent and
+            // continue to post blank/zero data to the network tables
+            acceptedResult =
+                    new CVPipelineResult(
+                            result.sequenceID,
+                            result.processingNanos,
+                            result.fps,
+                            List.of(),
+                            result.inputAndOutputFrame);
+        else acceptedResult = result;
         var now = WPIUtilJNI.now();
-        var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
+        var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos());
         var simplified =
                 new PhotonPipelineResult(
-                        result.sequenceID,
+                        acceptedResult.sequenceID,
                         captureMicros,
                         now,
-                        TrackedTarget.simpleFromTrackedTargets(result.targets),
-                        result.multiTagResult);
+                        TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
+                        acceptedResult.multiTagResult);
 
         // random guess at size of the array
         ts.resultPublisher.set(simplified, 1024);
@@ -148,11 +164,11 @@ public void accept(CVPipelineResult result) {
 
         ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
         ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
-        ts.latencyMillisEntry.set(result.getLatencyMillis());
-        ts.hasTargetEntry.set(result.hasTargets());
+        ts.latencyMillisEntry.set(acceptedResult.getLatencyMillis());
+        ts.hasTargetEntry.set(acceptedResult.hasTargets());
 
-        if (result.hasTargets()) {
-            var bestTarget = result.targets.get(0);
+        if (acceptedResult.hasTargets()) {
+            var bestTarget = acceptedResult.targets.get(0);
 
             ts.targetPitchEntry.set(bestTarget.getPitch());
             ts.targetYawEntry.set(bestTarget.getYaw());
@@ -176,18 +192,18 @@ public void accept(CVPipelineResult result) {
         }
 
         // Something in the result can sometimes be null -- so check probably too many things
-        if (result.inputAndOutputFrame != null
-                && result.inputAndOutputFrame.frameStaticProperties != null
-                && result.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
-            var fsp = result.inputAndOutputFrame.frameStaticProperties;
+        if (acceptedResult.inputAndOutputFrame != null
+                && acceptedResult.inputAndOutputFrame.frameStaticProperties != null
+                && acceptedResult.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
+            var fsp = acceptedResult.inputAndOutputFrame.frameStaticProperties;
             ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
             ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr());
         } else {
             ts.cameraIntrinsicsPublisher.accept(new double[] {});
             ts.cameraDistortionPublisher.accept(new double[] {});
         }
 
-        ts.heartbeatPublisher.set(result.sequenceID);
+        ts.heartbeatPublisher.set(acceptedResult.sequenceID);
 
         // TODO...nt4... is this needed?
         rootTable.getInstance().flush();

diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java b/photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java
@@ -18,12 +18,16 @@
 package org.photonvision.vision.calibration;
 
 import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.geometry.Pose3d;
+import java.nio.file.Path;
 import java.util.List;
 import org.opencv.core.Point;
 import org.opencv.core.Point3;
 
+// Ignore the previous calibration data that was stored in the json file.
+@JsonIgnoreProperties(ignoreUnknown = true)
 public final class BoardObservation implements Cloneable {
     // Expected feature 3d location in the camera frame
     @JsonProperty("locationInObjectSpace")
@@ -48,8 +52,8 @@ public final class BoardObservation implements Cloneable {
     @JsonProperty("snapshotName")
     public String snapshotName;
 
-    @JsonProperty("snapshotData")
-    public JsonImageMat snapshotData;
+    @JsonProperty("snapshotDataLocation")
+    public Path snapshotDataLocation;
 
     @JsonCreator
     public BoardObservation(
@@ -59,14 +63,14 @@ public BoardObservation(
             @JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
             @JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
             @JsonProperty("snapshotName") String snapshotName,
-            @JsonProperty("snapshotData") JsonImageMat snapshotData) {
+            @JsonProperty("snapshotDataLocation") Path snapshotDataLocation) {
         this.locationInObjectSpace = locationInObjectSpace;
         this.locationInImageSpace = locationInImageSpace;
         this.reprojectionErrors = reprojectionErrors;
         this.optimisedCameraToObject = optimisedCameraToObject;
         this.includeObservationInCalibration = includeObservationInCalibration;
         this.snapshotName = snapshotName;
-        this.snapshotData = snapshotData;
+        this.snapshotDataLocation = snapshotDataLocation;
     }
 
     @Override
@@ -83,8 +87,8 @@ public String toString() {
                 + includeObservationInCalibration
                 + ", snapshotName="
                 + snapshotName
-                + ", snapshotData="
-                + snapshotData
+                + ", snapshotDataLocation="
+                + snapshotDataLocation
                 + "]";
     }
 

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
@@ -17,12 +17,16 @@
 
 package org.photonvision.vision.pipe.impl;
 
+import java.nio.file.Path;
+import java.nio.file.Paths;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
 import java.util.stream.Collectors;
+import org.apache.commons.io.FileUtils;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.*;
+import org.opencv.imgcodecs.Imgcodecs;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.math.MathUtils;
@@ -32,7 +36,6 @@
 import org.photonvision.vision.calibration.BoardObservation;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.calibration.CameraLensModel;
-import org.photonvision.vision.calibration.JsonImageMat;
 import org.photonvision.vision.calibration.JsonMatOfDouble;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.pipe.CVPipe;
@@ -46,11 +49,15 @@ public class Calibrate3dPipe
     public static class CalibrationInput {
         final List<FindBoardCornersPipe.FindBoardCornersPipeResult> observations;
         final FrameStaticProperties imageProps;
+        final Path imageSavePath;
 
         public CalibrationInput(
-                List<FindBoardCornersPipeResult> observations, FrameStaticProperties imageProps) {
+                List<FindBoardCornersPipeResult> observations,
+                FrameStaticProperties imageProps,
+                Path imageSavePath) {
             this.observations = observations;
             this.imageProps = imageProps;
+            this.imageSavePath = imageSavePath;
         }
     }
 
@@ -86,16 +93,23 @@ protected CameraCalibrationCoefficients process(CalibrationInput in) {
 
         CameraCalibrationCoefficients ret;
         var start = System.nanoTime();
+
         if (MrCalJNILoader.getInstance().isLoaded() && params.useMrCal) {
             logger.debug("Calibrating with mrcal!");
             ret =
                     calibrateMrcal(
-                            filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
+                            filteredIn,
+                            in.imageProps.horizontalFocalLength,
+                            in.imageProps.verticalFocalLength,
+                            in.imageSavePath);
         } else {
             logger.debug("Calibrating with opencv!");
             ret =
                     calibrateOpenCV(
-                            filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
+                            filteredIn,
+                            in.imageProps.horizontalFocalLength,
+                            in.imageProps.verticalFocalLength,
+                            in.imageSavePath);
         }
         var dt = System.nanoTime() - start;
 
@@ -116,7 +130,10 @@ protected CameraCalibrationCoefficients process(CalibrationInput in) {
     }
 
     protected CameraCalibrationCoefficients calibrateOpenCV(
-            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
+            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in,
+            double fxGuess,
+            double fyGuess,
+            Path imageSavePath) {
         List<MatOfPoint3f> objPointsIn =
                 in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
         List<MatOfPoint2f> imgPointsIn =
@@ -179,7 +196,8 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
         JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
 
         var observations =
-                createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null);
+                createObservations(
+                        in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null, imageSavePath);
 
         cameraMatrix.release();
         distortionCoefficients.release();
@@ -200,7 +218,10 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
     }
 
     protected CameraCalibrationCoefficients calibrateMrcal(
-            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
+            List<FindBoardCornersPipe.FindBoardCornersPipeResult> in,
+            double fxGuess,
+            double fyGuess,
+            Path imageSavePath) {
         List<MatOfPoint2f> corner_locations =
                 in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
 
@@ -297,7 +318,8 @@ protected CameraCalibrationCoefficients calibrateMrcal(
                         distortionCoefficientsMat.getAsMatOfDouble(),
                         rvecs,
                         tvecs,
-                        new double[] {result.warp_x, result.warp_y});
+                        new double[] {result.warp_x, result.warp_y},
+                        imageSavePath);
 
         rvecs.forEach(Mat::release);
         tvecs.forEach(Mat::release);
@@ -319,10 +341,19 @@ private List<BoardObservation> createObservations(
             MatOfDouble distortionCoefficients_,
             List<Mat> rvecs,
             List<Mat> tvecs,
-            double[] calobject_warp) {
+            double[] calobject_warp,
+            Path imageSavePath) {
         List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
         List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
 
+        // Clear the calibration image folder of any old images before we save the new ones.
+
+        try {
+            FileUtils.cleanDirectory(imageSavePath.toFile());
+        } catch (Exception e) {
+            logger.error("Failed to clean calibration image directory", e);
+        }
+
         // For each observation, calc reprojection error
         Mat jac_temp = new Mat();
         List<BoardObservation> observations = new ArrayList<>();
@@ -383,14 +414,17 @@ private List<BoardObservation> createObservations(
 
             var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i));
 
-            JsonImageMat image = null;
             var inputImage = in.get(i).inputImage;
+            Path image_path = null;
+            String snapshotName = "img" + i + ".png";
             if (inputImage != null) {
-                image = new JsonImageMat(inputImage);
+                image_path = Paths.get(imageSavePath.toString(), snapshotName);
+                Imgcodecs.imwrite(image_path.toString(), inputImage);
             }
+
             observations.add(
                     new BoardObservation(
-                            i_objPts, i_imgPts, reprojectionError, camToBoard, true, "img" + i + ".png", image));
+                            i_objPts, i_imgPts, reprojectionError, camToBoard, true, snapshotName, image_path));
         }
         jac_temp.release();
 

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
@@ -18,6 +18,7 @@
 package org.photonvision.vision.pipeline;
 
 import edu.wpi.first.math.util.Units;
+import java.nio.file.Path;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.stream.Collectors;
@@ -69,11 +70,11 @@ public class Calibrate3dPipeline
 
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
 
-    public Calibrate3dPipeline(String uniqueName) {
-        this(12, uniqueName);
+    public Calibrate3dPipeline() {
+        this(12);
     }
 
-    public Calibrate3dPipeline(int minSnapshots, String uniqueName) {
+    public Calibrate3dPipeline(int minSnapshots) {
         super(PROCESSING_TYPE);
         this.settings = new Calibration3dPipelineSettings();
         this.foundCornersList = new ArrayList<>();
@@ -174,7 +175,7 @@ public boolean hasEnough() {
         return foundCornersList.size() >= minSnapshots;
     }
 
-    public CameraCalibrationCoefficients tryCalibration() {
+    public CameraCalibrationCoefficients tryCalibration(Path imageSavePath) {
         if (!hasEnough()) {
             logger.info(
                     "Not enough snapshots! Only got "
@@ -193,7 +194,8 @@ public CameraCalibrationCoefficients tryCalibration() {
          * and returns the corresponding image and object points
          */
         calibrationOutput =
-                calibrate3dPipe.run(new CalibrationInput(foundCornersList, frameStaticProperties));
+                calibrate3dPipe.run(
+                        new CalibrationInput(foundCornersList, frameStaticProperties, imageSavePath));
 
         this.calibrating = false;