Skip to content

Allow partial observations and remove dependence on OpenCV Mat types #28

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
thatcomputerguy0101 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Allow partial observations and remove dependence on OpenCV Mat types #28

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
121 changes: 85 additions & 36 deletions src/main/java/org/photonvision/mrcal/MrCalJNI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Iterator;
import java.util.List;
import org.opencv.core.MatOfFloat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Translation3d;
Expand All @@ -35,6 +35,25 @@ public static enum CameraLensModel {
LENSMODEL_SPLINED_STEREOGRAPHIC;
}

/**
* A single board observation for mrcal calibration
*
* @param corners The positions of each of the detected corners
* @param levels The decimation level each of the corners was observed at
* @param ids The id of each corner, if the observation was a partial observation; can be null
*/
public static record MrCalObservation(Point[] corners, float[] levels, int[] ids) {
/**
* Creates a complete board observation for mrcal calibration
*
* @param corners The positions of each of the detected corners
* @param levels The decimation level each of the corners was observed at
*/
public MrCalObservation(Point[] corners, float[] levels) {
this(corners, levels, null);
}
}

public static class MrCalResult {
public boolean success;
public double[] intrinsics;
Expand Down Expand Up @@ -250,43 +269,78 @@ public static native double[] compute_uncertainty(
double warpY);

/**
* Convert a list of board-pixel-corners and detection levels for each snapshot of a chessboard
* boardWidth x boardHeight to a packed double[] suitable to pass to
* MrCalJni::mrcal_calibrate_camera. Levels will be converted to weights using weight = 0.5^level,
* as explained
* Convert an iterator of board-pixel-corners, detection decimation levels, and corner IDs for
* each snapshot of a chessboard boardWidth x boardHeight to a packed double[] suitable to pass to
* MrCalJni::mrcal_calibrate_camera. Ids will be used to select which corners are actually
* present. Levels will be converted to weights using weight = 0.5^level, as explained
* [here](https://github.com/dkogan/mrcal/blob/7cd9ac4c854a4b244a35f554c9ebd0464d59e9ff/mrcal-calibrate-cameras#L152)
*/
private static double[] makeObservations(
List<MatOfPoint2f> board_corners,
List<MatOfFloat> board_corner_levels,
int observationCount,
Iterator<MrCalObservation> observationData,
int boardWidth,
int boardHeight) {
double[] observations = new double[boardWidth * boardHeight * 3 * board_corners.size()];
double[] observations = new double[boardWidth * boardHeight * 3 * observationCount];
Arrays.fill(observations, -1.0);

int i = 0;
for (int b = 0; b < board_corners.size(); b++) {
var board = board_corners.get(b);
var levels = board_corner_levels.get(b).toArray();
var corners = board.toArray();

if (!(corners.length == levels.length && corners.length == boardWidth * boardHeight)) {
for (int b = 0; b < observationCount; b++) {
if (!observationData.hasNext()) {
// Too few observations
return null;
}

// Assume that we're correct in terms of row/column major-ness (lol)
for (int c = 0; c < corners.length; c++) {
var corner = corners[c];
float level = levels[c];
final var observation = observationData.next();

final var corners = observation.corners;
final var levels = observation.levels;
final var ids = observation.ids;

observations[i * 3 + 0] = corner.x;
observations[i * 3 + 1] = corner.y;
observations[i * 3 + 2] = level;
if (ids == null) {
// No ids, assume a full rectangular board
if (!(corners.length == levels.length && corners.length <= boardWidth * boardHeight)) {
return null;
}

i += 1;
// Assume that we're correct in terms of row/column major-ness (lol)
for (int c = 0; c < corners.length; c++) {
var corner = corners[c];
float level = levels[c];

int i = boardWidth * boardHeight * b + c;

observations[i * 3 + 0] = corner.x;
observations[i * 3 + 1] = corner.y;
observations[i * 3 + 2] = level;
}
} else {
// Ids present, some corners may be missing
if (!(corners.length == levels.length
&& corners.length == ids.length
&& corners.length <= boardWidth * boardHeight)) {
return null;
}

// Assume that we're correct in terms of row/column major-ness (lol)
for (int c = 0; c < corners.length; c++) {
var corner = corners[c];
float level = levels[c];
int id = ids[c];

if (id < 0 || id >= boardWidth * boardHeight) {
return null;
}

int i = boardWidth * boardHeight * b + id;

observations[i * 3 + 0] = corner.x;
observations[i * 3 + 1] = corner.y;
observations[i * 3 + 2] = level;
}
}
}

if (i * 3 != observations.length) {
if (observationData.hasNext()) {
// Too many observations
return null;
}

Expand All @@ -300,9 +354,9 @@ private static double[] makeObservations(
* then calls {@link #mrcal_calibrate_camera} to perform calibration. Each corner's detection
* level is converted to a weight (0.5^level), and negative levels indicate undetected corners.
*
* @param board_corners List of detected chessboard corners for each frame
* @param board_corner_levels List of detection levels for each corner in each frame. Point weight
* is calculated as weight = 0.5^level. Negative weight will ignore this observation
* @param observationCount Number of observations in `observationData`
* @param observationData An iterator of observations, each containing a list of corner locations,
* decimation levels, and optional corner ids
* @param boardWidth Number of internal corners horizontally
* @param boardHeight Number of internal corners vertically
* @param boardSpacing Physical spacing between corners (meters)
Expand All @@ -312,21 +366,16 @@ private static double[] makeObservations(
* @return Calibration result with optimized intrinsics, poses, and error metrics
*/
public static MrCalResult calibrateCamera(
List<MatOfPoint2f> board_corners,
List<MatOfFloat> board_corner_levels,
int observationCount,
Iterator<MrCalObservation> observationData,
int boardWidth,
int boardHeight,
double boardSpacing,
int imageWidth,
int imageHeight,
double focalLen) {

if (!(board_corners.size() == board_corner_levels.size())) {
return new MrCalResult(false);
}

var observations =
makeObservations(board_corners, board_corner_levels, boardWidth, boardHeight);
var observations = makeObservations(observationCount, observationData, boardWidth, boardHeight);

return mrcal_calibrate_camera(
observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight, focalLen);
Expand Down