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 index 8a9ef7492f..bc15d5b922 100644 --- 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/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index be51316126..719b0f2969 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-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,10 +192,10 @@ 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 { @@ -187,7 +203,7 @@ public void accept(CVPipelineResult result) { 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 index 8bf41c112e..412e03a7c6 100644 --- 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 index 3ff1ccb461..a7a8eeec91 100644 --- 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 observations; final FrameStaticProperties imageProps; + final Path imageSavePath; public CalibrationInput( - List observations, FrameStaticProperties imageProps) { + List 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 in, double fxGuess, double fyGuess) { + List in, + double fxGuess, + double fyGuess, + Path imageSavePath) { List objPointsIn = in.stream().map(it -> it.objectPoints).collect(Collectors.toList()); List 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 in, double fxGuess, double fyGuess) { + List in, + double fxGuess, + double fyGuess, + Path imageSavePath) { List 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 createObservations( MatOfDouble distortionCoefficients_, List rvecs, List tvecs, - double[] calobject_warp) { + double[] calobject_warp, + Path imageSavePath) { List objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList()); List 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 observations = new ArrayList<>(); @@ -383,14 +414,17 @@ private List 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 index 951b09d50a..b009a97c4a 100644 --- 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; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index 2b7ba70bf5..be397b255c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -60,7 +60,6 @@ public class PipelineManager { PipelineManager( DriverModePipelineSettings driverSettings, List userPipelines, - String uniqueName, int defaultIndex) { this.userPipelineSettings = new ArrayList<>(userPipelines); // This is to respect the default res idx for vendor cameras @@ -69,7 +68,7 @@ public class PipelineManager { if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective); - calibration3dPipeline = new Calibrate3dPipeline(uniqueName); + calibration3dPipeline = new Calibrate3dPipeline(); // We know that at this stage, VisionRunner hasn't yet started so we're good to // do this from @@ -79,11 +78,7 @@ public class PipelineManager { } public PipelineManager(CameraConfiguration config) { - this( - config.driveModeSettings, - config.pipelineSettings, - config.uniqueName, - config.currentPipelineIndex); + this(config.driveModeSettings, config.pipelineSettings, config.currentPipelineIndex); } /** diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 6fd00c363b..f378f511c5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -105,8 +105,7 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, visionSource.getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera; // We don't show gain if the config says it's -1. So check here to make sure - // it's non-negative - // if it _is_ supported + // it's non-negative if it _is_ supported if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) { pipelineManager.userPipelineSettings.forEach( it -> { @@ -388,7 +387,12 @@ public void takeCalibrationSnapshot() { } public CameraCalibrationCoefficients endCalibration() { - var ret = pipelineManager.calibration3dPipeline.tryCalibration(); + var ret = + pipelineManager.calibration3dPipeline.tryCalibration( + ConfigManager.getInstance() + .getCalibrationImageSavePath( + pipelineManager.calibration3dPipeline.getSettings().resolution, + visionSource.getCameraConfiguration().uniqueName)); pipelineManager.setCalibrationMode(false); setPipeline(pipelineManager.getRequestedIndex()); diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index a7b7b00c87..bb8b5cb79a 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -32,6 +32,7 @@ import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgcodecs.Imgcodecs; +import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.LogLevel; import org.photonvision.common.logging.Logger; @@ -215,8 +216,7 @@ public static void calibrateCommon( assertTrue(directoryListing.length >= 12); - Calibrate3dPipeline calibration3dPipeline = - new Calibrate3dPipeline(10, "test_calibration_common"); + Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(10); calibration3dPipeline.getSettings().boardType = boardType; calibration3dPipeline.getSettings().markerSize = markerSize; calibration3dPipeline.getSettings().tagFamily = tagFamily; @@ -253,7 +253,9 @@ public static void calibrateCommon( .map(it -> it.imagePoints) .allMatch(it -> it.width() > 0 && it.height() > 0)); - var cal = calibration3dPipeline.tryCalibration(); + var cal = + calibration3dPipeline.tryCalibration( + ConfigManager.getInstance().getCalibrationImageSavePath(imgRes, "Calibration_Test")); calibration3dPipeline.finishCalibration(); // visuallyDebugDistortion(directoryListing, imgRes, cal ); diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java index 9ae44c2057..1219515cde 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java @@ -29,8 +29,7 @@ public class PipelineManagerTest { @Test public void testUniqueName() { TestUtils.loadLibraries(); - PipelineManager manager = - new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name", -1); + PipelineManager manager = new PipelineManager(new DriverModePipelineSettings(), List.of(), -1); manager.addPipeline(PipelineType.Reflective, "Another"); // We now have ["New Pipeline", "Another"] diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index 998b852642..50607cdf27 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -31,6 +31,7 @@ import java.util.Optional; import javax.imageio.ImageIO; import org.apache.commons.io.FileUtils; +import org.opencv.core.Mat; import org.opencv.core.MatOfByte; import org.opencv.core.MatOfInt; import org.opencv.imgcodecs.Imgcodecs; @@ -579,11 +580,23 @@ public static void onCalibrationSnapshotRequest(Context ctx) { // encode as jpeg to save even more space. reduces size of a 1280p image from 300k to 25k var jpegBytes = new MatOfByte(); - Imgcodecs.imencode( - ".jpg", - calList.observations.get(observationIdx).snapshotData.getAsMat(), - jpegBytes, - new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60)); + Mat img = null; + try { + img = + Imgcodecs.imread( + calList.observations.get(observationIdx).snapshotDataLocation.toString()); + } catch (Exception e) { + ctx.status(500); + ctx.result("Unable to read calibration image"); + return; + } + if (img == null || img.empty()) { + ctx.status(500); + ctx.result("Unable to read calibration image"); + return; + } + + Imgcodecs.imencode(".jpg", img, jpegBytes, new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 60)); ctx.result(jpegBytes.toArray()); jpegBytes.release();