Skip to content

Commit 0766d0e

Browse files
JuniormunkJuniormunk
authored
Fix large calibration datasets crashes (#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
1 parent d9b6199 commit 0766d0e

File tree

10 files changed

+141
-58
lines changed

10 files changed

+141
-58
lines changed

‎photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java

Lines changed: 16 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
import java.time.temporal.TemporalAccessor;
3131
import java.util.Date;
3232
import java.util.List;
33+
import org.opencv.core.Size;
3334
import org.photonvision.common.logging.LogGroup;
3435
import org.photonvision.common.logging.Logger;
3536
import org.photonvision.common.util.file.FileUtils;
@@ -60,8 +61,8 @@ enum ConfigSaveStrategy {
6061
ATOMIC_ZIP
6162
}
6263

63-
// This logic decides which kind of ConfigManager we load as the default. If we want
64-
// to switch back to the legacy config manager, change this constant
64+
// This logic decides which kind of ConfigManager we load as the default. If we want to switch
65+
// back to the legacy config manager, change this constant
6566
private static final ConfigSaveStrategy m_saveStrat = ConfigSaveStrategy.SQL;
6667

6768
public static ConfigManager getInstance() {
@@ -249,6 +250,19 @@ public Path getImageSavePath() {
249250
return imgFilePath.toPath();
250251
}
251252

253+
public Path getCalibrationImageSavePath(Size frameSize, String uniqueCameraName) {
254+
var imgFilePath =
255+
Path.of(
256+
configDirectoryFile.toString(),
257+
"calibration",
258+
uniqueCameraName,
259+
"imgs",
260+
frameSize.toString())
261+
.toFile();
262+
if (!imgFilePath.exists()) imgFilePath.mkdirs();
263+
return imgFilePath.toPath();
264+
}
265+
252266
public boolean saveUploadedHardwareConfig(Path uploadPath) {
253267
return m_provider.saveUploadedHardwareConfig(uploadPath);
254268
}

‎photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

Lines changed: 29 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
import edu.wpi.first.networktables.NetworkTable;
2222
import edu.wpi.first.networktables.NetworkTableEvent;
2323
import edu.wpi.first.util.WPIUtilJNI;
24+
import java.util.List;
2425
import java.util.function.BooleanSupplier;
2526
import java.util.function.Consumer;
2627
import java.util.function.Supplier;
@@ -32,6 +33,7 @@
3233
import org.photonvision.common.util.math.MathUtils;
3334
import org.photonvision.targeting.PhotonPipelineResult;
3435
import org.photonvision.vision.pipeline.result.CVPipelineResult;
36+
import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
3537
import org.photonvision.vision.target.TrackedTarget;
3638

3739
public class NTDataPublisher implements CVPipelineResultConsumer {
@@ -130,15 +132,29 @@ public void updateCameraNickname(String newCameraNickname) {
130132

131133
@Override
132134
public void accept(CVPipelineResult result) {
135+
CVPipelineResult acceptedResult;
136+
if (result
137+
instanceof
138+
CalibrationPipelineResult) // If the data is from a calibration pipeline, override the list
139+
// of targets to be null to prevent the data from being sent and
140+
// continue to post blank/zero data to the network tables
141+
acceptedResult =
142+
new CVPipelineResult(
143+
result.sequenceID,
144+
result.processingNanos,
145+
result.fps,
146+
List.of(),
147+
result.inputAndOutputFrame);
148+
else acceptedResult = result;
133149
var now = WPIUtilJNI.now();
134-
var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
150+
var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos());
135151
var simplified =
136152
new PhotonPipelineResult(
137-
result.sequenceID,
153+
acceptedResult.sequenceID,
138154
captureMicros,
139155
now,
140-
TrackedTarget.simpleFromTrackedTargets(result.targets),
141-
result.multiTagResult);
156+
TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
157+
acceptedResult.multiTagResult);
142158

143159
// random guess at size of the array
144160
ts.resultPublisher.set(simplified, 1024);
@@ -148,11 +164,11 @@ public void accept(CVPipelineResult result) {
148164

149165
ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
150166
ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
151-
ts.latencyMillisEntry.set(result.getLatencyMillis());
152-
ts.hasTargetEntry.set(result.hasTargets());
167+
ts.latencyMillisEntry.set(acceptedResult.getLatencyMillis());
168+
ts.hasTargetEntry.set(acceptedResult.hasTargets());
153169

154-
if (result.hasTargets()) {
155-
var bestTarget = result.targets.get(0);
170+
if (acceptedResult.hasTargets()) {
171+
var bestTarget = acceptedResult.targets.get(0);
156172

157173
ts.targetPitchEntry.set(bestTarget.getPitch());
158174
ts.targetYawEntry.set(bestTarget.getYaw());
@@ -176,18 +192,18 @@ public void accept(CVPipelineResult result) {
176192
}
177193

178194
// Something in the result can sometimes be null -- so check probably too many things
179-
if (result.inputAndOutputFrame != null
180-
&& result.inputAndOutputFrame.frameStaticProperties != null
181-
&& result.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
182-
var fsp = result.inputAndOutputFrame.frameStaticProperties;
195+
if (acceptedResult.inputAndOutputFrame != null
196+
&& acceptedResult.inputAndOutputFrame.frameStaticProperties != null
197+
&& acceptedResult.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
198+
var fsp = acceptedResult.inputAndOutputFrame.frameStaticProperties;
183199
ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
184200
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr());
185201
} else {
186202
ts.cameraIntrinsicsPublisher.accept(new double[] {});
187203
ts.cameraDistortionPublisher.accept(new double[] {});
188204
}
189205

190-
ts.heartbeatPublisher.set(result.sequenceID);
206+
ts.heartbeatPublisher.set(acceptedResult.sequenceID);
191207

192208
// TODO...nt4... is this needed?
193209
rootTable.getInstance().flush();

‎photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -18,12 +18,16 @@
1818
package org.photonvision.vision.calibration;
1919

2020
import com.fasterxml.jackson.annotation.JsonCreator;
21+
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
2122
import com.fasterxml.jackson.annotation.JsonProperty;
2223
import edu.wpi.first.math.geometry.Pose3d;
24+
import java.nio.file.Path;
2325
import java.util.List;
2426
import org.opencv.core.Point;
2527
import org.opencv.core.Point3;
2628

29+
// Ignore the previous calibration data that was stored in the json file.
30+
@JsonIgnoreProperties(ignoreUnknown = true)
2731
public final class BoardObservation implements Cloneable {
2832
// Expected feature 3d location in the camera frame
2933
@JsonProperty("locationInObjectSpace")
@@ -48,8 +52,8 @@ public final class BoardObservation implements Cloneable {
4852
@JsonProperty("snapshotName")
4953
public String snapshotName;
5054

51-
@JsonProperty("snapshotData")
52-
public JsonImageMat snapshotData;
55+
@JsonProperty("snapshotDataLocation")
56+
public Path snapshotDataLocation;
5357

5458
@JsonCreator
5559
public BoardObservation(
@@ -59,14 +63,14 @@ public BoardObservation(
5963
@JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
6064
@JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
6165
@JsonProperty("snapshotName") String snapshotName,
62-
@JsonProperty("snapshotData") JsonImageMat snapshotData) {
66+
@JsonProperty("snapshotDataLocation") Path snapshotDataLocation) {
6367
this.locationInObjectSpace = locationInObjectSpace;
6468
this.locationInImageSpace = locationInImageSpace;
6569
this.reprojectionErrors = reprojectionErrors;
6670
this.optimisedCameraToObject = optimisedCameraToObject;
6771
this.includeObservationInCalibration = includeObservationInCalibration;
6872
this.snapshotName = snapshotName;
69-
this.snapshotData = snapshotData;
73+
this.snapshotDataLocation = snapshotDataLocation;
7074
}
7175

7276
@Override
@@ -83,8 +87,8 @@ public String toString() {
8387
+ includeObservationInCalibration
8488
+ ", snapshotName="
8589
+ snapshotName
86-
+ ", snapshotData="
87-
+ snapshotData
90+
+ ", snapshotDataLocation="
91+
+ snapshotDataLocation
8892
+ "]";
8993
}
9094

‎photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java

Lines changed: 46 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,16 @@
1717

1818
package org.photonvision.vision.pipe.impl;
1919

20+
import java.nio.file.Path;
21+
import java.nio.file.Paths;
2022
import java.util.ArrayList;
2123
import java.util.Arrays;
2224
import java.util.List;
2325
import java.util.stream.Collectors;
26+
import org.apache.commons.io.FileUtils;
2427
import org.opencv.calib3d.Calib3d;
2528
import org.opencv.core.*;
29+
import org.opencv.imgcodecs.Imgcodecs;
2630
import org.photonvision.common.logging.LogGroup;
2731
import org.photonvision.common.logging.Logger;
2832
import org.photonvision.common.util.math.MathUtils;
@@ -32,7 +36,6 @@
3236
import org.photonvision.vision.calibration.BoardObservation;
3337
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
3438
import org.photonvision.vision.calibration.CameraLensModel;
35-
import org.photonvision.vision.calibration.JsonImageMat;
3639
import org.photonvision.vision.calibration.JsonMatOfDouble;
3740
import org.photonvision.vision.frame.FrameStaticProperties;
3841
import org.photonvision.vision.pipe.CVPipe;
@@ -46,11 +49,15 @@ public class Calibrate3dPipe
4649
public static class CalibrationInput {
4750
final List<FindBoardCornersPipe.FindBoardCornersPipeResult> observations;
4851
final FrameStaticProperties imageProps;
52+
final Path imageSavePath;
4953

5054
public CalibrationInput(
51-
List<FindBoardCornersPipeResult> observations, FrameStaticProperties imageProps) {
55+
List<FindBoardCornersPipeResult> observations,
56+
FrameStaticProperties imageProps,
57+
Path imageSavePath) {
5258
this.observations = observations;
5359
this.imageProps = imageProps;
60+
this.imageSavePath = imageSavePath;
5461
}
5562
}
5663

@@ -86,16 +93,23 @@ protected CameraCalibrationCoefficients process(CalibrationInput in) {
8693

8794
CameraCalibrationCoefficients ret;
8895
var start = System.nanoTime();
96+
8997
if (MrCalJNILoader.getInstance().isLoaded() && params.useMrCal) {
9098
logger.debug("Calibrating with mrcal!");
9199
ret =
92100
calibrateMrcal(
93-
filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
101+
filteredIn,
102+
in.imageProps.horizontalFocalLength,
103+
in.imageProps.verticalFocalLength,
104+
in.imageSavePath);
94105
} else {
95106
logger.debug("Calibrating with opencv!");
96107
ret =
97108
calibrateOpenCV(
98-
filteredIn, in.imageProps.horizontalFocalLength, in.imageProps.verticalFocalLength);
109+
filteredIn,
110+
in.imageProps.horizontalFocalLength,
111+
in.imageProps.verticalFocalLength,
112+
in.imageSavePath);
99113
}
100114
var dt = System.nanoTime() - start;
101115

@@ -116,7 +130,10 @@ protected CameraCalibrationCoefficients process(CalibrationInput in) {
116130
}
117131

118132
protected CameraCalibrationCoefficients calibrateOpenCV(
119-
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
133+
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in,
134+
double fxGuess,
135+
double fyGuess,
136+
Path imageSavePath) {
120137
List<MatOfPoint3f> objPointsIn =
121138
in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
122139
List<MatOfPoint2f> imgPointsIn =
@@ -179,7 +196,8 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
179196
JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
180197

181198
var observations =
182-
createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null);
199+
createObservations(
200+
in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null, imageSavePath);
183201

184202
cameraMatrix.release();
185203
distortionCoefficients.release();
@@ -200,7 +218,10 @@ protected CameraCalibrationCoefficients calibrateOpenCV(
200218
}
201219

202220
protected CameraCalibrationCoefficients calibrateMrcal(
203-
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in, double fxGuess, double fyGuess) {
221+
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in,
222+
double fxGuess,
223+
double fyGuess,
224+
Path imageSavePath) {
204225
List<MatOfPoint2f> corner_locations =
205226
in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
206227

@@ -297,7 +318,8 @@ protected CameraCalibrationCoefficients calibrateMrcal(
297318
distortionCoefficientsMat.getAsMatOfDouble(),
298319
rvecs,
299320
tvecs,
300-
new double[] {result.warp_x, result.warp_y});
321+
new double[] {result.warp_x, result.warp_y},
322+
imageSavePath);
301323

302324
rvecs.forEach(Mat::release);
303325
tvecs.forEach(Mat::release);
@@ -319,10 +341,19 @@ private List<BoardObservation> createObservations(
319341
MatOfDouble distortionCoefficients_,
320342
List<Mat> rvecs,
321343
List<Mat> tvecs,
322-
double[] calobject_warp) {
344+
double[] calobject_warp,
345+
Path imageSavePath) {
323346
List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
324347
List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
325348

349+
// Clear the calibration image folder of any old images before we save the new ones.
350+
351+
try {
352+
FileUtils.cleanDirectory(imageSavePath.toFile());
353+
} catch (Exception e) {
354+
logger.error("Failed to clean calibration image directory", e);
355+
}
356+
326357
// For each observation, calc reprojection error
327358
Mat jac_temp = new Mat();
328359
List<BoardObservation> observations = new ArrayList<>();
@@ -383,14 +414,17 @@ private List<BoardObservation> createObservations(
383414

384415
var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i));
385416

386-
JsonImageMat image = null;
387417
var inputImage = in.get(i).inputImage;
418+
Path image_path = null;
419+
String snapshotName = "img" + i + ".png";
388420
if (inputImage != null) {
389-
image = new JsonImageMat(inputImage);
421+
image_path = Paths.get(imageSavePath.toString(), snapshotName);
422+
Imgcodecs.imwrite(image_path.toString(), inputImage);
390423
}
424+
391425
observations.add(
392426
new BoardObservation(
393-
i_objPts, i_imgPts, reprojectionError, camToBoard, true, "img" + i + ".png", image));
427+
i_objPts, i_imgPts, reprojectionError, camToBoard, true, snapshotName, image_path));
394428
}
395429
jac_temp.release();
396430

‎photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
package org.photonvision.vision.pipeline;
1919

2020
import edu.wpi.first.math.util.Units;
21+
import java.nio.file.Path;
2122
import java.util.ArrayList;
2223
import java.util.List;
2324
import java.util.stream.Collectors;
@@ -69,11 +70,11 @@ public class Calibrate3dPipeline
6970

7071
private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
7172

72-
public Calibrate3dPipeline(String uniqueName) {
73-
this(12, uniqueName);
73+
public Calibrate3dPipeline() {
74+
this(12);
7475
}
7576

76-
public Calibrate3dPipeline(int minSnapshots, String uniqueName) {
77+
public Calibrate3dPipeline(int minSnapshots) {
7778
super(PROCESSING_TYPE);
7879
this.settings = new Calibration3dPipelineSettings();
7980
this.foundCornersList = new ArrayList<>();
@@ -174,7 +175,7 @@ public boolean hasEnough() {
174175
return foundCornersList.size() >= minSnapshots;
175176
}
176177

177-
public CameraCalibrationCoefficients tryCalibration() {
178+
public CameraCalibrationCoefficients tryCalibration(Path imageSavePath) {
178179
if (!hasEnough()) {
179180
logger.info(
180181
"Not enough snapshots! Only got "
@@ -193,7 +194,8 @@ public CameraCalibrationCoefficients tryCalibration() {
193194
* and returns the corresponding image and object points
194195
*/
195196
calibrationOutput =
196-
calibrate3dPipe.run(new CalibrationInput(foundCornersList, frameStaticProperties));
197+
calibrate3dPipe.run(
198+
new CalibrationInput(foundCornersList, frameStaticProperties, imageSavePath));
197199

198200
this.calibrating = false;
199201

0 commit comments

Comments
 (0)