Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-28 02:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix large calibration datasets crashes (#1453)

Browse Source 
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
This commit is contained in:
Cameron (3539)
2024-10-13 01:29:17 -04:00
committed by GitHub
parent d9b6199cf0
commit 0766d0e802
10 changed files with 141 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
View File

@@ -30,6 +30,7 @@ import java.time.format.DateTimeFormatter;
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 @@ public class ConfigManager {
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 class ConfigManager {
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);
}

42
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -21,6 +21,7 @@ import edu.wpi.first.math.geometry.Transform3d;
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.networktables.NTTopicSet;
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 class NTDataPublisher implements CVPipelineResultConsumer {
@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 class NTDataPublisher implements CVPipelineResultConsumer {
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 class NTDataPublisher implements CVPipelineResultConsumer {
}
// 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 class NTDataPublisher implements CVPipelineResultConsumer {
ts.cameraDistortionPublisher.accept(new double[] {});
}
ts.heartbeatPublisher.set(result.sequenceID);
ts.heartbeatPublisher.set(acceptedResult.sequenceID);
// TODO...nt4... is this needed?
rootTable.getInstance().flush();

16
photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java
View File

@@ -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 final class BoardObservation implements Cloneable {
@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 final class BoardObservation implements Cloneable {
+ includeObservationInCalibration
+ ", snapshotName="
+ snapshotName
+ ", snapshotData="
+ snapshotData
+ ", snapshotDataLocation="
+ snapshotDataLocation
+ "]";
}

58
photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
View File

@@ -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.mrcal.MrCalJNILoader;
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 @@ public class Calibrate3dPipe
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 @@ public class Calibrate3dPipe
}
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 @@ public class Calibrate3dPipe
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 @@ public class Calibrate3dPipe
}
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 @@ public class Calibrate3dPipe
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 @@ public class Calibrate3dPipe
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 @@ public class Calibrate3dPipe
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();

12
photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
View File

@@ -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 class Calibrate3dPipeline
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 class Calibrate3dPipeline
* 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;

9
photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
View File

@@ -60,7 +60,6 @@ public class PipelineManager {
PipelineManager(
DriverModePipelineSettings driverSettings,
List<CVPipelineSettings> 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);
}
/**

10
photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
View File

@@ -105,8 +105,7 @@ public class VisionModule {
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 class VisionModule {
}
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());

8
photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
View File

@@ -32,6 +32,7 @@ import org.opencv.calib3d.Calib3d;
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 class Calibrate3dPipeTest {
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 class Calibrate3dPipeTest {
.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 );

3
photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java
View File

@@ -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"]

23
photon-server/src/main/java/org/photonvision/server/RequestHandler.java
View File

@@ -31,6 +31,7 @@ import java.util.HashMap;
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 class RequestHandler {
// 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();