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

Save calibration data and show preliminary GUI (#1078)

Browse Source 
* Serialize all calibration data

* Run lint

* typing nit

* fix code

* move these tables around some

* Add cool formatting

* add request to get snapshots by resolution and camera

* re-enable all resolutions

* add wip so i can change computers (SQUASH ME AND KILL ME AHHHH)

* Get everything working but viewing snapshots

* Update RequestHandler.java

* Update CameraCalibrationInfoCard.vue

* Update CameraCalibrationInfoCard.vue

* add observation viewer

* round

* fix illiegal import

* Swap to PNG and serialize insolution

* move import/export buttons TO THE TOP

* Update WebsocketDataTypes.ts

* Add snapshotname to observation

* Refactor to serialize snapshot image itself

* Run lint

* Use new base64 image data in info card

* Update SettingTypes.ts

* Create calibration json -> mrcal converter script

* Update calibrationUtils.py

* Fix calibrate NPEs in teest

* Run lint

* Always run cornersubpix

* Update CameraCalibrationInfoCard.vue

Update CameraCalibrationInfoCard.vue

* Update OpenCVHelp.java

* Update OpenCVHelp.java

* Replace test mode camera JSONs

* Run wpiformat

* Revert intrinsics but keep other data

* Remove misc comments

* Rename JsonMat->JsonImageMat and add calobject_warp

* Update Server.java

* Rename cameraExtrinsics to distCoeffs

* fix typing issues

* use util methods

* Formatting fixes

* fix styling

* move to devTools

* remove unneeded or unused imports

* Remove fixed-right css

If its really that big of a deal, we can add it back later, kind of a drag to fix rn.

* Create util method

* Remove extra legacy calibration things

---------

Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
This commit is contained in:
Matt
2024-01-03 14:32:04 -07:00
committed by GitHub
parent e685334baa
commit 7f09f9e4f5
43 changed files with 1247 additions and 396 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java
View File

@@ -28,6 +28,7 @@ import org.photonvision.common.hardware.Platform;
import org.photonvision.common.networking.NetworkUtils;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.raspi.LibCameraJNILoader;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
import org.photonvision.vision.processes.VisionSource;
@@ -166,13 +167,14 @@ public class PhotonConfiguration {
public double fov;
public String nickname;
public String uniqueName;
public HashMap<String, Object> currentPipelineSettings;
public int currentPipelineIndex;
public List<String> pipelineNicknames;
public HashMap<Integer, HashMap<String, Object>> videoFormatList;
public int outputStreamPort;
public int inputStreamPort;
public List<HashMap<String, Object>> calibrations;
public List<CameraCalibrationCoefficients> calibrations;
public boolean isFovConfigurable = true;
public boolean isCSICamera;
}

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

@@ -184,7 +184,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
&& result.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
var fsp = result.inputAndOutputFrame.frameStaticProperties;
ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr());
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr());
} else {
ts.cameraIntrinsicsPublisher.accept(new double[] {});
ts.cameraDistortionPublisher.accept(new double[] {});

4
photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java
View File

@@ -24,4 +24,8 @@ public class ColorHelper {
public static Scalar colorToScalar(Color color) {
return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
}
public static Scalar colorToScalar(Color color, double alpha) {
return new Scalar(color.getBlue(), color.getGreen(), color.getRed(), alpha);
}
}

8
photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
View File

@@ -356,6 +356,10 @@ public class TestUtils {
return getCoeffs(LIFECAM_480P_CAL_FILE, testMode);
}
public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) {
return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode);
}
public static CameraCalibrationCoefficients getLaptop() {
return getCoeffs("laptop.json", true);
}
@@ -389,8 +393,4 @@ public class TestUtils {
.resolve("testimages")
.resolve(WPI2022Image.kTerminal22ft6in.path);
}
public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) {
return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode);
}
}

14
photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
View File

@@ -32,6 +32,7 @@ import java.io.FileOutputStream;
import java.io.IOException;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
public class JacksonUtils {
public static class UIMap extends HashMap<String, Object> {}
@@ -61,6 +62,19 @@ public class JacksonUtils {
saveJsonString(json, path, forceSync);
}
public static <T> T deserialize(Map<?, ?> s, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper =
JsonMapper.builder()
.configure(JsonReadFeature.ALLOW_JAVA_COMMENTS, true)
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT)
.build();
return objectMapper.convertValue(s, ref);
}
public static <T> T deserialize(String s, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();

21
photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

@@ -23,10 +23,12 @@ import edu.wpi.first.math.geometry.CoordinateSystem;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Arrays;
import java.util.List;
import org.opencv.core.Core;
import org.opencv.core.Mat;
public class MathUtils {
@@ -198,4 +200,23 @@ public class MathUtils {
var axis = rotation.getAxis().times(angle);
rvecOutput.put(0, 0, axis.getData());
}
/**
* Convert an Opencv rvec+tvec pair to a Pose3d.
*
* @param rVec Axis-angle rotation vector, where norm(rVec) is the angle about a unit vector in
* the direction of rVec
* @param tVec 3D translation
* @return Pose3d representing the same rigid transform
*/
public static Pose3d opencvRTtoPose3d(Mat rVec, Mat tVec) {
Translation3d translation =
new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
Rotation3d rotation =
new Rotation3d(
VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]),
Core.norm(rVec));
return new Pose3d(translation, rotation);
}
}

71
photon-core/src/main/java/org/photonvision/vision/calibration/BoardObservation.java Normal file
View File

@@ -0,0 +1,71 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.opencv.core.Point;
import org.opencv.core.Point3;
public final class BoardObservation {
// Expected feature 3d location in the camera frame
@JsonProperty("locationInObjectSpace")
public List<Point3> locationInObjectSpace;
// Observed location in pixel space
@JsonProperty("locationInImageSpace")
public List<Point> locationInImageSpace;
// (measured location in pixels) - (expected from FK)
@JsonProperty("reprojectionErrors")
public List<Point> reprojectionErrors;
// Solver optimized board poses
@JsonProperty("optimisedCameraToObject")
public Pose3d optimisedCameraToObject;
// If we should use this observation when re-calculating camera calibration
@JsonProperty("includeObservationInCalibration")
public boolean includeObservationInCalibration;
@JsonProperty("snapshotName")
public String snapshotName;
@JsonProperty("snapshotData")
public JsonImageMat snapshotData;
@JsonCreator
public BoardObservation(
@JsonProperty("locationInObjectSpace") List<Point3> locationInObjectSpace,
@JsonProperty("locationInImageSpace") List<Point> locationInImageSpace,
@JsonProperty("reprojectionErrors") List<Point> reprojectionErrors,
@JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
@JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
@JsonProperty("snapshotName") String snapshotName,
@JsonProperty("snapshotData") JsonImageMat snapshotData) {
this.locationInObjectSpace = locationInObjectSpace;
this.locationInImageSpace = locationInImageSpace;
this.reprojectionErrors = reprojectionErrors;
this.optimisedCameraToObject = optimisedCameraToObject;
this.includeObservationInCalibration = includeObservationInCalibration;
this.snapshotName = snapshotName;
this.snapshotData = snapshotData;
}
}

75
photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
View File

@@ -20,50 +20,72 @@ package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonAlias;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import com.fasterxml.jackson.databind.JsonNode;
import java.util.List;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
import org.photonvision.vision.opencv.Releasable;
@JsonIgnoreProperties(ignoreUnknown = true)
public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("resolution")
public final Size resolution;
@JsonProperty("cameraIntrinsics")
public final JsonMat cameraIntrinsics;
public final JsonMatOfDouble cameraIntrinsics;
@JsonProperty("cameraExtrinsics")
@JsonAlias({"cameraExtrinsics", "distCoeffs"})
public final JsonMat distCoeffs;
@JsonProperty("distCoeffs")
@JsonAlias({"distCoeffs", "distCoeffs"})
public final JsonMatOfDouble distCoeffs;
@JsonProperty("perViewErrors")
public final double[] perViewErrors;
@JsonProperty("observations")
public final List<BoardObservation> observations;
@JsonProperty("standardDeviation")
public final double standardDeviation;
@JsonProperty("calobjectWarp")
public final double[] calobjectWarp;
@JsonIgnore private final double[] intrinsicsArr = new double[9];
@JsonIgnore private final double[] distCoeffsArr = new double[5];
@JsonIgnore private final double[] extrinsicsArr = new double[5];
/**
* Contains all camera calibration data for a particular resolution of a camera. Designed for use
* with standard opencv camera calibration matrices. For details on the layout of camera
* intrinsics/distortion matrices, see:
* https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d
*
* @param resolution The resolution this applies to. We don't assume camera binning or try
* rescaling calibration
* @param cameraIntrinsics Camera intrinsics parameters matrix, in the standard opencv form.
* @param distCoeffs Camera distortion coefficients array. Variable length depending on order of
* distortion model
* @param calobjectWarp Board deformation parameters, for calibrators that can estimate that. See:
* https://mrcal.secretsauce.net/formulation.html#board-deformation
* @param observations List of snapshots used to construct this calibration
*/
@JsonCreator
public CameraCalibrationCoefficients(
@JsonProperty("resolution") Size resolution,
@JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics,
@JsonProperty("cameraExtrinsics") JsonMat distCoeffs,
@JsonProperty("perViewErrors") double[] perViewErrors,
@JsonProperty("standardDeviation") double standardDeviation) {
@JsonProperty("cameraIntrinsics") JsonMatOfDouble cameraIntrinsics,
@JsonProperty("distCoeffs") JsonMatOfDouble distCoeffs,
@JsonProperty("calobjectWarp") double[] calobjectWarp,
@JsonProperty("observations") List<BoardObservation> observations) {
this.resolution = resolution;
this.cameraIntrinsics = cameraIntrinsics;
this.distCoeffs = distCoeffs;
this.perViewErrors = perViewErrors;
this.standardDeviation = standardDeviation;
this.calobjectWarp = calobjectWarp;
// Legacy migration just to make sure that observations is at worst empty and never null
if (observations == null) {
observations = List.of();
}
this.observations = observations;
// do this once so gets are quick
getCameraIntrinsicsMat().get(0, 0, intrinsicsArr);
getDistCoeffsMat().get(0, 0, extrinsicsArr);
getDistCoeffsMat().get(0, 0, distCoeffsArr);
}
@JsonIgnore
@@ -82,18 +104,13 @@ public class CameraCalibrationCoefficients implements Releasable {
}
@JsonIgnore
public double[] getExtrinsicsArr() {
return extrinsicsArr;
public double[] getDistCoeffsArr() {
return distCoeffsArr;
}
@JsonIgnore
public double[] getPerViewErrors() {
return perViewErrors;
}
@JsonIgnore
public double getStandardDeviation() {
return standardDeviation;
public List<BoardObservation> getPerViewErrors() {
return observations;
}
@Override
@@ -130,14 +147,14 @@ public class CameraCalibrationCoefficients implements Releasable {
dist_coefs.get(4).doubleValue(),
};
var cam_jsonmat = new JsonMat(3, 3, cam_arr);
var distortion_jsonmat = new JsonMat(1, 5, dist_array);
var cam_jsonmat = new JsonMatOfDouble(3, 3, cam_arr);
var distortion_jsonmat = new JsonMatOfDouble(1, 5, dist_array);
var error = json.get("avg_reprojection_error").asDouble();
var width = json.get("img_size").get(0).doubleValue();
var height = json.get("img_size").get(1).doubleValue();
return new CameraCalibrationCoefficients(
new Size(width, height), cam_jsonmat, distortion_jsonmat, new double[] {error}, 0);
new Size(width, height), cam_jsonmat, distortion_jsonmat, new double[0], List.of());
}
}

79
photon-core/src/main/java/org/photonvision/vision/calibration/JsonImageMat.java Normal file
View File

@@ -0,0 +1,79 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.Base64;
import org.opencv.core.Mat;
import org.opencv.core.MatOfByte;
import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.vision.opencv.Releasable;
/** JSON-serializable image. Data is stored as base64-encoded PNG data. */
public class JsonImageMat implements Releasable {
public final int rows;
public final int cols;
public final int type;
// We store image data as a base64-encoded PNG inside a Java string. This lets us serialize it
// without too much overhead and still use JSON.
public final String data;
// Cached matrices to avoid object recreation
@JsonIgnore private Mat wrappedMat = null;
public JsonImageMat(Mat mat) {
this.rows = mat.rows();
this.cols = mat.cols();
this.type = mat.type();
// Convert from Mat -> png byte array -> base64
var buf = new MatOfByte();
Imgcodecs.imencode(".png", mat, buf);
data = Base64.getEncoder().encodeToString(buf.toArray());
buf.release();
}
public JsonImageMat(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@JsonProperty("data") String data) {
this.rows = rows;
this.cols = cols;
this.type = type;
this.data = data;
}
@JsonIgnore
public Mat getAsMat() {
if (wrappedMat == null) {
// Convert back from base64 string -> png -> Mat
var bytes = Base64.getDecoder().decode(data);
var pngData = new MatOfByte(bytes);
this.wrappedMat = Imgcodecs.imdecode(pngData, Imgcodecs.IMREAD_COLOR);
}
return this.wrappedMat;
}
@Override
public void release() {
if (wrappedMat != null) wrappedMat.release();
}
}

11
photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java → photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java
View File

@@ -29,7 +29,8 @@ import org.opencv.core.MatOfDouble;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.opencv.Releasable;
public class JsonMat implements Releasable {
/** JSON-serializable image. Data is stored as a raw JSON array. */
public class JsonMatOfDouble implements Releasable {
public final int rows;
public final int cols;
public final int type;
@@ -41,11 +42,11 @@ public class JsonMat implements Releasable {
private MatOfDouble wrappedMatOfDouble;
public JsonMat(int rows, int cols, double[] data) {
public JsonMatOfDouble(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
}
public JsonMat(
public JsonMatOfDouble(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@@ -84,9 +85,9 @@ public class JsonMat implements Releasable {
return Arrays.copyOfRange(data, 0, dataLen);
}
public static JsonMat fromMat(Mat mat) {
public static JsonMatOfDouble fromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat));
}
@JsonIgnore

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

@@ -22,20 +22,23 @@ import com.fasterxml.jackson.databind.ObjectMapper;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.calibration.BoardObservation;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.calibration.JsonMat;
import org.photonvision.vision.calibration.JsonImageMat;
import org.photonvision.vision.calibration.JsonMatOfDouble;
import org.photonvision.vision.pipe.CVPipe;
public class Calibrate3dPipe
extends CVPipe<
List<Triple<Size, Mat, Mat>>,
List<FindBoardCornersPipe.FindBoardCornersPipeResult>,
CameraCalibrationCoefficients,
Calibrate3dPipe.CalibratePipeParams> {
// Camera matrix stores the center of the image and focal length across the x and y-axis in a 3x3
@@ -69,31 +72,34 @@ public class Calibrate3dPipe
* @return Result of processing.
*/
@Override
protected CameraCalibrationCoefficients process(List<Triple<Size, Mat, Mat>> in) {
protected CameraCalibrationCoefficients process(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
in =
in.stream()
.filter(
it ->
it != null
&& it.getLeft() != null
&& it.getMiddle() != null
&& it.getRight() != null)
&& it.imagePoints != null
&& it.objectPoints != null
&& it.size != null)
.collect(Collectors.toList());
List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
if (objPoints.size() != imgPts.size()) {
logger.error("objpts.size != imgpts.size");
return null;
}
try {
// FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
// imageSize from, other parameters are output Mats
var objPoints = in.stream().map(Triple::getMiddle).collect(Collectors.toList());
var imgPts = in.stream().map(Triple::getRight).collect(Collectors.toList());
if (objPoints.size() != imgPts.size()) {
logger.error("objpts.size != imgpts.size");
return null;
}
calibrationAccuracy =
Calib3d.calibrateCameraExtended(
objPoints,
imgPts,
new Size(in.get(0).getLeft().width, in.get(0).getLeft().height),
new Size(in.get(0).size.width, in.get(0).size.height),
cameraMatrix,
distortionCoefficients,
rvecs,
@@ -106,15 +112,59 @@ public class Calibrate3dPipe
e.printStackTrace();
return null;
}
JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix);
JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients);
// Create a new CameraCalibrationCoefficients object to pass onto SolvePnP
double[] perViewErrorsArray =
new double[(int) perViewErrors.total() * perViewErrors.channels()];
perViewErrors.get(0, 0, perViewErrorsArray);
JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix);
JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
// For each observation, calc reprojection error
Mat jac_temp = new Mat();
List<BoardObservation> observations = new ArrayList<>();
for (int i = 0; i < objPoints.size(); i++) {
MatOfPoint3f i_objPtsNative = new MatOfPoint3f();
objPoints.get(i).copyTo(i_objPtsNative);
var i_objPts = i_objPtsNative.toList();
var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList();
var img_pts_reprojected = new MatOfPoint2f();
try {
Calib3d.projectPoints(
i_objPtsNative,
rvecs.get(i),
tvecs.get(i),
cameraMatrix,
distortionCoefficients,
img_pts_reprojected,
jac_temp,
0.0);
} catch (Exception e) {
e.printStackTrace();
continue;
}
var img_pts_reprojected_list = img_pts_reprojected.toList();
var reprojectionError = new ArrayList<Point>();
for (int j = 0; j < img_pts_reprojected_list.size(); j++) {
// error = (measured - expected)
var measured = img_pts_reprojected_list.get(j);
var expected = i_imgPts.get(j);
var error = new Point(measured.x - expected.x, measured.y - expected.y);
reprojectionError.add(error);
}
var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i));
JsonImageMat image = null;
var inputImage = in.get(i).inputImage;
if (inputImage != null) {
image = new JsonImageMat(inputImage);
}
observations.add(
new BoardObservation(
i_objPts, i_imgPts, reprojectionError, camToBoard, true, "img" + i + ".png", image));
}
jac_temp.release();
// Standard deviation of results
double stdDev = calculateSD(perViewErrorsArray);
try {
// Print calibration successful
logger.info(
@@ -126,32 +176,12 @@ public class Calibrate3dPipe
+ new ObjectMapper().writeValueAsString(cameraMatrixMat)
+ "\ndistortionCoeffs:\n"
+ new ObjectMapper().writeValueAsString(distortionCoefficientsMat)
+ "\nWith Standard Deviation Of\n"
+ stdDev
+ "\n");
} catch (JsonProcessingException e) {
logger.error("Failed to parse calibration data to json!", e);
}
return new CameraCalibrationCoefficients(
params.resolution, cameraMatrixMat, distortionCoefficientsMat, perViewErrorsArray, stdDev);
}
// Calculate standard deviation of the RMS error of the snapshots
private static double calculateSD(double[] numArray) {
double sum = 0.0, standardDeviation = 0.0;
int length = numArray.length;
for (double num : numArray) {
sum += num;
}
double mean = sum / length;
for (double num : numArray) {
standardDeviation += Math.pow(num - mean, 2);
}
return Math.sqrt(standardDeviation / length);
params.resolution, cameraMatrixMat, distortionCoefficientsMat, new double[0], observations);
}
public static class CalibratePipeParams {

62
photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java Normal file
View File

@@ -0,0 +1,62 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.pipe.impl;
import java.awt.Color;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.pipe.MutatingPipe;
import org.photonvision.vision.target.TrackedTarget;
public class DrawCalibrationPipe
extends MutatingPipe<
Pair<Mat, List<TrackedTarget>>, DrawCalibrationPipe.DrawCalibrationPipeParams> {
@Override
protected Void process(Pair<Mat, List<TrackedTarget>> in) {
var image = in.getLeft();
for (var target : in.getRight()) {
for (var c : target.getTargetCorners()) {
c =
new Point(
c.x / params.divisor.value.doubleValue(), c.y / params.divisor.value.doubleValue());
var r = 4;
var r2 = r / Math.sqrt(2);
var color = ColorHelper.colorToScalar(Color.RED, 0.4);
Imgproc.circle(image, c, r, color, 1);
Imgproc.line(image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color);
Imgproc.line(image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color);
}
}
return null;
}
public static class DrawCalibrationPipeParams {
private final FrameDivisor divisor;
public DrawCalibrationPipeParams(FrameDivisor divisor) {
this.divisor = divisor;
}
}
}

72
photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
View File

@@ -18,19 +18,21 @@
package org.photonvision.vision.pipe.impl;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.pipeline.UICalibrationData;
public class FindBoardCornersPipe
extends CVPipe<
Pair<Mat, Mat>, Triple<Size, Mat, Mat>, FindBoardCornersPipe.FindCornersPipeParams> {
Pair<Mat, Mat>,
FindBoardCornersPipe.FindBoardCornersPipeResult,
FindBoardCornersPipe.FindCornersPipeParams> {
private static final Logger logger =
new Logger(FindBoardCornersPipe.class, LogGroup.VisionModule);
@@ -112,10 +114,7 @@ public class FindBoardCornersPipe
* @return All valid Mats for camera calibration
*/
@Override
protected Triple<Size, Mat, Mat> process(Pair<Mat, Mat> in) {
// Create the object points
createObjectPoints();
protected FindBoardCornersPipeResult process(Pair<Mat, Mat> in) {
return findBoardCorners(in);
}
@@ -217,7 +216,7 @@ public class FindBoardCornersPipe
*
* @return Frame resolution, object points, board corners
*/
private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
createObjectPoints();
var inFrame = in.getLeft();
@@ -228,10 +227,15 @@ public class FindBoardCornersPipe
boolean boardFound = false;
if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
// This is for chessboards
// Reduce the image size to be much more manageable
Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame));
// Note that opencv will copy the frame if no resize is requested; we can skip this since we
// don't need that copy. See:
// https://github.com/opencv/opencv/blob/a8ec6586118c3f8e8f48549a85f2da7a5b78bcc9/modules/imgproc/src/resize.cpp#L4185
if (params.divisor != FrameDivisor.NONE) {
Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame));
} else {
smallerInFrame = inFrame;
}
// Run the chessboard corner finder on the smaller image
boardFound =
@@ -244,14 +248,13 @@ public class FindBoardCornersPipe
}
} else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
// For dot boards
boardFound =
Calib3d.findCirclesGrid(
inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
}
if (!boardFound) {
// If we can't find a chessboard/dot board, just return
// If we can't find a chessboard/dot board, give up
return null;
}
@@ -264,31 +267,23 @@ public class FindBoardCornersPipe
// Get the size of the inFrame
this.imageSize = new Size(inFrame.width(), inFrame.height());
// Do sub corner pix for drawing chessboard
// Do sub corner pix for drawing chessboard when using OpenCV
Imgproc.cornerSubPix(
inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria);
// convert back to BGR
// Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_GRAY2BGR);
// draw the chessboard, doesn't have to be different for a dot board since it just re projects
// the corners we found
Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true);
// // Add the 3D points and the points of the corners found
// if (addToSnapList) {
// this.listOfObjectPoints.add(objectPoints);
// this.listOfImagePoints.add(boardCorners);
// }
return Triple.of(inFrame.size(), objPts, outBoardCorners);
return new FindBoardCornersPipeResult(inFrame.size(), objPts, outBoardCorners);
}
public static class FindCornersPipeParams {
private final int boardHeight;
private final int boardWidth;
private final UICalibrationData.BoardType type;
private final double gridSize;
private final FrameDivisor divisor;
final int boardHeight;
final int boardWidth;
final UICalibrationData.BoardType type;
final double gridSize;
final FrameDivisor divisor;
public FindCornersPipeParams(
int boardHeight,
@@ -331,4 +326,27 @@ public class FindBoardCornersPipe
return divisor == other.divisor;
}
}
public static class FindBoardCornersPipeResult implements Releasable {
public Size size;
public MatOfPoint2f objectPoints;
public MatOfPoint2f imagePoints;
// Set later only if we need it
public Mat inputImage = null;
public FindBoardCornersPipeResult(
Size size, MatOfPoint2f objectPoints, MatOfPoint2f imagePoints) {
this.size = size;
this.objectPoints = objectPoints;
this.imagePoints = imagePoints;
}
@Override
public void release() {
objectPoints.release();
imagePoints.release();
if (inputImage != null) inputImage.release();
}
}
}

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

@@ -18,39 +18,38 @@
package org.photonvision.vision.pipeline;
import edu.wpi.first.math.util.Units;
import java.nio.file.Path;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.file.FileUtils;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.calibration.BoardObservation;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.ImageRotationMode;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
public class Calibrate3dPipeline
extends CVPipeline<CVPipelineResult, Calibration3dPipelineSettings> {
// For logging
private static final Logger logger = new Logger(Calibrate3dPipeline.class, LogGroup.General);
// Only 2 pipes needed, one for finding the board corners and one for actually calibrating
// Find board corners decides internally between opencv and mrgingham
private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
@@ -59,7 +58,7 @@ public class Calibrate3dPipeline
private boolean takeSnapshot = false;
// Output of the corners
final List<Triple<Size, Mat, Mat>> foundCornersList;
final List<FindBoardCornersPipeResult> foundCornersList;
/// Output of the calibration, getter method is set for this.
private CVPipeResult<CameraCalibrationCoefficients> calibrationOutput;
@@ -68,16 +67,13 @@ public class Calibrate3dPipeline
private boolean calibrating = false;
// Path to save images
private final Path imageDir = ConfigManager.getInstance().getCalibDir();
private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
public Calibrate3dPipeline() {
this(12);
public Calibrate3dPipeline(String uniqueName) {
this(12, uniqueName);
}
public Calibrate3dPipeline(int minSnapshots) {
public Calibrate3dPipeline(int minSnapshots, String uniqueName) {
super(PROCESSING_TYPE);
this.settings = new Calibration3dPipelineSettings();
this.foundCornersList = new ArrayList<>();
@@ -99,11 +95,6 @@ public class Calibrate3dPipeline
new Calibrate3dPipe.CalibratePipeParams(
new Size(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight));
calibrate3dPipe.setParams(calibratePipeParams);
// if (cameraQuirks.hasQuirk(CameraQuirk.PiCam) && LibCameraJNI.isSupported()) {
// LibCameraJNI.setRotation(settings.inputImageRotationMode.value);
// // LibCameraJNI.setShouldCopyColor(true);
// }
}
@Override
@@ -114,12 +105,22 @@ public class Calibrate3dPipeline
return new CVPipelineResult(0, 0, null, frame);
}
if (getSettings().inputImageRotationMode != ImageRotationMode.DEG_0) {
// All this calibration assumes zero rotation. If we want a rotation, it should be applied at
// the output
logger.error(
"Input image rotation was non-zero! Calibration wasn't designed to deal with this. Attempting to manually change back to zero");
getSettings().inputImageRotationMode = ImageRotationMode.DEG_0;
return new CVPipelineResult(0, 0, List.of(), frame);
}
long sumPipeNanosElapsed = 0L;
// Check if the frame has chessboard corners
var outputColorCVMat = new CVMat();
inputColorMat.copyTo(outputColorCVMat.getMat());
var findBoardResult =
FindBoardCornersPipeResult findBoardResult =
findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output;
var fpsResult = calculateFPSPipe.run(null);
@@ -130,10 +131,10 @@ public class Calibrate3dPipeline
takeSnapshot = false;
if (findBoardResult != null) {
// Only copy the image into the result when we absolutely must
findBoardResult.inputImage = inputColorMat.clone();
foundCornersList.add(findBoardResult);
Imgcodecs.imwrite(
Path.of(imageDir.toString(), "img" + foundCornersList.size() + ".jpg").toString(),
inputColorMat);
// update the UI
broadcastState();
@@ -143,19 +144,18 @@ public class Calibrate3dPipeline
frame.release();
// Return the drawn chessboard if corners are found, if not, then return the input image.
return new CVPipelineResult(
return new CalibrationPipelineResult(
sumPipeNanosElapsed,
fps, // Unused but here in case
Collections.emptyList(),
new MultiTargetPNPResult(),
new Frame(
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties),
getCornersList());
}
public void deleteSavedImages() {
imageDir.toFile().mkdirs();
imageDir.toFile().mkdir();
FileUtils.deleteDirectory(imageDir);
List<List<Point>> getCornersList() {
return foundCornersList.stream()
.map(it -> it.imagePoints.toList())
.collect(Collectors.toList());
}
public boolean hasEnough() {
@@ -188,16 +188,12 @@ public class Calibrate3dPipeline
takeSnapshot = true;
}
public double[] perViewErrors() {
return calibrationOutput.output.perViewErrors;
public List<BoardObservation> perViewErrors() {
return calibrationOutput.output.observations;
}
public void finishCalibration() {
foundCornersList.forEach(
it -> {
it.getMiddle().release();
it.getRight().release();
});
foundCornersList.forEach(it -> it.release());
foundCornersList.clear();
broadcastState();

16
photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java
View File

@@ -38,6 +38,7 @@ public class OutputStreamPipeline {
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
private final DrawCalibrationPipe drawCalibrationPipe = new DrawCalibrationPipe();
private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
@@ -113,6 +114,9 @@ public class OutputStreamPipeline {
resizeImagePipe.setParams(
new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
drawCalibrationPipe.setParams(
new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor));
}
public CVPipelineResult process(
@@ -149,8 +153,9 @@ public class OutputStreamPipeline {
sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
if (!(settings instanceof AprilTagPipelineSettings)
&& !(settings instanceof ArucoPipelineSettings)) {
// If we're processing anything other than Apriltags...
&& !(settings instanceof ArucoPipelineSettings)
&& !(settings instanceof Calibration3dPipelineSettings)) {
// If we're processing anything other than Apriltags..
var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
@@ -172,7 +177,14 @@ public class OutputStreamPipeline {
pipeProfileNanos[7] = 0;
pipeProfileNanos[8] = 0;
}
} else if (settings instanceof Calibration3dPipelineSettings) {
pipeProfileNanos[5] = 0;
pipeProfileNanos[6] = 0;
var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw));
sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
pipeProfileNanos[8] = 0;
} else if (settings instanceof AprilTagPipelineSettings) {
// If we are doing apriltags...
if (settings.solvePNPEnabled) {

30
photon-core/src/main/java/org/photonvision/vision/pipeline/UICalibrationData.java
View File

@@ -17,17 +17,17 @@
package org.photonvision.vision.pipeline;
import java.util.Map;
public class UICalibrationData {
public final int videoModeIndex;
public int videoModeIndex;
public int count;
public final int minCount;
public final boolean hasEnough;
public final double squareSizeIn;
public final int patternWidth;
public final int patternHeight;
public final BoardType boardType; //
public int minCount;
public boolean hasEnough;
public double squareSizeIn;
public int patternWidth;
public int patternHeight;
public BoardType boardType;
public UICalibrationData() {}
public UICalibrationData(
int count,
@@ -53,18 +53,6 @@ public class UICalibrationData {
DOTBOARD
}
public static UICalibrationData fromMap(Map<String, Object> map) {
return new UICalibrationData(
((Number) map.get("count")).intValue(),
((Number) map.get("videoModeIndex")).intValue(),
((Number) map.get("minCount")).intValue(),
(boolean) map.get("hasEnough"),
((Number) map.get("squareSizeIn")).doubleValue(),
((Number) map.get("patternWidth")).intValue(),
((Number) map.get("patternHeight")).intValue(),
BoardType.values()[(int) map.get("boardType")]);
}
@Override
public String toString() {
return "UICalibrationData{"

35
photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java Normal file
View File

@@ -0,0 +1,35 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.pipeline.result;
import java.util.List;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.target.TrackedTarget;
public class CalibrationPipelineResult extends CVPipelineResult {
private static List<TrackedTarget> cornersToTarget(List<List<Point>> corners) {
return corners.stream().map(TrackedTarget::new).collect(Collectors.toList());
}
public CalibrationPipelineResult(
double latencyNanos, double fps, Frame outputFrame, List<List<Point>> corners) {
super(latencyNanos, fps, cornersToTarget(corners), outputFrame);
}
}

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

@@ -37,7 +37,7 @@ public class PipelineManager {
public static final int CAL_3D_INDEX = -2;
protected final List<CVPipelineSettings> userPipelineSettings;
protected final Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline();
protected final Calibrate3dPipeline calibration3dPipeline;
protected final DriverModePipeline driverModePipeline = new DriverModePipeline();
/** Index of the currently active pipeline. Defaults to 0. */
@@ -56,21 +56,23 @@ public class PipelineManager {
/**
* Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
* pipelines.
*
* @param userPipelines Pipelines to add to the manager.
*/
public PipelineManager(
DriverModePipelineSettings driverSettings, List<CVPipelineSettings> userPipelines) {
PipelineManager(
DriverModePipelineSettings driverSettings,
List<CVPipelineSettings> userPipelines,
String uniqueName) {
this.userPipelineSettings = new ArrayList<>(userPipelines);
// This is to respect the default res idx for vendor cameras
this.driverModePipeline.setSettings(driverSettings);
if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective);
calibration3dPipeline = new Calibrate3dPipeline(uniqueName);
}
public PipelineManager(CameraConfiguration config) {
this(config.driveModeSettings, config.pipelineSettings);
this(config.driveModeSettings, config.pipelineSettings, config.uniqueName);
}
/**

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

@@ -26,6 +26,7 @@ import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.function.BiConsumer;
import org.opencv.core.Size;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.PhotonConfiguration;
@@ -335,7 +336,10 @@ public class VisionModule {
public void startCalibration(UICalibrationData data) {
var settings = pipelineManager.calibration3dPipeline.getSettings();
pipelineManager.calibration3dPipeline.deleteSavedImages();
var videoMode = visionSource.getSettables().getAllVideoModes().get(data.videoModeIndex);
var resolution = new Size(videoMode.width, videoMode.height);
settings.cameraVideoModeIndex = data.videoModeIndex;
visionSource.getSettables().setVideoModeIndex(data.videoModeIndex);
logger.info(
@@ -347,6 +351,7 @@ public class VisionModule {
settings.boardHeight = data.patternHeight;
settings.boardWidth = data.patternWidth;
settings.boardType = data.boardType;
settings.resolution = resolution;
// Disable gain if not applicable
if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
@@ -501,6 +506,7 @@ public class VisionModule {
ret.fov = visionSource.getSettables().getFOV();
ret.isCSICamera = visionSource.getCameraConfiguration().cameraType == CameraType.ZeroCopyPicam;
ret.nickname = visionSource.getSettables().getConfiguration().nickname;
ret.uniqueName = visionSource.getSettables().getConfiguration().uniqueName;
ret.currentPipelineSettings =
SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings());
ret.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
@@ -528,20 +534,7 @@ public class VisionModule {
ret.outputStreamPort = this.outputStreamPort;
ret.inputStreamPort = this.inputStreamPort;
var calList = new ArrayList<HashMap<String, Object>>();
for (var c : visionSource.getSettables().getConfiguration().calibrations) {
var internalMap = new HashMap<String, Object>();
internalMap.put("perViewErrors", c.perViewErrors);
internalMap.put("standardDeviation", c.standardDeviation);
internalMap.put("width", c.resolution.width);
internalMap.put("height", c.resolution.height);
internalMap.put("intrinsics", c.cameraIntrinsics.data);
internalMap.put("distCoeffs", c.distCoeffs.data);
calList.add(internalMap);
}
ret.calibrations = calList;
ret.calibrations = visionSource.getSettables().getConfiguration().calibrations;
ret.isFovConfigurable =
!(ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV()

13
photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
View File

@@ -26,6 +26,7 @@ import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -103,9 +104,15 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
parentModule.saveAndBroadcastAll();
return;
case "startCalibration":
var data = UICalibrationData.fromMap((Map<String, Object>) newPropValue);
parentModule.startCalibration(data);
parentModule.saveAndBroadcastAll();
try {
var data =
JacksonUtils.deserialize(
(Map<String, Object>) newPropValue, UICalibrationData.class);
parentModule.startCalibration(data);
parentModule.saveAndBroadcastAll();
} catch (Exception e) {
logger.error("Error deserailizing start-cal request", e);
}
return;
case "saveInputSnapshot":
parentModule.saveInputSnapshot();

8
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -146,6 +146,14 @@ public class TrackedTarget implements Releasable {
m_skew = 0;
}
public TrackedTarget(List<Point> corners) {
m_mainContour = new Contour(new MatOfPoint());
m_mainContour.mat.fromList(List.of(new Point(0, 0), new Point(0, 1), new Point(1, 0)));
this.setTargetCorners(corners);
m_targetOffsetPoint = new Point();
m_robotOffsetPoint = new Point();
}
public TrackedTarget(
ArucoDetectionResult result,
AprilTagPoseEstimate tagPose,

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

@@ -24,10 +24,8 @@ import edu.wpi.first.math.util.Units;
import java.io.File;
import java.nio.file.Path;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.opencv.calib3d.Calib3d;
@@ -44,6 +42,7 @@ import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
public class Calibrate3dPipeTest {
@BeforeAll
@@ -66,7 +65,7 @@ public class Calibrate3dPipeTest {
new FindBoardCornersPipe.FindCornersPipeParams(
11, 4, UICalibrationData.BoardType.DOTBOARD, 15, FrameDivisor.NONE));
List<Triple<Size, Mat, Mat>> foundCornersList = new ArrayList<>();
List<FindBoardCornersPipeResult> foundCornersList = new ArrayList<>();
for (var f : frames) {
var copy = new Mat();
@@ -78,9 +77,7 @@ public class Calibrate3dPipeTest {
calibrate3dPipe.setParams(new Calibrate3dPipe.CalibratePipeParams(new Size(640, 480)));
var calibrate3dPipeOutput = calibrate3dPipe.run(foundCornersList);
assertTrue(calibrate3dPipeOutput.output.perViewErrors.length > 0);
System.out.println(
"Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.output.perViewErrors));
assertTrue(calibrate3dPipeOutput.output.observations.size() > 0);
for (var f : frames) {
f.release();
@@ -94,7 +91,7 @@ public class Calibrate3dPipeTest {
File dir = new File(TestUtils.getDotBoardImagesPath().toAbsolutePath().toString());
File[] directoryListing = dir.listFiles();
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "unique_name_lol");
calibration3dPipeline.getSettings().boardHeight = 11;
calibration3dPipeline.getSettings().boardWidth = 4;
calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.DOTBOARD;
@@ -117,7 +114,7 @@ public class Calibrate3dPipeTest {
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(Triple::getRight)
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
calibration3dPipeline.removeSnapshot(0);
@@ -132,20 +129,16 @@ public class Calibrate3dPipeTest {
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(Triple::getRight)
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
var cal = calibration3dPipeline.tryCalibration();
calibration3dPipeline.finishCalibration();
assertNotNull(cal);
assertNotNull(cal.perViewErrors);
System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors));
assertNotNull(cal.observations);
System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString());
System.out.println("Dist Coeffs: " + cal.distCoeffs.toString());
System.out.println("Standard Deviation: " + cal.standardDeviation);
System.out.println(
"Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString());
// Confirm we didn't get leaky on our mat usage
// assertTrue(CVMat.getMatCount() == startMatCount); // TODO Figure out why this doesn't work in
@@ -260,7 +253,7 @@ public class Calibrate3dPipeTest {
assertTrue(directoryListing.length >= 25);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "test_squares_common");
calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.CHESSBOARD;
calibration3dPipeline.getSettings().resolution = imgRes;
calibration3dPipeline.getSettings().boardHeight = (int) Math.round(boardDim.height);
@@ -288,7 +281,7 @@ public class Calibrate3dPipeTest {
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(Triple::getRight)
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
var cal = calibration3dPipeline.tryCalibration();
@@ -298,7 +291,7 @@ public class Calibrate3dPipeTest {
// Confirm we have indeed gotten valid calibration objects
assertNotNull(cal);
assertNotNull(cal.perViewErrors);
assertNotNull(cal.observations);
// Confirm the calibrated center pixel is fairly close to of the "expected" location at the
// center of the sensor.
@@ -310,12 +303,8 @@ public class Calibrate3dPipeTest {
assertTrue(centerXErrPct < 10.0);
assertTrue(centerYErrPct < 10.0);
System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors));
System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics);
System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString());
System.out.println("Dist Coeffs: " + cal.distCoeffs.toString());
System.out.println("Standard Deviation: " + cal.standardDeviation);
System.out.println(
"Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString());
// Confirm we didn't get leaky on our mat usage
// assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this doesn't

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

@@ -29,7 +29,8 @@ public class PipelineManagerTest {
@Test
public void testUniqueName() {
TestUtils.loadLibraries();
PipelineManager manager = new PipelineManager(new DriverModePipelineSettings(), List.of());
PipelineManager manager =
new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name");
manager.addPipeline(PipelineType.Reflective, "Another");
// We now have ["New Pipeline", "Another"]