mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
[backend] Camera calibration (#10)
* Fleshed out pipeline * Fixed pipeline empty mat * Remove orig pipeline * Remove gradle changes * Added docs * [Server] Camera Calibration * [Server] Camera Calibration * [Server] Camera Calibration * [Server] Camera Calibration w/ PerViewErrors * [Server] Camera Calibration w/ PerViewErrors * [Server] Camera Calibration w/ PerViewErrors * [Server] Camera Calibration w/ PerViewErrors * [Server] Camera Calibration * [Server] Camera Calibration * [Server] Added logging to Camera Calibration
This commit is contained in:
Xzibit
@@ -161,6 +161,10 @@ public class TestUtils {
|
||||
return getPowercellPath().resolve(image.path);
|
||||
}
|
||||
|
||||
public static Path getDotBoardImagesPath() {
|
||||
return getResourcesFolderPath().resolve("calibrationBoardImages");
|
||||
}
|
||||
|
||||
public static void loadLibraries() {
|
||||
try {
|
||||
CameraServerCvJNI.forceLoad();
|
||||
|
||||
@@ -35,14 +35,19 @@ public class CameraCalibrationCoefficients implements Releasable {
|
||||
@JsonProperty("cameraExtrinsics")
|
||||
public final JsonMat cameraExtrinsics;
|
||||
|
||||
@JsonProperty("perViewErrors")
|
||||
public final double[] perViewErrors;
|
||||
|
||||
@JsonCreator
|
||||
public CameraCalibrationCoefficients(
|
||||
@JsonProperty("resolution") Size resolution,
|
||||
@JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics,
|
||||
@JsonProperty("cameraExtrinsics") JsonMat cameraExtrinsics) {
|
||||
@JsonProperty("cameraExtrinsics") JsonMat cameraExtrinsics,
|
||||
@JsonProperty("perViewErrors") double[] perViewErrors) {
|
||||
this.resolution = resolution;
|
||||
this.cameraIntrinsics = cameraIntrinsics;
|
||||
this.cameraExtrinsics = cameraExtrinsics;
|
||||
this.perViewErrors = perViewErrors;
|
||||
}
|
||||
|
||||
@JsonIgnore
|
||||
@@ -55,6 +60,11 @@ public class CameraCalibrationCoefficients implements Releasable {
|
||||
return cameraExtrinsics.getAsMatOfDouble();
|
||||
}
|
||||
|
||||
@JsonIgnore
|
||||
public double[] getPerViewErrors() {
|
||||
return perViewErrors;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void release() {
|
||||
cameraIntrinsics.release();
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
/*
|
||||
* Copyright (C) 2020 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 com.fasterxml.jackson.core.JsonProcessingException;
|
||||
import com.fasterxml.jackson.databind.ObjectMapper;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
import org.opencv.calib3d.Calib3d;
|
||||
import org.opencv.core.*;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.vision.calibration.JsonMat;
|
||||
import org.photonvision.vision.pipe.CVPipe;
|
||||
|
||||
public class Calibrate3dPipe
|
||||
extends CVPipe<
|
||||
List<List<Mat>>, 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 Mat cameraMatrix = new Mat();
|
||||
// Stores the radical and tangential distortion in a 5x1 matrix
|
||||
private MatOfDouble distortionCoefficients = new MatOfDouble();
|
||||
|
||||
// For loggging
|
||||
private static final Logger logger = new Logger(Calibrate3dPipe.class, LogGroup.General);
|
||||
|
||||
// Translational and rotational matrices
|
||||
private List<Mat> rvecs = new ArrayList<>();
|
||||
private List<Mat> tvecs = new ArrayList<>();
|
||||
|
||||
// The Standard deviation of the estimated parameters
|
||||
private Mat stdDeviationsIntrinsics = new Mat();
|
||||
private Mat stdDeviationsExtrinsics = new Mat();
|
||||
|
||||
// Contains the re projection error of each snapshot by re projecting the corners we found and
|
||||
// finding the euclidean distance between the actual corners.
|
||||
private Mat perViewErrors = new Mat();
|
||||
|
||||
// RMS of the calibration
|
||||
private double calibrationAccuracy;
|
||||
|
||||
/**
|
||||
* Runs the process for the pipe.
|
||||
*
|
||||
* @param in Input for pipe processing.
|
||||
* @return Result of processing.
|
||||
*/
|
||||
@Override
|
||||
protected CameraCalibrationCoefficients process(List<List<Mat>> in) {
|
||||
try {
|
||||
// FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
|
||||
// imageSize from, other parameters are output Mats
|
||||
calibrationAccuracy =
|
||||
Calib3d.calibrateCameraExtended(
|
||||
in.get(1),
|
||||
in.get(2),
|
||||
new Size(in.get(0).get(0).width(), in.get(0).get(0).height()),
|
||||
cameraMatrix,
|
||||
distortionCoefficients,
|
||||
rvecs,
|
||||
tvecs,
|
||||
stdDeviationsIntrinsics,
|
||||
stdDeviationsExtrinsics,
|
||||
perViewErrors);
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix);
|
||||
JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients);
|
||||
try {
|
||||
// Print calibration successful
|
||||
logger.info(
|
||||
"CALIBRATION SUCCESS (with accuracy "
|
||||
+ calibrationAccuracy
|
||||
+ ")! camMatrix: \n"
|
||||
+ new ObjectMapper().writeValueAsString(cameraMatrixMat)
|
||||
+ "\ndistortionCoeffs:\n"
|
||||
+ new ObjectMapper().writeValueAsString(distortionCoefficientsMat)
|
||||
+ "\n");
|
||||
} catch (JsonProcessingException e) {
|
||||
logger.error(Arrays.toString(e.getStackTrace()));
|
||||
}
|
||||
// Create a new CameraCalibrationCoefficients object to pass onto SolvePnP
|
||||
double[] perViewErrorsArray =
|
||||
new double[(int) perViewErrors.total() * perViewErrors.channels()];
|
||||
perViewErrors.get(0, 0, perViewErrorsArray);
|
||||
return new CameraCalibrationCoefficients(
|
||||
params.resolution, cameraMatrixMat, distortionCoefficientsMat, perViewErrorsArray);
|
||||
}
|
||||
|
||||
public static class CalibratePipeParams {
|
||||
// Only needs resolution to pass onto CameraCalibrationCoefficients object.
|
||||
private final Size resolution;
|
||||
|
||||
public CalibratePipeParams(Size resolution) {
|
||||
this.resolution = resolution;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,163 @@
|
||||
/*
|
||||
* Copyright (C) 2020 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.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.apache.commons.lang3.tuple.Pair;
|
||||
import org.opencv.calib3d.Calib3d;
|
||||
import org.opencv.core.*;
|
||||
import org.opencv.imgproc.Imgproc;
|
||||
import org.photonvision.vision.pipe.CVPipe;
|
||||
|
||||
public class FindBoardCornersPipe
|
||||
extends CVPipe<List<Mat>, List<List<Mat>>, FindBoardCornersPipe.FindCornersPipeParams> {
|
||||
MatOfPoint3f objectPoints = new MatOfPoint3f();
|
||||
private List<Mat> listOfObjectPoints = new ArrayList<>();
|
||||
private List<Mat> listOfImagePoints = new ArrayList<>();
|
||||
|
||||
Size imageSize;
|
||||
Size patternSize;
|
||||
|
||||
private MatOfPoint2f boardCorners = new MatOfPoint2f();
|
||||
|
||||
// SubCornerPix params
|
||||
private final Size windowSize = new Size(11, 11);
|
||||
private final Size zeroZone = new Size(-1, -1);
|
||||
private final TermCriteria criteria = new TermCriteria(3, 30, 0.001);
|
||||
|
||||
private boolean objectPointsCreated = false;
|
||||
|
||||
public void createObjectPoints() {
|
||||
if (objectPointsCreated) return;
|
||||
|
||||
/*If using a chessboard, then the pattern size if the inner corners of the board. For example, the pattern size of a 9x9 chessboard would be 8x8
|
||||
If using a dot board, then the pattern size width is the sum of the bottom 2 rows and the height is the left or right most column
|
||||
For example, a 5x4 dot board would have a pattern size of 11x4
|
||||
* */
|
||||
this.patternSize = new Size(params.boardWidth, params.boardHeight);
|
||||
|
||||
// Chessboard and dot board have different 3D points to project as a dot board has alternating
|
||||
// dots per column
|
||||
if (params.isUsingChessboard) {
|
||||
// Here we can create an NxN grid since a chessboard is rectangular
|
||||
for (int i = 0; i < patternSize.height * patternSize.width; i++) {
|
||||
objectPoints.push_back(
|
||||
new MatOfPoint3f(
|
||||
new Point3((double) i / patternSize.width, i % patternSize.width, 0.0f)));
|
||||
}
|
||||
} else {
|
||||
// Here we need to alternate the amount of dots per column since a dot board is not
|
||||
// rectangular and also by taking in account the grid size which should be in mm
|
||||
for (int i = 0; i < patternSize.height; i++) {
|
||||
for (int j = 0; j < patternSize.width; j++) {
|
||||
objectPoints.push_back(
|
||||
new MatOfPoint3f(
|
||||
new Point3((2 * j + i % 2) * params.gridSize, i * params.gridSize, 0.0d)));
|
||||
}
|
||||
}
|
||||
}
|
||||
objectPointsCreated = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Runs the process for the pipe.
|
||||
*
|
||||
* @param in Input for pipe processing.
|
||||
* @return All valid Mats for camera calibration
|
||||
*/
|
||||
@Override
|
||||
protected List<List<Mat>> process(List<Mat> in) {
|
||||
// If we have less than 20 snapshots we need to return null
|
||||
if (in.size() < 20) return null;
|
||||
// Contains all valid Mats where a chessboard or dot board have been found
|
||||
List<Mat> outputMats = new ArrayList<>();
|
||||
|
||||
// Create the object points
|
||||
createObjectPoints();
|
||||
|
||||
for (Mat board : in) {
|
||||
if (findBoardCorners(board).getLeft()) {
|
||||
outputMats.add(board);
|
||||
}
|
||||
}
|
||||
// Contains the list of valid Mats, object points and images points where objectPoints.size() =
|
||||
// imagePoints.size()
|
||||
return List.of(outputMats, listOfObjectPoints, listOfImagePoints);
|
||||
}
|
||||
|
||||
public Pair<Boolean, Mat> findBoardCorners(Mat frame) {
|
||||
createObjectPoints();
|
||||
|
||||
// Convert the frame to grayscale to increase contrast
|
||||
Imgproc.cvtColor(frame, frame, Imgproc.COLOR_BGR2GRAY);
|
||||
boolean boardFound;
|
||||
|
||||
if (params.isUsingChessboard) {
|
||||
// This is for chessboards
|
||||
boardFound = Calib3d.findChessboardCorners(frame, patternSize, boardCorners);
|
||||
} else {
|
||||
// For dot boards
|
||||
boardFound =
|
||||
Calib3d.findCirclesGrid(
|
||||
frame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
|
||||
}
|
||||
|
||||
if (!boardFound) {
|
||||
// If we can't find a chessboard/dot board, convert the frame back to BGR and return false.
|
||||
Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
|
||||
|
||||
return Pair.of(false, null);
|
||||
}
|
||||
// Get the size of the frame
|
||||
this.imageSize = new Size(frame.width(), frame.height());
|
||||
|
||||
// Add the 3D points and the points of the corners found
|
||||
this.listOfObjectPoints.add(objectPoints);
|
||||
this.listOfImagePoints.add(boardCorners);
|
||||
|
||||
// Do sub corner pix for drawing chessboard
|
||||
Imgproc.cornerSubPix(frame, boardCorners, windowSize, zeroZone, criteria);
|
||||
|
||||
// convert back to BGR
|
||||
Imgproc.cvtColor(frame, frame, 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
|
||||
Mat chessboardDrawn = new Mat();
|
||||
frame.copyTo(chessboardDrawn);
|
||||
Calib3d.drawChessboardCorners(chessboardDrawn, patternSize, boardCorners, true);
|
||||
boardCorners = new MatOfPoint2f();
|
||||
return Pair.of(true, chessboardDrawn);
|
||||
}
|
||||
|
||||
public static class FindCornersPipeParams {
|
||||
|
||||
private final int boardHeight;
|
||||
private final int boardWidth;
|
||||
private final boolean isUsingChessboard;
|
||||
private final double gridSize;
|
||||
|
||||
public FindCornersPipeParams(
|
||||
int boardHeight, int boardWidth, boolean isUsingChessboard, double gridSize) {
|
||||
this.boardHeight = boardHeight;
|
||||
this.boardWidth = boardWidth;
|
||||
this.isUsingChessboard = isUsingChessboard;
|
||||
this.gridSize = gridSize; // mm
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -17,26 +17,124 @@
|
||||
|
||||
package org.photonvision.vision.pipeline;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.opencv.core.Mat;
|
||||
import org.photonvision.common.util.math.MathUtils;
|
||||
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.vision.frame.Frame;
|
||||
import org.photonvision.vision.frame.FrameStaticProperties;
|
||||
import org.photonvision.vision.opencv.CVMat;
|
||||
import org.photonvision.vision.pipe.CVPipeResult;
|
||||
import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
|
||||
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
|
||||
import org.photonvision.vision.pipeline.result.CVPipelineResult;
|
||||
import org.photonvision.vision.processes.PipelineManager;
|
||||
|
||||
public class Calibration3dPipeline extends CVPipeline<CVPipelineResult, CVPipelineSettings> {
|
||||
public class Calibration3dPipeline
|
||||
extends CVPipeline<CVPipelineResult, Calibration3dPipelineSettings> {
|
||||
|
||||
// TODO: Everything here
|
||||
// Only 2 pipes needed, one for finding the board corners and one for actually calibrating
|
||||
private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe();
|
||||
private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();
|
||||
|
||||
// Getter methods have been set for calibrate and takeSnapshot
|
||||
private int numSnapshots = 0;
|
||||
private boolean calibrate = false;
|
||||
private boolean takeSnapshot = false;
|
||||
|
||||
// BoardSnapshots is a list of all valid snapshots taken
|
||||
private ArrayList<Mat> boardSnapshots;
|
||||
|
||||
// Output of the corners
|
||||
private CVPipeResult<List<List<Mat>>> findCornersPipeOutput;
|
||||
|
||||
/// Output of the calibration, getter method is set for this.
|
||||
private CVPipeResult<CameraCalibrationCoefficients> calibrationOutput;
|
||||
|
||||
public Calibration3dPipeline() {
|
||||
settings = new CVPipelineSettings();
|
||||
settings.pipelineIndex = PipelineManager.CAL_3D_INDEX;
|
||||
this.settings = new Calibration3dPipelineSettings();
|
||||
this.boardSnapshots = new ArrayList<>();
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void setPipeParams(
|
||||
FrameStaticProperties frameStaticProperties, CVPipelineSettings settings) {}
|
||||
FrameStaticProperties frameStaticProperties, Calibration3dPipelineSettings settings) {
|
||||
FindBoardCornersPipe.FindCornersPipeParams findCornersPipeParams =
|
||||
new FindBoardCornersPipe.FindCornersPipeParams(
|
||||
settings.boardHeight,
|
||||
settings.boardWidth,
|
||||
settings.isUsingChessboard,
|
||||
settings.gridSize);
|
||||
findBoardCornersPipe.setParams(findCornersPipeParams);
|
||||
|
||||
Calibrate3dPipe.CalibratePipeParams calibratePipeParams =
|
||||
new Calibrate3dPipe.CalibratePipeParams(settings.resolution);
|
||||
calibrate3dPipe.setParams(calibratePipeParams);
|
||||
}
|
||||
|
||||
@Override
|
||||
protected CVPipelineResult process(Frame frame, CVPipelineSettings settings) {
|
||||
return null;
|
||||
protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings settings) {
|
||||
// Set the pipe parameters
|
||||
setPipeParams(frame.frameStaticProperties, settings);
|
||||
|
||||
long sumPipeNanosElapsed = 0L;
|
||||
|
||||
// hasEnough() is a getter method for numSnapshots that checks if there are more than 25
|
||||
// snapshots
|
||||
// calibrate will be true when it is get by it's putter method
|
||||
if (hasEnough() && calibrate) {
|
||||
|
||||
/*Pass the board corners to the pipe, which will check again to see if all boards are valid
|
||||
and returns the corresponding image and object points*/
|
||||
findCornersPipeOutput = findBoardCornersPipe.apply(boardSnapshots);
|
||||
// Increment the time it took to process all board pics to total elapsed time
|
||||
sumPipeNanosElapsed += findCornersPipeOutput.nanosElapsed;
|
||||
|
||||
calibrationOutput = calibrate3dPipe.apply(findCornersPipeOutput.result);
|
||||
sumPipeNanosElapsed += calibrationOutput.nanosElapsed;
|
||||
|
||||
calibrate = false;
|
||||
numSnapshots = 0;
|
||||
boardSnapshots.clear();
|
||||
|
||||
} else if (takeSnapshot) {
|
||||
var hasBoard = findBoardCornersPipe.findBoardCorners(frame.image.getMat());
|
||||
if (hasBoard.getLeft()) {
|
||||
Mat board = new Mat();
|
||||
frame.image.getMat().copyTo(board);
|
||||
// See if mat is empty
|
||||
boardSnapshots.add(board);
|
||||
|
||||
// Set snapshot to false and increment number of snapshots taken
|
||||
takeSnapshot = false;
|
||||
numSnapshots++;
|
||||
return new CVPipelineResult(
|
||||
MathUtils.nanosToMillis(sumPipeNanosElapsed),
|
||||
null,
|
||||
new Frame(new CVMat(hasBoard.getRight()), frame.frameStaticProperties));
|
||||
}
|
||||
}
|
||||
|
||||
return new CVPipelineResult(MathUtils.nanosToMillis(sumPipeNanosElapsed), null, frame);
|
||||
}
|
||||
|
||||
public boolean hasEnough() {
|
||||
return numSnapshots >= 25;
|
||||
}
|
||||
|
||||
public void startCalibration() {
|
||||
calibrate = true;
|
||||
}
|
||||
|
||||
public void takeSnapshot() {
|
||||
takeSnapshot = true;
|
||||
}
|
||||
|
||||
public double[] perViewErrors() {
|
||||
return calibrationOutput.result.perViewErrors;
|
||||
}
|
||||
|
||||
public CameraCalibrationCoefficients cameraCalibrationCoefficients() {
|
||||
return calibrationOutput.result;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
* Copyright (C) 2020 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;
|
||||
|
||||
import org.opencv.core.Size;
|
||||
|
||||
public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
|
||||
public int boardHeight = 0;
|
||||
public int boardWidth = 0;
|
||||
public boolean isUsingChessboard = true;
|
||||
public double gridSize = 0;
|
||||
|
||||
public Size resolution = new Size(640, 480);
|
||||
}
|
||||
Reference in New Issue
Block a user