mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-30 02:31:40 +00:00
Calibration Rotation! (#1464)
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)
@@ -36,6 +36,7 @@ extensions = [
|
||||
"sphinxcontrib.ghcontributors",
|
||||
"sphinx_design",
|
||||
"myst_parser",
|
||||
"sphinx.ext.mathjax",
|
||||
]
|
||||
|
||||
# Configure OpenGraph support
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
# Calibration and Image Rotation
|
||||
|
||||
## Rotating Points
|
||||
|
||||
To stay consistent with the OpenCV camera coordinate frame, we put the origin in the top left, with X right, Y down, and Z out (as required by the right-hand rule). Intuitively though, if I ask you to rotate an image 90 degrees clockwise though, you'd probably rotate it about -Z in this coordinate system. Just be aware of this inconsistency.
|
||||
|
||||
|
||||
|
||||
If we have any one point in any of those coordinate systems, we can transform it into any of the other ones using standard geometry libraries by performing relative transformations (like in this pseudocode):
|
||||
|
||||
```
|
||||
Translation2d tag_corner1 = new Translation2d();
|
||||
Translation2d rotated = tag_corner1.relativeTo(ORIGIN_ROTATED_90_CCW);
|
||||
```
|
||||
|
||||
## Image Distortion
|
||||
|
||||
The distortion coefficients for OPENCV8 is given in order `[k1 k2 p1 p2 k3 k4 k5 k6]`. Mrcal names these coefficients `[k_0 k_1, k_2, k_3, k_4, k_5, k_6, k_7]`.
|
||||
|
||||
```{math}
|
||||
\begin{align*}
|
||||
\vec P &\equiv \frac{\vec p_{xy}}{p_z} \\
|
||||
r &\equiv \left|\vec P\right| \\
|
||||
\vec P_\mathrm{radial} &\equiv \frac{ 1 + k_0 r^2 + k_1 r^4 + k_4 r^6}{ 1 + k_5 r^2 + k_6 r^4 + k_7 r^6} \vec P \\
|
||||
\vec P_\mathrm{tangential} &\equiv
|
||||
\left[ \begin{aligned}
|
||||
2 k_2 P_0 P_1 &+ k_3 \left(r^2 + 2 P_0^2 \right) \\
|
||||
2 k_3 P_0 P_1 &+ k_2 \left(r^2 + 2 P_1^2 \right)
|
||||
\end{aligned}\right] \\
|
||||
\vec q &= \vec f_{xy} \left( \vec P_\mathrm{radial} + \vec P_\mathrm{tangential} \right) + \vec c_{xy}
|
||||
\end{align*}
|
||||
```
|
||||
|
||||
From this, we observe at `k_0, k_1, k_4, k_5, k_6, k_7` depend only on the norm of {math}`\vec P`, and will be constant given a rotated image. However, `k_2` and `k_3` go with {math}`P_0 \cdot P_1`, `k_3` with {math}`P_0^2`, and `k_2` with {math}`P_1^2`.
|
||||
|
||||
Let's try a concrete example. With a 90 degree CCW rotation, we have {math}`P0=-P_{1\mathrm{rotated}}` and {math}`P1=P_{0\mathrm{rotated}}`. Let's substitute in
|
||||
|
||||
```{math}
|
||||
\begin{align*}
|
||||
\left[ \begin{aligned}
|
||||
2 k_2 P_0 P_1 &+ k_3 \left(r^2 + 2 P_0^2 \right) \\
|
||||
2 k_3 P_0 P_1 &+ k_2 \left(r^2 + 2 P_1^2 \right)
|
||||
\end{aligned}\right] &=
|
||||
\left[ \begin{aligned}
|
||||
2 k_{2\mathrm{rotated}} (-P_{1\mathrm{rotated}}) P_{0\mathrm{rotated}} &+ k_{3\mathrm{rotated}} \left(r^2 + 2 (-P_{1\mathrm{rotated}})^2 \right) \\
|
||||
2 k_{3\mathrm{rotated}} (-P_{1\mathrm{rotated}}) P_{0\mathrm{rotated}} &+ k_{2\mathrm{rotated}} \left(r^2 + 2 P_{0\mathrm{rotated}}^2 \right)
|
||||
\end{aligned}\right] \\
|
||||
&=
|
||||
\left[ \begin{aligned}
|
||||
-2 k_{2\mathrm{rotated}} P_{1\mathrm{rotated}} P_{0\mathrm{rotated}} &+ k_{3\mathrm{rotated}} \left(r^2 + 2 P_{1\mathrm{rotated}}^2 \right) \\
|
||||
-2 k_{3\mathrm{rotated}} P_{1\mathrm{rotated}} P_{0\mathrm{rotated}} &+ k_{2\mathrm{rotated}} \left(r^2 + 2 P_{0\mathrm{rotated}}^2 \right)
|
||||
\end{aligned}\right]
|
||||
\end{align*}
|
||||
```
|
||||
|
||||
By inspection, this results in just applying another 90 degree rotation to the k2/k3 parameters. Proof is left as an exercise for the reader. Note that we can repeat this rotation to yield equations for tangential distortion for 180 and 270 degrees.
|
||||
|
||||
```{math}
|
||||
k_2'=-k_3
|
||||
k_3'=k_2
|
||||
```
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 14 KiB |
@@ -0,0 +1,6 @@
|
||||
# Software Architecture Design Descriptions
|
||||
|
||||
```{toctree}
|
||||
:maxdepth: 1
|
||||
image-rotation
|
||||
```
|
||||
@@ -4,4 +4,5 @@
|
||||
building-photon
|
||||
building-docs
|
||||
developer-docs/index
|
||||
design-descriptions/index
|
||||
```
|
||||
|
||||
@@ -130,32 +130,12 @@ const interactiveCols = computed(() =>
|
||||
tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions"
|
||||
@input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ cameraBlueGain: args }, false)"
|
||||
/>
|
||||
<!-- Disable camera orientation as stop gap for Issue 1084 until calibration data gets rotated. https://github.com/PhotonVision/photonvision/issues/1084 -->
|
||||
<v-banner
|
||||
v-show="
|
||||
useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
|
||||
useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode != 0
|
||||
"
|
||||
rounded
|
||||
dark
|
||||
color="red"
|
||||
text-color="white"
|
||||
class="mt-3"
|
||||
icon="mdi-alert-circle-outline"
|
||||
>
|
||||
Warning! A known bug affects rotation of calibrated camera. Turn off rotation here and rotate using
|
||||
cameraToRobotTransform in your robot code.
|
||||
</v-banner>
|
||||
<pv-select
|
||||
v-model="useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode"
|
||||
label="Orientation"
|
||||
tooltip="Rotates the camera stream. Rotation not available when camera has been calibrated."
|
||||
:items="cameraRotations"
|
||||
:select-cols="interactiveCols"
|
||||
:disabled="
|
||||
useCameraSettingsStore().isCurrentVideoFormatCalibrated &&
|
||||
useCameraSettingsStore().currentPipelineSettings.inputImageRotationMode == 0
|
||||
"
|
||||
@input="(args) => useCameraSettingsStore().changeCurrentPipelineSetting({ inputImageRotationMode: args }, false)"
|
||||
/>
|
||||
<pv-select
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)) {
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -101,7 +101,7 @@ public class LibcameraGpuFrameProvider extends FrameProvider {
|
||||
processedMat,
|
||||
type,
|
||||
MathUtils.wpiNanoTime() - latency,
|
||||
settables.getFrameStaticProperties());
|
||||
settables.getFrameStaticProperties().rotate(settables.getRotation()));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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?");
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -568,8 +568,8 @@ public class RequestHandler {
|
||||
.stream()
|
||||
.filter(
|
||||
it ->
|
||||
Math.abs(it.resolution.width - width) < 1e-4
|
||||
&& Math.abs(it.resolution.height - height) < 1e-4)
|
||||
Math.abs(it.unrotatedImageSize.width - width) < 1e-4
|
||||
&& Math.abs(it.unrotatedImageSize.height - height) < 1e-4)
|
||||
.findFirst()
|
||||
.orElse(null);
|
||||
|
||||
@@ -617,8 +617,8 @@ public class RequestHandler {
|
||||
cc.calibrations.stream()
|
||||
.filter(
|
||||
it ->
|
||||
Math.abs(it.resolution.width - width) < 1e-4
|
||||
&& Math.abs(it.resolution.height - height) < 1e-4)
|
||||
Math.abs(it.unrotatedImageSize.width - width) < 1e-4
|
||||
&& Math.abs(it.unrotatedImageSize.height - height) < 1e-4)
|
||||
.findFirst()
|
||||
.orElse(null);
|
||||
|
||||
|
||||
@@ -251,6 +251,65 @@ public final class OpenCVHelp {
|
||||
return trl.rotateBy(NWU_TO_EDN);
|
||||
}
|
||||
|
||||
/**
|
||||
* Distort a list of points in pixels using the OPENCV5/8 models. See image-rotation.md or
|
||||
* https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html for the math here.
|
||||
*
|
||||
* @param pointsList the undistorted points
|
||||
* @param cameraMatrix standard OpenCV camera mat
|
||||
* @param distCoeffs standard OpenCV distortion coefficeints. Must OPENCV5 or OPENCV8
|
||||
*/
|
||||
public static List<Point> distortPoints(
|
||||
List<Point> pointsList, Mat cameraMatrix, Mat distCoeffs) {
|
||||
var ret = new ArrayList<Point>();
|
||||
|
||||
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 p1 = distCoeffs.get(0, 2)[0];
|
||||
var p2 = distCoeffs.get(0, 3)[0];
|
||||
var k3 = distCoeffs.get(0, 4)[0];
|
||||
|
||||
double k4 = 0;
|
||||
double k5 = 0;
|
||||
double k6 = 0;
|
||||
if (distCoeffs.cols() == 8) {
|
||||
k4 = distCoeffs.get(0, 5)[0];
|
||||
k5 = distCoeffs.get(0, 6)[0];
|
||||
k6 = distCoeffs.get(0, 7)[0];
|
||||
}
|
||||
|
||||
for (Point point : pointsList) {
|
||||
// To relative coordinates
|
||||
double xprime = (point.x - cx) / fx; // cx, cy is the center of distortion
|
||||
double yprime = (point.y - cy) / fy;
|
||||
|
||||
double r_sq = xprime * xprime + yprime * yprime; // square of the radius from center
|
||||
|
||||
// Radial distortion
|
||||
double radialDistortion =
|
||||
(1 + k1 * r_sq + k2 * r_sq * r_sq + k3 * r_sq * r_sq * r_sq)
|
||||
/ (1 + k4 * r_sq + k5 * r_sq * r_sq + k6 * r_sq * r_sq * r_sq);
|
||||
double xDistort = xprime * radialDistortion;
|
||||
double yDistort = yprime * radialDistortion;
|
||||
|
||||
// Tangential distortion
|
||||
xDistort = xDistort + (2 * p1 * xprime * yprime + p2 * (r_sq + 2 * xprime * xprime));
|
||||
yDistort = yDistort + (p1 * (r_sq + 2 * yprime * yprime) + 2 * p2 * xprime * yprime);
|
||||
|
||||
// Back to absolute coordinates.
|
||||
xDistort = xDistort * fx + cx;
|
||||
yDistort = yDistort * fy + cy;
|
||||
ret.add(new Point(xDistort, yDistort));
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Project object points from the 3d world into the 2d camera image. The camera
|
||||
* properties(intrinsics, distortion) determine the results of this projection.
|
||||
|
||||
Reference in New Issue
Block a user