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

Calibration Rotation! (#1464)

Browse Source 
Rotate camera calibration coefficients based on camera rotation. Probably. Seems to work. Maybe.

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Cameron (3539)
2024-10-19 01:23:23 -04:00
committed by GitHub
parent 388b3fa2ef
commit b38de6b506
22 changed files with 614 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
View File

@@ -158,12 +158,23 @@ public class CameraConfiguration {
pipelineSettings = settings;
}
/**
* Replace a calibration in our list with the same unrotatedImageSize with a new one, or add it if
* none exists yet. If we are replacing an existing calibration, the old one will be "released"
* and the underlying data matrices will become invalid.
*
* @param calibration The calibration to add.
*/
public void addCalibration(CameraCalibrationCoefficients calibration) {
logger.info("adding calibration " + calibration.resolution);
logger.info("adding calibration " + calibration.unrotatedImageSize);
calibrations.stream()
.filter(it -> it.resolution.equals(calibration.resolution))
.filter(it -> it.unrotatedImageSize.equals(calibration.unrotatedImageSize))
.findAny()
.ifPresent(calibrations::remove);
.ifPresent(
(it) -> {
it.release();
calibrations.remove(it);
});
calibrations.add(calibration);
}

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

@@ -27,12 +27,13 @@ import java.util.List;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
import org.photonvision.vision.opencv.ImageRotationMode;
import org.photonvision.vision.opencv.Releasable;
@JsonIgnoreProperties(ignoreUnknown = true)
public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("resolution")
public final Size resolution;
public final Size unrotatedImageSize;
@JsonProperty("cameraIntrinsics")
public final JsonMatOfDouble cameraIntrinsics;
@@ -56,9 +57,6 @@ public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("lensmodel")
public final CameraLensModel lensmodel;
@JsonIgnore private final double[] intrinsicsArr = new double[9];
@JsonIgnore private final double[] distCoeffsArr = 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
@@ -87,7 +85,7 @@ public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("calobjectSize") Size calobjectSize,
@JsonProperty("calobjectSpacing") double calobjectSpacing,
@JsonProperty("lensmodel") CameraLensModel lensmodel) {
this.resolution = resolution;
this.unrotatedImageSize = resolution;
this.cameraIntrinsics = cameraIntrinsics;
this.distCoeffs = distCoeffs;
this.calobjectWarp = calobjectWarp;
@@ -100,10 +98,93 @@ public class CameraCalibrationCoefficients implements Releasable {
observations = List.of();
}
this.observations = observations;
}
// do this once so gets are quick
getCameraIntrinsicsMat().get(0, 0, intrinsicsArr);
getDistCoeffsMat().get(0, 0, distCoeffsArr);
public CameraCalibrationCoefficients rotateCoefficients(ImageRotationMode rotation) {
if (rotation == ImageRotationMode.DEG_0) {
return this;
}
Mat rotatedIntrinsics = getCameraIntrinsicsMat().clone();
Mat rotatedDistCoeffs = getDistCoeffsMat().clone();
double cx = getCameraIntrinsicsMat().get(0, 2)[0];
double cy = getCameraIntrinsicsMat().get(1, 2)[0];
double fx = getCameraIntrinsicsMat().get(0, 0)[0];
double fy = getCameraIntrinsicsMat().get(1, 1)[0];
// only adjust p1 and p2 the rest are radial distortion coefficients
double p1 = getDistCoeffsMat().get(0, 2)[0];
double p2 = getDistCoeffsMat().get(0, 3)[0];
// A bunch of horrifying opaque rotation black magic. See image-rotation.md for more details.
switch (rotation) {
case DEG_0:
break;
case DEG_270_CCW:
// FX
rotatedIntrinsics.put(0, 0, fy);
// FY
rotatedIntrinsics.put(1, 1, fx);
// CX
rotatedIntrinsics.put(0, 2, unrotatedImageSize.height - cy);
// CY
rotatedIntrinsics.put(1, 2, cx);
// P1
rotatedDistCoeffs.put(0, 2, p2);
// P2
rotatedDistCoeffs.put(0, 3, -p1);
break;
case DEG_180_CCW:
// CX
rotatedIntrinsics.put(0, 2, unrotatedImageSize.width - cx);
// CY
rotatedIntrinsics.put(1, 2, unrotatedImageSize.height - cy);
// P1
rotatedDistCoeffs.put(0, 2, -p1);
// P2
rotatedDistCoeffs.put(0, 3, -p2);
break;
case DEG_90_CCW:
// FX
rotatedIntrinsics.put(0, 0, fy);
// FY
rotatedIntrinsics.put(1, 1, fx);
// CX
rotatedIntrinsics.put(0, 2, cy);
// CY
rotatedIntrinsics.put(1, 2, unrotatedImageSize.width - cx);
// P1
rotatedDistCoeffs.put(0, 2, -p2);
// P2
rotatedDistCoeffs.put(0, 3, p1);
break;
}
JsonMatOfDouble newIntrinsics = JsonMatOfDouble.fromMat(rotatedIntrinsics);
JsonMatOfDouble newDistCoeffs = JsonMatOfDouble.fromMat(rotatedDistCoeffs);
rotatedIntrinsics.release();
rotatedDistCoeffs.release();
var rotatedImageSize = new Size(unrotatedImageSize.height, unrotatedImageSize.width);
return new CameraCalibrationCoefficients(
rotatedImageSize,
newIntrinsics,
newDistCoeffs,
calobjectWarp,
observations,
calobjectSize,
calobjectSpacing,
lensmodel);
}
@JsonIgnore
@@ -118,12 +199,12 @@ public class CameraCalibrationCoefficients implements Releasable {
@JsonIgnore
public double[] getIntrinsicsArr() {
return intrinsicsArr;
return cameraIntrinsics.data;
}
@JsonIgnore
public double[] getDistCoeffsArr() {
return distCoeffsArr;
return distCoeffs.data;
}
@JsonIgnore
@@ -140,7 +221,7 @@ public class CameraCalibrationCoefficients implements Releasable {
@Override
public String toString() {
return "CameraCalibrationCoefficients [resolution="
+ resolution
+ unrotatedImageSize
+ ", cameraIntrinsics="
+ cameraIntrinsics
+ ", distCoeffs="
@@ -149,16 +230,12 @@ public class CameraCalibrationCoefficients implements Releasable {
+ observations.size()
+ ", calobjectWarp="
+ Arrays.toString(calobjectWarp)
+ ", intrinsicsArr="
+ Arrays.toString(intrinsicsArr)
+ ", distCoeffsArr="
+ Arrays.toString(distCoeffsArr)
+ "]";
}
public UICameraCalibrationCoefficients cloneWithoutObservations() {
return new UICameraCalibrationCoefficients(
resolution,
unrotatedImageSize,
cameraIntrinsics,
distCoeffs,
calobjectWarp,

2
photon-core/src/main/java/org/photonvision/vision/calibration/CameraLensModel.java
View File

@@ -28,7 +28,7 @@ public enum CameraLensModel {
/** Mrcal steriographic lens model. See LENSMODEL_STEREOGRAPHIC in the mrcal docs */
LENSMODEL_STERIOGRAPHIC,
/**
* Mrcal splined-steriographic lens model. See LENSMODEL_SPLINED_STEREOGRAPHIC_ in the mrcal docs
* Mrcal splined-steriographic lens model. See LENSMODEL_SPLINED_STEREOGRAPHIC in the mrcal docs
*/
LENSMODEL_SPLINED_STERIOGRAPHIC
}

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

@@ -26,7 +26,6 @@ import org.ejml.simple.SimpleMatrix;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.opencv.Releasable;
/** JSON-serializable image. Data is stored as a raw JSON array. */
@@ -41,6 +40,7 @@ public class JsonMatOfDouble implements Releasable {
@JsonIgnore private Matrix wpilibMat = null;
@JsonIgnore private MatOfDouble wrappedMatOfDouble;
private boolean released = false;
public JsonMatOfDouble(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
@@ -57,36 +57,14 @@ public class JsonMatOfDouble implements Releasable {
this.data = data;
}
private static boolean isCameraMatrixMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3;
}
private static boolean isDistortionCoeffsMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1;
}
private static boolean isCalibrationMat(Mat mat) {
return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat);
}
@JsonIgnore
public static double[] getDataFromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
double[] data = new double[(int) (mat.total() * mat.elemSize())];
mat.get(0, 0, data);
int dataLen = -1;
if (isCameraMatrixMat(mat)) dataLen = 9;
if (isDistortionCoeffsMat(mat)) dataLen = 5;
// truncate Mat data to correct number data points.
return Arrays.copyOfRange(data, 0, dataLen);
return data;
}
public static JsonMatOfDouble fromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat));
}
@@ -98,11 +76,20 @@ public class JsonMatOfDouble implements Releasable {
this.wrappedMat = new Mat(this.rows, this.cols, this.type);
this.wrappedMat.put(0, 0, this.data);
}
if (this.released) {
throw new RuntimeException("This calibration object was already released");
}
return this.wrappedMat;
}
@JsonIgnore
public MatOfDouble getAsMatOfDouble() {
if (this.released) {
throw new RuntimeException("This calibration object was already released");
}
if (this.wrappedMatOfDouble == null) {
this.wrappedMatOfDouble = new MatOfDouble();
getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F);
@@ -110,6 +97,7 @@ public class JsonMatOfDouble implements Releasable {
return this.wrappedMatOfDouble;
}
@SuppressWarnings("unchecked")
@JsonIgnore
public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
if (wpilibMat == null) {
@@ -120,12 +108,14 @@ public class JsonMatOfDouble implements Releasable {
@Override
public void release() {
getAsMat().release();
}
if (wrappedMat != null) {
wrappedMat.release();
}
if (wrappedMatOfDouble != null) {
wrappedMatOfDouble.release();
}
public Packet populatePacket(Packet packet) {
packet.encode(this.data);
return packet;
this.released = true;
}
@Override

6
photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java
View File

@@ -55,6 +55,10 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
}
}
public ImageRotationMode getRotation() {
return m_rotationMode;
}
public LibcameraGpuSettables(CameraConfiguration configuration) {
super(configuration);
@@ -212,7 +216,7 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
getConfiguration().path,
mode.width,
mode.height,
(m_rotationMode == ImageRotationMode.DEG_180 ? 180 : 0));
(m_rotationMode == ImageRotationMode.DEG_180_CCW ? 180 : 0));
if (r_ptr == 0) {
logger.error("Couldn't create a zero copy Pi Camera while switching video modes");
if (!LibCameraJNI.destroyCamera(r_ptr)) {

31
photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
View File

@@ -21,6 +21,7 @@ import edu.wpi.first.cscore.VideoMode;
import org.opencv.core.Point;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.opencv.ImageRotationMode;
/** Represents the properties of a frame. */
public class FrameStaticProperties {
@@ -35,6 +36,10 @@ public class FrameStaticProperties {
public final double verticalFocalLength;
public CameraCalibrationCoefficients cameraCalibration;
// CameraCalibrationCoefficients hold native memory, so cache them here to avoid extra allocations
private final FrameStaticProperties[] cachedRotationStaticProperties =
new FrameStaticProperties[4];
/**
* Instantiates a new Frame static properties.
*
@@ -85,6 +90,32 @@ public class FrameStaticProperties {
}
}
public FrameStaticProperties rotate(ImageRotationMode rotation) {
if (rotation == ImageRotationMode.DEG_0) {
return this;
}
int newWidth = imageWidth;
int newHeight = imageHeight;
if (rotation == ImageRotationMode.DEG_90_CCW || rotation == ImageRotationMode.DEG_270_CCW) {
newWidth = imageHeight;
newHeight = imageWidth;
}
if (cameraCalibration == null) {
return new FrameStaticProperties(newWidth, newHeight, fov, null);
}
if (cachedRotationStaticProperties[rotation.ordinal()] == null) {
cachedRotationStaticProperties[rotation.ordinal()] =
new FrameStaticProperties(
newWidth, newHeight, fov, cameraCalibration.rotateCoefficients(rotation));
}
return cachedRotationStaticProperties[rotation.ordinal()];
}
/**
* Calculates the horizontal and vertical FOV components from a given diagonal FOV and image size.
*

2
photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java
View File

@@ -99,7 +99,7 @@ public abstract class CpuImageProcessor extends FrameProvider {
outputMat,
m_processType,
input.captureTimestamp,
input.staticProps);
input.staticProps.rotate(m_rImagePipe.getParams().rotation));
}
@Override

2
photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java
View File

@@ -101,7 +101,7 @@ public class LibcameraGpuFrameProvider extends FrameProvider {
processedMat,
type,
MathUtils.wpiNanoTime() - latency,
settables.getFrameStaticProperties());
settables.getFrameStaticProperties().rotate(settables.getRotation()));
}
}

57
photon-core/src/main/java/org/photonvision/vision/opencv/ImageRotationMode.java
View File

@@ -17,19 +17,62 @@
package org.photonvision.vision.opencv;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import org.opencv.core.Core;
import org.opencv.core.Point;
/**
* An image rotation about the camera's +Z axis, which points out of the camera towards the world.
* This is mirrored relative to what you might traditionally think of as image rotation, which is
* about an axis coming out of the image towards the viewer or camera. TODO: pull this from
* image-rotation.md
*/
public enum ImageRotationMode {
DEG_0(-1),
DEG_90(0),
DEG_180(1),
DEG_270(2);
DEG_0(-1, new Rotation2d()),
// rotating an image matrix clockwise is a ccw rotation about camera +Z, lmao
DEG_90_CCW(Core.ROTATE_90_COUNTERCLOCKWISE, new Rotation2d(Units.degreesToRadians(90))),
DEG_180_CCW(Core.ROTATE_180, new Rotation2d(Units.degreesToRadians(180))),
DEG_270_CCW(Core.ROTATE_90_CLOCKWISE, new Rotation2d(Units.degreesToRadians(-90)));
public final int value;
public final Rotation2d rotation2d;
ImageRotationMode(int value) {
private ImageRotationMode(int value, Rotation2d tr) {
this.value = value;
this.rotation2d = tr;
}
public boolean isRotated() {
return this.value == DEG_90.value || this.value == DEG_270.value;
/**
* Rotate a point in an image
*
* @param point The point in the unrotated image
* @param width Image width, in pixels
* @param height Image height, in pixels
* @return The point in the rotated frame
*/
public Point rotatePoint(Point point, double width, double height) {
Pose2d offset;
switch (this) {
case DEG_0:
return point;
case DEG_90_CCW:
offset = new Pose2d(width, 0, rotation2d);
break;
case DEG_180_CCW:
offset = new Pose2d(width, height, rotation2d);
break;
case DEG_270_CCW:
offset = new Pose2d(0, height, rotation2d);
break;
default:
throw new RuntimeException("Totally bjork");
}
var pointAsPose = new Pose2d(point.x, point.y, new Rotation2d());
var ret = pointAsPose.relativeTo(offset);
return new Point(ret.getX(), ret.getY());
}
}

7
photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw2dCrosshairPipe.java
View File

@@ -47,13 +47,6 @@ public class Draw2dCrosshairPipe
double y = params.frameStaticProperties.centerY;
double scale = params.frameStaticProperties.imageWidth / (double) params.divisor.value / 32.0;
if (this.params.rotMode == ImageRotationMode.DEG_270
|| this.params.rotMode == ImageRotationMode.DEG_90) {
var tmp = x;
x = y;
y = tmp;
}
switch (params.robotOffsetPointMode) {
case Single:
if (params.singleOffsetPoint.x != 0 && params.singleOffsetPoint.y != 0) {

53
photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
View File

@@ -18,7 +18,6 @@
package org.photonvision.vision.pipe.impl;
import java.awt.*;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
@@ -28,6 +27,7 @@ import org.opencv.imgproc.Imgproc;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.pipe.MutatingPipe;
@@ -92,7 +92,11 @@ public class Draw3dTargetsPipe
if (params.redistortPoints) {
// Distort the points, so they match the image they're being overlaid on
distortPoints(tempMat, tempMat);
tempMat.fromList(
OpenCVHelp.distortPoints(
tempMat.toList(),
params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
params.cameraCalibrationCoefficients.getDistCoeffsMat()));
}
var bottomPoints = tempMat.toList();
@@ -108,7 +112,11 @@ public class Draw3dTargetsPipe
if (params.redistortPoints) {
// Distort the points, so they match the image they're being overlaid on
distortPoints(tempMat, tempMat);
tempMat.fromList(
OpenCVHelp.distortPoints(
tempMat.toList(),
params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
params.cameraCalibrationCoefficients.getDistCoeffsMat()));
}
var topPoints = tempMat.toList();
@@ -223,45 +231,6 @@ public class Draw3dTargetsPipe
return null;
}
private void distortPoints(MatOfPoint2f src, MatOfPoint2f dst) {
var pointsList = src.toList();
var dstList = new ArrayList<Point>();
final Mat cameraMatrix = params.cameraCalibrationCoefficients.getCameraIntrinsicsMat();
// k1, k2, p1, p2, k3
final Mat distCoeffs = params.cameraCalibrationCoefficients.getDistCoeffsMat();
var cx = cameraMatrix.get(0, 2)[0];
var cy = cameraMatrix.get(1, 2)[0];
var fx = cameraMatrix.get(0, 0)[0];
var fy = cameraMatrix.get(1, 1)[0];
var k1 = distCoeffs.get(0, 0)[0];
var k2 = distCoeffs.get(0, 1)[0];
var k3 = distCoeffs.get(0, 4)[0];
var p1 = distCoeffs.get(0, 2)[0];
var p2 = distCoeffs.get(0, 3)[0];
for (Point point : pointsList) {
// To relative coordinates <- this is the step you are missing.
double x = (point.x - cx) / fx; // cx, cy is the center of distortion
double y = (point.y - cy) / fy;
double r2 = x * x + y * y; // square of the radius from center
// Radial distortion
double xDistort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
double yDistort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
// Tangential distortion
xDistort = xDistort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x));
yDistort = yDistort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y);
// Back to absolute coordinates.
xDistort = xDistort * fx + cx;
yDistort = yDistort * fy + cy;
dstList.add(new Point(xDistort, yDistort));
}
dst.fromList(dstList);
}
private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) {
var hull = src.toArray();
var pointArray = new Point[hull.length];

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

@@ -619,8 +619,8 @@ public class VisionModule {
public void addCalibrationToConfig(CameraCalibrationCoefficients newCalibration) {
if (newCalibration != null) {
logger.info("Got new calibration for " + newCalibration.resolution);
visionSource.getSettables().getConfiguration().addCalibration(newCalibration);
logger.info("Got new calibration for " + newCalibration.unrotatedImageSize);
visionSource.getSettables().addCalibration(newCalibration);
} else {
logger.error("Got null calibration?");
}

4
photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
View File

@@ -114,8 +114,8 @@ public abstract class VisionSourceSettables {
configuration.calibrations.stream()
.filter(
it ->
it.resolution.width == videoMode.width
&& it.resolution.height == videoMode.height)
it.unrotatedImageSize.width == videoMode.width
&& it.unrotatedImageSize.height == videoMode.height)
.findFirst()
.orElse(null));
}

252
photon-core/src/test/java/org/photonvision/vision/pipeline/CalibrationRotationPipeTest.java Normal file
View File

@@ -0,0 +1,252 @@
/*
* 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;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.io.IOException;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.junit.jupiter.api.Assertions;
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.opencv.core.Point;
import org.opencv.core.Size;
import org.photonvision.common.configuration.ConfigManager;
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.estimation.OpenCVHelp;
import org.photonvision.mrcal.MrCalJNILoader;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.calibration.CameraLensModel;
import org.photonvision.vision.calibration.JsonMatOfDouble;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.ImageRotationMode;
import org.photonvision.vision.pipe.impl.SolvePNPPipe;
import org.photonvision.vision.pipe.impl.SolvePNPPipe.SolvePNPPipeParams;
import org.photonvision.vision.target.TargetModel;
import org.photonvision.vision.target.TrackedTarget;
public class CalibrationRotationPipeTest {
@BeforeAll
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);
ConfigManager.getInstance().load();
}
@Test
public void meme() {
var s = new Size(200, 100);
var p = new Point(2, 1);
{
var angle = ImageRotationMode.DEG_90_CCW;
var expected = new Point(p.y, s.width - p.x);
var rotatedP = angle.rotatePoint(p, s.width, s.height);
assertEquals(expected.x, rotatedP.x, 1e-6);
assertEquals(expected.y, rotatedP.y, 1e-6);
}
{
var angle = ImageRotationMode.DEG_180_CCW;
var expected = new Point(s.width - p.x, s.height - p.y);
var rotatedP = angle.rotatePoint(p, s.width, s.height);
assertEquals(expected.x, rotatedP.x, 1e-6);
assertEquals(expected.y, rotatedP.y, 1e-6);
}
{
var angle = ImageRotationMode.DEG_270_CCW;
var expected = new Point(s.height - p.y, p.x);
var rotatedP = angle.rotatePoint(p, s.width, s.height);
assertEquals(expected.x, rotatedP.x, 1e-6);
assertEquals(expected.y, rotatedP.y, 1e-6);
}
}
@CartesianTest
public void testUndistortImagePointsWithRotation(@Enum ImageRotationMode rot) {
if (rot == ImageRotationMode.DEG_0) {
return;
}
CameraCalibrationCoefficients coeffs =
new CameraCalibrationCoefficients(
new Size(1270, 720),
new JsonMatOfDouble(
3,
3,
new double[] {
900, 0, 500,
0, 951, 321,
0, 0, 1
}),
new JsonMatOfDouble(
1,
8,
new double[] {
0.25,
-1.5,
0.0017808248356550637,
.00004,
2.179764689221826,
-0.034952777924711353,
0.09625562194891251,
-0.1860797479660746
}),
new double[] {},
List.of(),
new Size(),
1,
CameraLensModel.LENSMODEL_OPENCV);
FrameStaticProperties frameProps =
new FrameStaticProperties(
(int) coeffs.unrotatedImageSize.width,
(int) coeffs.unrotatedImageSize.height,
66,
coeffs);
FrameStaticProperties rotatedFrameProps = frameProps.rotate(rot);
Point[] originalPoints = {new Point(100, 100), new Point(200, 200), new Point(300, 100)};
// Distort the origional points
var distortedOriginalPoints =
OpenCVHelp.distortPoints(
List.of(originalPoints),
frameProps.cameraCalibration.getCameraIntrinsicsMat(),
frameProps.cameraCalibration.getDistCoeffsMat());
// and rotate them once distorted
var rotatedDistortedPoints =
distortedOriginalPoints.stream()
.map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight))
.collect(Collectors.toList());
// Now let's instead rotate then distort
var rotatedOrigionalPoints =
Arrays.stream(originalPoints)
.map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight))
.collect(Collectors.toList());
var distortedRotatedPoints =
OpenCVHelp.distortPoints(
rotatedOrigionalPoints,
rotatedFrameProps.cameraCalibration.getCameraIntrinsicsMat(),
rotatedFrameProps.cameraCalibration.getDistCoeffsMat());
System.out.println("Rotated distorted: " + rotatedDistortedPoints.toString());
System.out.println("Distorted rotated: " + distortedRotatedPoints.toString());
for (int i = 0; i < distortedRotatedPoints.size(); i++) {
assertEquals(rotatedDistortedPoints.get(i).x, distortedRotatedPoints.get(i).x, 1e-6);
assertEquals(rotatedDistortedPoints.get(i).y, distortedRotatedPoints.get(i).y, 1e-6);
}
}
@Test
public void testApriltagRotated() {
// matt's lifecam
CameraCalibrationCoefficients coeffs =
new CameraCalibrationCoefficients(
new Size(1270, 720),
new JsonMatOfDouble(
3,
3,
new double[] {
1132.983599412085, 0.0, 610.3195830765223,
0.0, 1138.2884596791835, 346.4121207400337,
0.0, 0.0, 1.0
}),
new JsonMatOfDouble(
1,
8,
new double[] {
0.11508197558262527,
-1.158603446817735,
0.0017808248356550637,
4.3915976993589873E-4,
2.179764689221826,
-0.034952777924711353,
0.04625562194891251,
-0.0860797479660746
}),
new double[] {},
List.of(),
new Size(),
1,
CameraLensModel.LENSMODEL_OPENCV);
// Matt's lifecam pointing at a wall
var distortedCorners =
List.of(
new Point(834.702271, 338.878143),
new Point(1011.808899, 345.824463),
new Point(964.300476, 225.330795),
new Point(803.971191, 217.359055));
SolvePNPPipe pipe = new SolvePNPPipe();
pipe.setParams(new SolvePNPPipeParams(coeffs, TargetModel.kAprilTag6p5in_36h11));
var ret = pipe.run(List.of(new TrackedTarget(distortedCorners)));
// rotate and try again
var rotAngle = ImageRotationMode.DEG_90_CCW;
var rotatedDistortedPoints =
distortedCorners.stream()
.map(it -> rotAngle.rotatePoint(it, 1280, 720))
.collect(Collectors.toList());
pipe.setParams(
new SolvePNPPipeParams(
coeffs.rotateCoefficients(rotAngle), TargetModel.kAprilTag6p5in_36h11));
var retRotated = pipe.run(List.of(new TrackedTarget(rotatedDistortedPoints)));
var pose_base = ret.output.get(0).getBestCameraToTarget3d();
// So this is ostensibly a rotation about camera +Z,
// but this is actually camera +X for our AprilTag pipe since we rotate to stay in ""WPILib""".
// Negative to return to upright
var pose_unrotated = retRotated.output.get(0).getBestCameraToTarget3d();
System.out.println("Base: " + pose_base);
System.out.println("rot-unrot: " + pose_unrotated);
Assertions.assertEquals(pose_base.getX(), pose_unrotated.getX(), 0.01);
Assertions.assertEquals(pose_base.getY(), pose_unrotated.getY(), 0.01);
Assertions.assertEquals(pose_base.getZ(), pose_unrotated.getZ(), 0.01);
Assertions.assertEquals(
pose_base.getRotation().getX(), pose_unrotated.getRotation().getX(), 0.01);
Assertions.assertEquals(
pose_base.getRotation().getY(), pose_unrotated.getRotation().getY(), 0.01);
Assertions.assertEquals(
pose_base.getRotation().getZ(), pose_unrotated.getRotation().getZ(), 0.01);
}
}