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

Undistort corner pitch/yaw using opencv (#1250)

Browse Source 
* Undistort pitch/yaw

* Actually implement lol

* Update TargetCalculations.java

* fix yawpitch test units

* format

---------

Co-authored-by: amquake <noleetarrr@gmail.com>
This commit is contained in:
Matt
2024-05-02 21:17:28 -04:00
committed by GitHub
parent 6535710fc4
commit 00c2a25730
4 changed files with 112 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java
View File

@@ -16,13 +16,18 @@
*/
package org.photonvision.vision.target;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.opencv.core.TermCriteria;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.opencv.DualOffsetValues;
public class TargetCalculations {
/**
* Calculates the yaw and pitch of a point in the image. Yaw and pitch must be calculated together
* to account for perspective distortion. Yaw is positive right, and pitch is positive up.
@@ -33,6 +38,7 @@ public class TargetCalculations {
* @param offsetCenterY The Y value of the offset principal point (cy) in pixels
* @param targetCenterY The Y value of the target's center point in pixels
* @param verticalFocalLength The vertical focal length (fy) in pixels
* @param cameraCal Camera calibration parameters, or null if not calibrated
* @return The yaw and pitch from the principal axis to the target center, in degrees.
*/
public static DoubleCouple calculateYawPitch(
@@ -41,7 +47,34 @@ public class TargetCalculations {
double horizontalFocalLength,
double offsetCenterY,
double targetCenterY,
double verticalFocalLength) {
double verticalFocalLength,
CameraCalibrationCoefficients cameraCal) {
if (cameraCal != null) {
// undistort
MatOfPoint2f temp = new MatOfPoint2f();
temp.fromArray(new Point(targetCenterX, targetCenterY));
// Tighten up termination criteria
var termCriteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 1e-6);
Calib3d.undistortImagePoints(
temp,
temp,
cameraCal.getCameraIntrinsicsMat(),
cameraCal.getDistCoeffsMat(),
termCriteria);
float buff[] = new float[2];
temp.get(0, 0, buff);
temp.release();
// if outside of the imager, convergence fails, or really really bad user camera cal,
// undistort will fail by giving us nans. at some point we should log this failure
// if we can't undistort, don't change the cnter location
if (Float.isFinite(buff[0]) && Float.isFinite(buff[1])) {
targetCenterX = buff[0];
targetCenterY = buff[1];
}
}
double yaw = Math.atan((targetCenterX - offsetCenterX) / horizontalFocalLength);
double pitch =
Math.atan((offsetCenterY - targetCenterY) / (verticalFocalLength / Math.cos(yaw)));

18
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -33,6 +33,7 @@ import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.CVShape;
import org.photonvision.vision.opencv.Contour;
@@ -92,7 +93,8 @@ public class TrackedTarget implements Releasable {
params.horizontalFocalLength,
params.cameraCenterPoint.y,
tagDetection.getCenterY(),
params.verticalFocalLength);
params.verticalFocalLength,
params.cameraCal);
m_yaw = yawPitch.getFirst();
m_pitch = yawPitch.getSecond();
var bestPose = new Transform3d();
@@ -187,7 +189,8 @@ public class TrackedTarget implements Releasable {
params.horizontalFocalLength,
params.cameraCenterPoint.y,
result.getCenterY(),
params.verticalFocalLength);
params.verticalFocalLength,
params.cameraCal);
m_yaw = yawPitch.getFirst();
m_pitch = yawPitch.getSecond();
@@ -323,7 +326,8 @@ public class TrackedTarget implements Releasable {
params.horizontalFocalLength,
m_robotOffsetPoint.y,
m_targetOffsetPoint.y,
params.verticalFocalLength);
params.verticalFocalLength,
params.cameraCal);
m_yaw = yawPitch.getFirst();
m_pitch = yawPitch.getSecond();
@@ -480,6 +484,9 @@ public class TrackedTarget implements Releasable {
// area calculation values
final double imageArea;
// Camera calibration, null if not calibrated
final CameraCalibrationCoefficients cameraCal;
public TargetCalculationParameters(
boolean isLandscape,
TargetOffsetPointEdge targetOffsetPointEdge,
@@ -489,7 +496,8 @@ public class TrackedTarget implements Releasable {
Point cameraCenterPoint,
double horizontalFocalLength,
double verticalFocalLength,
double imageArea) {
double imageArea,
CameraCalibrationCoefficients cal) {
this.isLandscape = isLandscape;
this.targetOffsetPointEdge = targetOffsetPointEdge;
@@ -500,6 +508,7 @@ public class TrackedTarget implements Releasable {
this.horizontalFocalLength = horizontalFocalLength;
this.verticalFocalLength = verticalFocalLength;
this.imageArea = imageArea;
this.cameraCal = cal;
}
public TargetCalculationParameters(
@@ -520,6 +529,7 @@ public class TrackedTarget implements Releasable {
this.horizontalFocalLength = frameStaticProperties.horizontalFocalLength;
this.verticalFocalLength = frameStaticProperties.verticalFocalLength;
this.imageArea = frameStaticProperties.imageArea;
this.cameraCal = frameStaticProperties.cameraCalibration;
}
}
}

81
photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java
View File

@@ -21,6 +21,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import java.util.stream.Stream;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeAll;
@@ -33,12 +34,15 @@ import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.util.TestUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
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.DualOffsetValues;
public class TargetCalculationsTest {
private static Size imageSize = new Size(800, 600);
private static Size imageSize = new Size(1280, 720);
private static Point imageCenterPoint =
new Point(imageSize.width / 2.0 - 0.5, imageSize.height / 2.0 - 0.5);
private static final double diagFOV = Math.toRadians(70.0);
@@ -55,7 +59,8 @@ public class TargetCalculationsTest {
imageCenterPoint,
props.horizontalFocalLength,
props.verticalFocalLength,
imageSize.width * imageSize.height);
imageSize.width * imageSize.height,
null);
@BeforeAll
public static void setup() {
@@ -76,7 +81,8 @@ public class TargetCalculationsTest {
params.horizontalFocalLength,
imageCenterPoint.y,
targetCenterPoint.y,
params.verticalFocalLength);
params.verticalFocalLength,
params.cameraCal);
assertTrue(targetYawPitch.getFirst() > 0, "Yaw is not positive right");
assertTrue(targetYawPitch.getSecond() < 0, "Pitch is not positive up");
@@ -91,7 +97,8 @@ public class TargetCalculationsTest {
params.horizontalFocalLength,
imageCenterPoint.y,
imageCenterPoint.y,
params.verticalFocalLength);
params.verticalFocalLength,
params.cameraCal);
assertEquals(fovs.getFirst() / 2.0, maxYaw.getFirst(), 0.025, "Horizontal FOV check failed");
var maxPitch =
TargetCalculations.calculateYawPitch(
@@ -100,7 +107,8 @@ public class TargetCalculationsTest {
params.horizontalFocalLength,
imageCenterPoint.y,
0,
params.verticalFocalLength);
params.verticalFocalLength,
params.cameraCal);
assertEquals(fovs.getSecond() / 2.0, maxPitch.getSecond(), 0.025, "Vertical FOV check failed");
}
@@ -112,17 +120,40 @@ public class TargetCalculationsTest {
Arguments.of(0, 10),
Arguments.of(10, 10),
Arguments.of(-10, -10),
Arguments.of(30, 45),
Arguments.of(-45, -20));
Arguments.of(-18, 14),
Arguments.of(-23, -16));
}
private static double[] testCameraMatrix = {240, 0, 320, 0, 240, 320, 0, 0, 1};
@ParameterizedTest
@MethodSource("testYawPitchCalcArgs")
public void testYawPitchCalc(double yawDeg, double pitchDeg) {
Mat testCameraMat = new Mat(3, 3, CvType.CV_64F);
testCameraMat.put(0, 0, testCameraMatrix);
// FOV: ~58.5 deg horizontal, ~35 deg vertical
JsonMatOfDouble testCameraMatrix =
new JsonMatOfDouble(
3, 3, new double[] {1142.341323, 0, 621.384201309, 0, 1139.92214, 349.897631, 0, 0, 1});
JsonMatOfDouble testDistortion =
new JsonMatOfDouble(
5,
1,
new double[] {
0.186841202993646,
-1.482894102216622,
0.005692954661309707,
0.0006757267756945662,
2.8659664873321287
});
double IMAGER_WIDTH = 1280, IMAGER_HEIGHT = 720;
var testCameraCal =
new CameraCalibrationCoefficients(
imageSize,
testCameraMatrix,
testDistortion,
new double[0],
List.of(),
new Size(),
0,
CameraLensModel.LENSMODEL_OPENCV);
// Since we create this translation using the given yaw/pitch, we should see the same angles
// calculated
var targetTrl =
@@ -136,21 +167,33 @@ public class TargetCalculationsTest {
objectPoints,
new MatOfDouble(0, 0, 0),
new MatOfDouble(0, 0, 0),
testCameraMat,
new MatOfDouble(0, 0, 0, 0, 0),
testCameraCal.getCameraIntrinsicsMat(),
testCameraCal.getDistCoeffsMat(),
imagePoints);
var point = imagePoints.toArray()[0];
// need point within FOV to be valid
assertTrue(Math.abs(point.x) >= 0);
assertTrue(Math.abs(point.x) <= IMAGER_WIDTH);
assertTrue(Math.abs(point.y) >= 0);
assertTrue(Math.abs(point.y) <= IMAGER_HEIGHT);
// Test if the target yaw/pitch calculation matches what the target was created with
var yawPitch =
TargetCalculations.calculateYawPitch(
testCameraCal.cameraIntrinsics.data[2],
point.x,
testCameraMatrix[2],
testCameraMatrix[0],
testCameraCal.cameraIntrinsics.data[0],
testCameraCal.cameraIntrinsics.data[5],
point.y,
testCameraMatrix[5],
testCameraMatrix[4]);
assertEquals(yawDeg, yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect");
assertEquals(pitchDeg, yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect");
testCameraCal.cameraIntrinsics.data[4],
testCameraCal);
// convert photon angles to wpilib NWU angles
assertEquals(yawDeg, -yawPitch.getFirst(), 1e-3, "Yaw calculation incorrect");
assertEquals(pitchDeg, -yawPitch.getSecond(), 1e-3, "Pitch calculation incorrect");
testCameraCal.release();
testDistortion.release();
}
@Test

3
photon-core/src/test/java/org/photonvision/vision/target/TrackedTargetTest.java
View File

@@ -59,7 +59,8 @@ public class TrackedTargetTest {
new Point(imageSize.width / 2, imageSize.height / 2),
61,
34.3,
imageSize.area());
imageSize.area(),
null);
var trackedTarget = new TrackedTarget(pTarget, setting, null);
// TODO change these hardcoded values