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

Use mrcal for camera-calibration (#1036)

Browse Source 
Uses jars built from https://github.com/photonvision/mrcal-java/
See: https://mrcal.secretsauce.net/ and https://docs.photonvision.org/en/latest/docs/calibration/calibration.html#investigating-calibration-data-with-mrcal

---------

Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
This commit is contained in:
Matt
2024-01-05 12:26:17 -07:00
committed by GitHub
parent b033f7e585
commit 0af5a62d5e
59 changed files with 1327 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,6 +27,7 @@ import org.photonvision.PhotonVersion;
import org.photonvision.common.hardware.Platform;
import org.photonvision.common.networking.NetworkUtils;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.mrcal.MrCalJNILoader;
import org.photonvision.raspi.LibCameraJNILoader;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.processes.VisionModule;
@@ -140,6 +141,7 @@ public class PhotonConfiguration {
LibCameraJNILoader.isSupported()
? "Zerocopy Libcamera Working"
: ""); // TODO add support for other types of GPU accel
generalSubmap.put("mrCalWorking", MrCalJNILoader.isWorking());
generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
generalSubmap.put("hardwarePlatform", Platform.getPlatformName());
settingsSubmap.put("general", generalSubmap);

50
photon-core/src/main/java/org/photonvision/common/hardware/Platform.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.common.hardware;
import com.jogamp.common.os.Platform.OSType;
import edu.wpi.first.util.RuntimeDetector;
import java.io.BufferedReader;
import java.io.IOException;
@@ -27,23 +28,32 @@ import org.photonvision.common.util.ShellExec;
@SuppressWarnings("unused")
public enum Platform {
// WPILib Supported (JNI)
WINDOWS_64("Windows x64", false, OSType.WINDOWS, true),
LINUX_32("Linux x86", false, OSType.LINUX, true),
LINUX_64("Linux x64", false, OSType.LINUX, true),
WINDOWS_64("Windows x64", "winx64", false, OSType.WINDOWS, true),
LINUX_32("Linux x86", "linuxx64", false, OSType.LINUX, true),
LINUX_64("Linux x64", "linuxx64", false, OSType.LINUX, true),
LINUX_RASPBIAN32(
"Linux Raspbian 32-bit", true, OSType.LINUX, true), // Raspberry Pi 3/4 with a 32-bit image
"Linux Raspbian 32-bit",
"linuxarm32",
true,
OSType.LINUX,
true), // Raspberry Pi 3/4 with a 32-bit image
LINUX_RASPBIAN64(
"Linux Raspbian 64-bit", true, OSType.LINUX, true), // Raspberry Pi 3/4 with a 64-bit image
LINUX_AARCH64("Linux AARCH64", false, OSType.LINUX, true), // Jetson Nano, Jetson TX2
"Linux Raspbian 64-bit",
"linuxarm64",
true,
OSType.LINUX,
true), // Raspberry Pi 3/4 with a 64-bit image
LINUX_AARCH64(
"Linux AARCH64", "linuxarm64", false, OSType.LINUX, true), // Jetson Nano, Jetson TX2
// PhotonVision Supported (Manual build/install)
LINUX_ARM32("Linux ARM32", false, OSType.LINUX, true), // ODROID XU4, C1+
LINUX_ARM64("Linux ARM64", false, OSType.LINUX, true), // ODROID C2, N2
LINUX_ARM32("Linux ARM32", "linuxarm32", false, OSType.LINUX, true), // ODROID XU4, C1+
LINUX_ARM64("Linux ARM64", "linuxarm64", false, OSType.LINUX, true), // ODROID C2, N2
// Completely unsupported
WINDOWS_32("Windows x86", false, OSType.WINDOWS, false),
MACOS("Mac OS", false, OSType.MACOS, false),
UNKNOWN("Unsupported Platform", false, OSType.UNKNOWN, false);
WINDOWS_32("Windows x86", "windowsx64", false, OSType.WINDOWS, false),
MACOS("Mac OS", "osxuniversal", false, OSType.MACOS, false),
UNKNOWN("Unsupported Platform", "", false, OSType.UNKNOWN, false);
private enum OSType {
WINDOWS,
@@ -54,6 +64,7 @@ public enum Platform {
private static final ShellExec shell = new ShellExec(true, false);
public final String description;
public final String nativeLibraryFolderName;
public final boolean isPi;
public final OSType osType;
public final boolean isSupported;
@@ -62,11 +73,17 @@ public enum Platform {
private static final Platform currentPlatform = getCurrentPlatform();
private static final boolean isRoot = checkForRoot();
Platform(String description, boolean isPi, OSType osType, boolean isSupported) {
Platform(
String description,
String nativeLibFolderName,
boolean isPi,
OSType osType,
boolean isSupported) {
this.description = description;
this.isPi = isPi;
this.osType = osType;
this.isSupported = isSupported;
this.nativeLibraryFolderName = nativeLibFolderName;
}
//////////////////////////////////////////////////////
@@ -89,6 +106,10 @@ public enum Platform {
}
}
public static String getNativeLibraryFolderName() {
return currentPlatform.nativeLibraryFolderName;
}
public static boolean isRoot() {
return isRoot;
}
@@ -214,4 +235,9 @@ public enum Platform {
return false;
}
}
public static boolean isWindows() {
var p = getCurrentPlatform();
return (p == WINDOWS_32 || p == WINDOWS_64);
}
}

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

@@ -39,7 +39,11 @@ import org.opencv.highgui.HighGui;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
public class TestUtils {
private static boolean has_loaded = false;
public static boolean loadLibraries() {
if (has_loaded) return true;
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
WPIMathJNI.Helper.setExtractOnStaticLoad(false);
@@ -61,11 +65,13 @@ public class TestUtils {
"cscorejni",
"apriltagjni");
return true;
has_loaded = true;
} catch (IOException e) {
e.printStackTrace();
return false;
has_loaded = false;
}
return has_loaded;
}
@SuppressWarnings("unused")

83
photon-core/src/main/java/org/photonvision/jni/PhotonJNICommon.java Normal file
View File

@@ -0,0 +1,83 @@
/*
* 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.jni;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.List;
import org.photonvision.common.hardware.Platform;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
public abstract class PhotonJNICommon {
static boolean libraryLoaded = false;
protected static Logger logger = null;
protected static synchronized void forceLoad(Class<?> clazz, List<String> libraries)
throws IOException {
if (libraryLoaded) return;
if (logger == null) logger = new Logger(clazz, LogGroup.Camera);
for (var libraryName : libraries) {
try {
// We always extract the shared object (we could hash each so, but that's a lot of work)
var arch_name = Platform.getNativeLibraryFolderName();
var nativeLibName = System.mapLibraryName(libraryName);
var in = clazz.getResourceAsStream("/nativelibraries/" + arch_name + "/" + nativeLibName);
if (in == null) {
libraryLoaded = false;
return;
}
// It's important that we don't mangle the names of these files on Windows at least
File temp = new File(System.getProperty("java.io.tmpdir"), nativeLibName);
FileOutputStream fos = new FileOutputStream(temp);
int read = -1;
byte[] buffer = new byte[1024];
while ((read = in.read(buffer)) != -1) {
fos.write(buffer, 0, read);
}
fos.close();
in.close();
System.load(temp.getAbsolutePath());
logger.info("Successfully loaded shared object " + temp.getName());
} catch (UnsatisfiedLinkError e) {
logger.error("Couldn't load shared object " + libraryName, e);
e.printStackTrace();
// logger.error(System.getProperty("java.library.path"));
break;
}
}
libraryLoaded = true;
}
protected static synchronized void forceLoad(Class<?> clazz, String libraryName)
throws IOException {
forceLoad(clazz, List.of(libraryName));
}
public static boolean isWorking() {
return libraryLoaded;
}
}

56
photon-core/src/main/java/org/photonvision/mrcal/MrCalJNILoader.java Normal file
View File

@@ -0,0 +1,56 @@
/*
* 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.mrcal;
import java.io.IOException;
import java.util.List;
import org.photonvision.common.hardware.Platform;
import org.photonvision.common.util.TestUtils;
import org.photonvision.jni.PhotonJNICommon;
public class MrCalJNILoader extends PhotonJNICommon {
public static synchronized void forceLoad() throws IOException {
// Force load opencv
TestUtils.loadLibraries();
// Library naming is dumb and has "lib" appended for Windows when it ought not to
if (Platform.isWindows()) {
// Order is correct to match dependencies of libraries
forceLoad(
MrCalJNILoader.class,
List.of(
"libamd",
"libcamd",
"libcolamd",
"libccolamd",
"openblas",
"libgcc_s_seh-1",
"libgfortran-5",
"liblapack",
"libcholmod",
"mrcal_jni"));
} else {
// Nothing else to do on linux
forceLoad(MrCalJNILoader.class, List.of("mrcal_jni"));
}
if (!MrCalJNILoader.isWorking()) {
throw new IOException("Unable to load mrcal JNI!");
}
}
}

4
photon-core/src/main/java/org/photonvision/raspi/LibCameraJNILoader.java
View File

@@ -23,6 +23,10 @@ import java.io.IOException;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
/**
* Helper for extracting photon-libcamera-gl-driver shared library files. TODO: Refactor to use
* PhotonJNICommon
*/
public class LibCameraJNILoader {
private static boolean libraryLoaded = false;
private static final Logger logger = new Logger(LibCameraJNILoader.class, LogGroup.Camera);

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

@@ -23,6 +23,7 @@ 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.Arrays;
import java.util.List;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
@@ -47,6 +48,12 @@ public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("calobjectWarp")
public final double[] calobjectWarp;
@JsonProperty("calobjectSize")
public final Size calobjectSize;
@JsonProperty("calobjectSpacing")
public final double calobjectSpacing;
@JsonIgnore private final double[] intrinsicsArr = new double[9];
@JsonIgnore private final double[] distCoeffsArr = new double[5];
@@ -64,6 +71,9 @@ public class CameraCalibrationCoefficients implements Releasable {
* @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
* @param calobjectSize Dimensions of the object used to calibrate, in # of squares in
* width/height
* @param calobjectSpacing Spacing between adjacent squares, in meters
*/
@JsonCreator
public CameraCalibrationCoefficients(
@@ -71,11 +81,15 @@ public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("cameraIntrinsics") JsonMatOfDouble cameraIntrinsics,
@JsonProperty("distCoeffs") JsonMatOfDouble distCoeffs,
@JsonProperty("calobjectWarp") double[] calobjectWarp,
@JsonProperty("observations") List<BoardObservation> observations) {
@JsonProperty("observations") List<BoardObservation> observations,
@JsonProperty("calobjectSize") Size calobjectSize,
@JsonProperty("calobjectSpacing") double calobjectSpacing) {
this.resolution = resolution;
this.cameraIntrinsics = cameraIntrinsics;
this.distCoeffs = distCoeffs;
this.calobjectWarp = calobjectWarp;
this.calobjectSize = calobjectSize;
this.calobjectSpacing = calobjectSpacing;
// Legacy migration just to make sure that observations is at worst empty and never null
if (observations == null) {
@@ -150,11 +164,35 @@ public class CameraCalibrationCoefficients implements Releasable {
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[0], List.of());
new Size(width, height),
cam_jsonmat,
distortion_jsonmat,
new double[0],
List.of(),
new Size(0, 0),
0);
}
@Override
public String toString() {
return "CameraCalibrationCoefficients [resolution="
+ resolution
+ ", cameraIntrinsics="
+ cameraIntrinsics
+ ", distCoeffs="
+ distCoeffs
+ ", observations="
+ observations
+ ", calobjectWarp="
+ Arrays.toString(calobjectWarp)
+ ", intrinsicsArr="
+ Arrays.toString(intrinsicsArr)
+ ", distCoeffsArr="
+ Arrays.toString(distCoeffsArr)
+ "]";
}
}

19
photon-core/src/main/java/org/photonvision/vision/calibration/JsonMatOfDouble.java
View File

@@ -127,4 +127,23 @@ public class JsonMatOfDouble implements Releasable {
packet.encode(this.data);
return packet;
}
@Override
public String toString() {
return "JsonMat [rows="
+ rows
+ ", cols="
+ cols
+ ", type="
+ type
+ ", data="
+ Arrays.toString(data)
+ ", wrappedMat="
+ wrappedMat
+ ", wpilibMat="
+ wpilibMat
+ ", wrappedMatOfDouble="
+ wrappedMatOfDouble
+ "]";
}
}

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

@@ -17,19 +17,22 @@
package org.photonvision.vision.pipe.impl;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfPoint2f;
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.mrcal.MrCalJNI;
import org.photonvision.mrcal.MrCalJNI.MrCalResult;
import org.photonvision.mrcal.MrCalJNILoader;
import org.photonvision.vision.calibration.BoardObservation;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.calibration.JsonImageMat;
@@ -41,19 +44,9 @@ public class Calibrate3dPipe
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
// matrix
private final Mat cameraMatrix = new Mat();
// Stores the radical and tangential distortion in a 5x1 matrix
private final MatOfDouble distortionCoefficients = new MatOfDouble();
// For logging
private static final Logger logger = new Logger(Calibrate3dPipe.class, LogGroup.General);
// Translational and rotational matrices
private final List<Mat> rvecs = new ArrayList<>();
private final List<Mat> tvecs = new ArrayList<>();
// The Standard deviation of the estimated parameters
private final Mat stdDeviationsIntrinsics = new Mat();
private final Mat stdDeviationsExtrinsics = new Mat();
@@ -62,9 +55,6 @@ public class Calibrate3dPipe
// finding the Euclidean distance between the actual corners.
private final Mat perViewErrors = new Mat();
// RMS of the calibration
private double calibrationAccuracy;
/**
* Runs the process for the pipe.
*
@@ -84,6 +74,35 @@ public class Calibrate3dPipe
&& it.size != null)
.collect(Collectors.toList());
CameraCalibrationCoefficients ret;
var start = System.nanoTime();
if (MrCalJNILoader.isWorking() && params.useMrCal) {
logger.debug("Calibrating with mrcal!");
ret = calibrateMrcal(in);
} else {
logger.debug("Calibrating with opencv!");
ret = calibrateOpenCV(in);
}
var dt = System.nanoTime() - start;
if (ret != null)
logger.info(
"CALIBRATION SUCCESS for res "
+ in.get(0).size
+ " in "
+ dt / 1e6
+ "ms! camMatrix: \n"
+ Arrays.toString(ret.cameraIntrinsics.data)
+ "\ndistortionCoeffs:\n"
+ Arrays.toString(ret.distCoeffs.data)
+ "\n");
else logger.info("Calibration failed! Review log for more details");
return ret;
}
protected CameraCalibrationCoefficients calibrateOpenCV(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
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()) {
@@ -91,6 +110,14 @@ public class Calibrate3dPipe
return null;
}
Mat cameraMatrix = new Mat();
MatOfDouble distortionCoefficients = new MatOfDouble();
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();
// RMS of the calibration
double calibrationAccuracy;
try {
// FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
// imageSize from, other parameters are output Mats
@@ -116,6 +143,116 @@ public class Calibrate3dPipe
JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix);
JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
var observations =
createObservations(in, cameraMatrix, distortionCoefficients, rvecs, tvecs, null);
cameraMatrix.release();
distortionCoefficients.release();
rvecs.forEach(Mat::release);
tvecs.forEach(Mat::release);
return new CameraCalibrationCoefficients(
in.get(0).size,
cameraMatrixMat,
distortionCoefficientsMat,
new double[0],
observations,
new Size(params.boardWidth, params.boardHeight),
params.squareSize);
}
protected CameraCalibrationCoefficients calibrateMrcal(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
List<MatOfPoint2f> corner_locations =
in.stream().map(it -> it.imagePoints).map(MatOfPoint2f::new).collect(Collectors.toList());
int imageWidth = (int) in.get(0).size.width;
int imageHeight = (int) in.get(0).size.height;
final double FOCAL_LENGTH_GUESS = 1200;
MrCalResult result =
MrCalJNI.calibrateCamera(
corner_locations,
params.boardWidth,
params.boardHeight,
params.squareSize,
imageWidth,
imageHeight,
FOCAL_LENGTH_GUESS);
// intrinsics are fx fy cx cy from mrcal
JsonMatOfDouble cameraMatrixMat =
new JsonMatOfDouble(
3,
3,
CvType.CV_64FC1,
new double[] {
// fx 0 cx
result.intrinsics[0],
0,
result.intrinsics[2],
// 0 fy cy
0,
result.intrinsics[1],
result.intrinsics[3],
// 0 0 1
0,
0,
1
});
JsonMatOfDouble distortionCoefficientsMat =
new JsonMatOfDouble(1, 8, CvType.CV_64FC1, Arrays.copyOfRange(result.intrinsics, 4, 12));
// Calculate optimized board poses manually. We get this for free from mrcal too, but that's not
// JNIed (yet)
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();
for (var o : in) {
var rvec = new Mat();
var tvec = new Mat();
Calib3d.solvePnP(
o.objectPoints,
o.imagePoints,
cameraMatrixMat.getAsMat(),
distortionCoefficientsMat.getAsMatOfDouble(),
rvec,
tvec);
rvecs.add(rvec);
tvecs.add(tvec);
}
List<BoardObservation> observations =
createObservations(
in,
cameraMatrixMat.getAsMat(),
distortionCoefficientsMat.getAsMatOfDouble(),
rvecs,
tvecs,
new double[] {result.warp_x, result.warp_y});
rvecs.forEach(Mat::release);
tvecs.forEach(Mat::release);
return new CameraCalibrationCoefficients(
in.get(0).size,
cameraMatrixMat,
distortionCoefficientsMat,
new double[] {result.warp_x, result.warp_y},
observations,
new Size(params.boardWidth, params.boardHeight),
params.squareSize);
}
private List<BoardObservation> createObservations(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in,
Mat cameraMatrix_,
MatOfDouble distortionCoefficients_,
List<Mat> rvecs,
List<Mat> tvecs,
double[] calobject_warp) {
List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
List<Mat> imgPts = in.stream().map(it -> it.imagePoints).collect(Collectors.toList());
// For each observation, calc reprojection error
Mat jac_temp = new Mat();
List<BoardObservation> observations = new ArrayList<>();
@@ -125,14 +262,36 @@ public class Calibrate3dPipe
var i_objPts = i_objPtsNative.toList();
var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList();
// Apply warp, if set
if (calobject_warp != null && calobject_warp.length == 2) {
// mrcal warp model!
// The chessboard spans [-1, 1] on the x and y axies. We then let z=k_x(1-x^2)+k_y(1-y^2)
double xmin = 0;
double ymin = 0;
double xmax = params.boardWidth * params.squareSize;
double ymax = params.boardHeight * params.squareSize;
double k_x = calobject_warp[0];
double k_y = calobject_warp[1];
// Convert to list, remap z, and back to cv::Mat
var list = i_objPtsNative.toArray();
for (var pt : list) {
double x_norm = MathUtils.map(pt.x, xmin, xmax, -1, 1);
double y_norm = MathUtils.map(pt.y, ymin, ymax, -1, 1);
pt.z = k_x * (1 - x_norm * x_norm) + k_y * (1 - y_norm * y_norm);
}
i_objPtsNative.fromArray(list);
}
var img_pts_reprojected = new MatOfPoint2f();
try {
Calib3d.projectPoints(
i_objPtsNative,
rvecs.get(i),
tvecs.get(i),
cameraMatrix,
distortionCoefficients,
cameraMatrix_,
distortionCoefficients_,
img_pts_reprojected,
jac_temp,
0.0);
@@ -164,33 +323,24 @@ public class Calibrate3dPipe
}
jac_temp.release();
// Standard deviation of results
try {
// Print calibration successful
logger.info(
"CALIBRATION SUCCESS for res "
+ params.resolution
+ " (with accuracy "
+ calibrationAccuracy
+ ")! camMatrix: \n"
+ new ObjectMapper().writeValueAsString(cameraMatrixMat)
+ "\ndistortionCoeffs:\n"
+ new ObjectMapper().writeValueAsString(distortionCoefficientsMat)
+ "\n");
} catch (JsonProcessingException e) {
logger.error("Failed to parse calibration data to json!", e);
}
return new CameraCalibrationCoefficients(
params.resolution, cameraMatrixMat, distortionCoefficientsMat, new double[0], observations);
return observations;
}
public static class CalibratePipeParams {
// Only needs resolution to pass onto CameraCalibrationCoefficients object.
private final Size resolution;
// Size (in # of corners) of the calibration object
public int boardHeight;
public int boardWidth;
// And size of each square
public double squareSize;
public CalibratePipeParams(Size resolution) {
// logger.info("res: " + resolution.toString());
this.resolution = resolution;
public boolean useMrCal;
public CalibratePipeParams(
int boardHeightSquares, int boardWidthSquares, double squareSize, boolean usemrcal) {
this.boardHeight = boardHeightSquares - 1;
this.boardWidth = boardWidthSquares - 1;
this.squareSize = squareSize;
this.useMrCal = usemrcal;
}
}
}

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

@@ -261,7 +261,7 @@ public class FindBoardCornersPipe
var outBoardCorners = new MatOfPoint2f();
boardCorners.copyTo(outBoardCorners);
var objPts = new MatOfPoint2f();
var objPts = new MatOfPoint3f();
objectPoints.copyTo(objPts);
// Get the size of the inFrame
@@ -329,14 +329,14 @@ public class FindBoardCornersPipe
public static class FindBoardCornersPipeResult implements Releasable {
public Size size;
public MatOfPoint2f objectPoints;
public MatOfPoint3f objectPoints;
public MatOfPoint2f imagePoints;
// Set later only if we need it
public Mat inputImage = null;
public FindBoardCornersPipeResult(
Size size, MatOfPoint2f objectPoints, MatOfPoint2f imagePoints) {
Size size, MatOfPoint3f objectPoints, MatOfPoint2f imagePoints) {
this.size = size;
this.objectPoints = objectPoints;
this.imagePoints = imagePoints;

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

@@ -24,7 +24,6 @@ import java.util.stream.Collectors;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.logging.LogGroup;
@@ -58,7 +57,7 @@ public class Calibrate3dPipeline
private boolean takeSnapshot = false;
// Output of the corners
final List<FindBoardCornersPipeResult> foundCornersList;
public final List<FindBoardCornersPipeResult> foundCornersList;
/// Output of the calibration, getter method is set for this.
private CVPipeResult<CameraCalibrationCoefficients> calibrationOutput;
@@ -93,7 +92,7 @@ public class Calibrate3dPipeline
Calibrate3dPipe.CalibratePipeParams calibratePipeParams =
new Calibrate3dPipe.CalibratePipeParams(
new Size(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight));
settings.boardHeight, settings.boardWidth, settings.gridSize, settings.useMrCal);
calibrate3dPipe.setParams(calibratePipeParams);
}
@@ -210,7 +209,8 @@ public class Calibrate3dPipeline
Units.metersToInches(settings.gridSize),
settings.boardWidth,
settings.boardHeight,
settings.boardType));
settings.boardType,
settings.useMrCal));
DataChangeService.getInstance()
.publishEvent(OutgoingUIEvent.wrappedOf("calibrationData", state));

1
photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java
View File

@@ -28,6 +28,7 @@ public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
public double gridSize = Units.inchesToMeters(1.0);
public Size resolution = new Size(640, 480);
public boolean useMrCal = true;
public Calibration3dPipelineSettings() {
super();

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

@@ -26,6 +26,7 @@ public class UICalibrationData {
public int patternWidth;
public int patternHeight;
public BoardType boardType;
public boolean useMrCal;
public UICalibrationData() {}
@@ -37,7 +38,8 @@ public class UICalibrationData {
double squareSizeIn,
int patternWidth,
int patternHeight,
BoardType boardType) {
BoardType boardType,
boolean useMrCal) {
this.count = count;
this.minCount = minCount;
this.videoModeIndex = videoModeIndex;
@@ -46,6 +48,7 @@ public class UICalibrationData {
this.patternWidth = patternWidth;
this.patternHeight = patternHeight;
this.boardType = boardType;
this.useMrCal = useMrCal;
}
public enum BoardType {

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

@@ -351,6 +351,7 @@ public class VisionModule {
settings.boardHeight = data.patternHeight;
settings.boardWidth = data.patternWidth;
settings.boardType = data.boardType;
settings.useMrCal = data.useMrCal;
settings.resolution = resolution;
// Disable gain if not applicable

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

@@ -22,17 +22,21 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.util.Units;
import java.io.File;
import java.io.IOException;
import java.nio.file.Path;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.junitpioneer.jupiter.cartesian.CartesianTest;
import org.junitpioneer.jupiter.cartesian.CartesianTest.Enum;
import org.junitpioneer.jupiter.cartesian.CartesianTest.Values;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.LogLevel;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TestUtils;
import org.photonvision.mrcal.MrCalJNILoader;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.frame.Frame;
@@ -40,226 +44,104 @@ import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.frame.FrameStaticProperties;
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
public static void init() {
public static void init() throws IOException {
TestUtils.loadLibraries();
MrCalJNILoader.forceLoad();
var logLevel = LogLevel.DEBUG;
Logger.setLevel(LogGroup.Camera, logLevel);
Logger.setLevel(LogGroup.WebServer, logLevel);
Logger.setLevel(LogGroup.VisionModule, logLevel);
Logger.setLevel(LogGroup.Data, logLevel);
Logger.setLevel(LogGroup.Config, logLevel);
Logger.setLevel(LogGroup.General, logLevel);
}
@Test
public void perViewErrorsTest() {
List<Mat> frames = new ArrayList<>();
enum CalibrationDatasets {
LIFECAM_480("lifecam/2024-01-02_lifecam_480", new Size(640, 480), new Size(11, 11)),
LIFECAM_1280("lifecam/2024-01-02_lifecam_1280", new Size(1280, 720), new Size(11, 11));
File dir = new File(TestUtils.getDotBoardImagesPath().toAbsolutePath().toString());
File[] directoryListing = dir.listFiles();
for (var file : directoryListing) {
frames.add(Imgcodecs.imread(file.getAbsolutePath()));
}
final String path;
final Size size;
final Size boardSize;
FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
findBoardCornersPipe.setParams(
new FindBoardCornersPipe.FindCornersPipeParams(
11, 4, UICalibrationData.BoardType.DOTBOARD, 15, FrameDivisor.NONE));
List<FindBoardCornersPipeResult> foundCornersList = new ArrayList<>();
for (var f : frames) {
var copy = new Mat();
f.copyTo(copy);
foundCornersList.add(findBoardCornersPipe.run(Pair.of(f, copy)).output);
}
Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();
calibrate3dPipe.setParams(new Calibrate3dPipe.CalibratePipeParams(new Size(640, 480)));
var calibrate3dPipeOutput = calibrate3dPipe.run(foundCornersList);
assertTrue(calibrate3dPipeOutput.output.observations.size() > 0);
for (var f : frames) {
f.release();
private CalibrationDatasets(String path, Size image, Size chessboard) {
this.path = path;
this.size = image;
this.boardSize = chessboard;
}
}
@Test
public void calibrationPipelineTest() {
int startMatCount = CVMat.getMatCount();
File dir = new File(TestUtils.getDotBoardImagesPath().toAbsolutePath().toString());
File[] directoryListing = dir.listFiles();
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "unique_name_lol");
calibration3dPipeline.getSettings().boardHeight = 11;
calibration3dPipeline.getSettings().boardWidth = 4;
calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.DOTBOARD;
calibration3dPipeline.getSettings().gridSize = 15;
calibration3dPipeline.getSettings().resolution = new Size(640, 480);
for (var file : directoryListing) {
calibration3dPipeline.takeSnapshot();
var frame =
new Frame(
new CVMat(Imgcodecs.imread(file.getAbsolutePath())),
new CVMat(),
FrameThresholdType.NONE,
new FrameStaticProperties(640, 480, 60, null));
var output = calibration3dPipeline.run(frame, QuirkyCamera.DefaultCamera);
// TestUtils.showImage(output.inputAndOutputFrame.processedImage.getMat());
output.release();
frame.release();
}
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
calibration3dPipeline.removeSnapshot(0);
var frame =
new Frame(
new CVMat(Imgcodecs.imread(directoryListing[0].getAbsolutePath())),
new CVMat(),
FrameThresholdType.NONE,
new FrameStaticProperties(640, 480, 60, null));
calibration3dPipeline.run(frame, QuirkyCamera.DefaultCamera).release();
frame.release();
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
var cal = calibration3dPipeline.tryCalibration();
calibration3dPipeline.finishCalibration();
assertNotNull(cal);
assertNotNull(cal.observations);
System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString());
System.out.println("Dist Coeffs: " + cal.distCoeffs.toString());
// Confirm we didn't get leaky on our mat usage
// assertTrue(CVMat.getMatCount() == startMatCount); // TODO Figure out why this doesn't work in
// CI
System.out.println("CVMats left: " + CVMat.getMatCount() + " Start: " + startMatCount);
}
@Test
public void calibrateSquares320x240_pi() {
/**
* Run camera calibration on a given dataset
*
* @param dataset Location of images and their size
* @param useMrCal If we should use mrcal or opencv for camera calibration
*/
@CartesianTest
public void calibrateTestMatrix(
@Enum(CalibrationDatasets.class) CalibrationDatasets dataset,
@Values(booleans = {true, false}) boolean useMrCal) {
// Pi3 and V1.3 camera
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "piCam", "320_240_1").toFile();
Size sz = new Size(320, 240);
calibrateSquaresCommon(sz, dir);
File dir = Path.of(base, dataset.path).toFile();
calibrateSquaresCommon(dataset.size, dir, dataset.boardSize, useMrCal);
}
@Test
public void calibrateSquares640x480_pi() {
// Pi3 and V1.3 camera
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "piCam", "640_480_1").toFile();
Size sz = new Size(640, 480);
calibrateSquaresCommon(sz, dir);
}
@Test
public void calibrateSquares960x720_pi() {
// Pi3 and V1.3 camera
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "piCam", "960_720_1").toFile();
Size sz = new Size(960, 720);
calibrateSquaresCommon(sz, dir);
}
@Test
public void calibrateSquares1920x1080_pi() {
// Pi3 and V1.3 camera
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "piCam", "1920_1080_1").toFile();
Size sz = new Size(1920, 1080);
calibrateSquaresCommon(sz, dir);
}
@Test
public void calibrateSquares320x240_gloworm() {
// Gloworm Beta
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "gloworm", "320_240_1").toFile();
Size sz = new Size(320, 240);
Size boardDim = new Size(9, 7);
calibrateSquaresCommon(sz, dir, boardDim);
}
@Test
public void calibrateSquares_960_720_gloworm() {
// Gloworm Beta
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "gloworm", "960_720_1").toFile();
Size sz = new Size(960, 720);
Size boardDim = new Size(9, 7);
calibrateSquaresCommon(sz, dir, boardDim);
}
@Test
public void calibrateSquares_1280_720_gloworm() {
// Gloworm Beta
// This image set will return a fairly offset Y-pixel for the optical center point
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "gloworm", "1280_720_1").toFile();
Size sz = new Size(1280, 720);
Size boardDim = new Size(9, 7);
calibrateSquaresCommon(sz, dir, boardDim, 640, 192);
}
@Test
public void calibrateSquares_1920_1080_gloworm() {
// Gloworm Beta
// This image set has most samples on the right, and is expected to return a slightly
// wonky calibration.
String base = TestUtils.getSquaresBoardImagesPath().toAbsolutePath().toString();
File dir = Path.of(base, "gloworm", "1920_1080_1").toFile();
Size sz = new Size(1920, 1080);
Size boardDim = new Size(9, 7);
calibrateSquaresCommon(sz, dir, boardDim, 1311, 540);
}
public void calibrateSquaresCommon(Size imgRes, File rootFolder) {
calibrateSquaresCommon(imgRes, rootFolder, new Size(8, 8));
}
public void calibrateSquaresCommon(Size imgRes, File rootFolder, Size boardDim) {
public static void calibrateSquaresCommon(
Size imgRes, File rootFolder, Size boardDim, boolean useMrCal) {
calibrateSquaresCommon(
imgRes, rootFolder, boardDim, Units.inchesToMeters(1), imgRes.width / 2, imgRes.height / 2);
imgRes,
rootFolder,
boardDim,
Units.inchesToMeters(1),
imgRes.width / 2,
imgRes.height / 2,
useMrCal);
}
public void calibrateSquaresCommon(
Size imgRes, File rootFolder, Size boardDim, double expectedXCenter, double expectedYCenter) {
public static void calibrateSquaresCommon(
Size imgRes,
File rootFolder,
Size boardDim,
double expectedXCenter,
double expectedYCenter,
boolean useMrCal) {
calibrateSquaresCommon(
imgRes, rootFolder, boardDim, Units.inchesToMeters(1), expectedXCenter, expectedYCenter);
imgRes,
rootFolder,
boardDim,
Units.inchesToMeters(1),
expectedXCenter,
expectedYCenter,
useMrCal);
}
public void calibrateSquaresCommon(
public static void calibrateSquaresCommon(
Size imgRes,
File rootFolder,
Size boardDim,
double boardGridSize_m,
double expectedXCenter,
double expectedYCenter) {
double expectedYCenter,
boolean useMrCal) {
int startMatCount = CVMat.getMatCount();
File[] directoryListing = rootFolder.listFiles();
assertTrue(directoryListing.length >= 25);
assertTrue(directoryListing.length >= 12);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "test_squares_common");
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(10, "test_squares_common");
calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.CHESSBOARD;
calibration3dPipeline.getSettings().resolution = imgRes;
calibration3dPipeline.getSettings().boardHeight = (int) Math.round(boardDim.height);
calibration3dPipeline.getSettings().boardWidth = (int) Math.round(boardDim.width);
calibration3dPipeline.getSettings().gridSize = boardGridSize_m;
calibration3dPipeline.getSettings().streamingFrameDivisor = FrameDivisor.NONE;
calibration3dPipeline.getSettings().useMrCal = useMrCal;
for (var file : directoryListing) {
if (file.isFile()) {