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

[photonlib] Simulation Visualization Update (#895)

Browse Source
This commit is contained in:
amquake
2023-09-19 16:10:04 -07:00
committed by GitHub
parent 9e371de1cb
commit 7f283640c4
11 changed files with 1140 additions and 335 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
photon-lib/src/test/java/org/photonvision/OpenCVTest.java
View File

@@ -48,6 +48,7 @@ import org.junit.jupiter.api.Test;
import org.opencv.core.Core;
import org.photonvision.estimation.CameraTargetRelation;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionSystemSim;
@@ -143,36 +144,46 @@ public class OpenCVTest {
new VisionTargetSim(
new Pose3d(1, 0, 0, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0);
var cameraPose = new Pose3d(0, 0, 0, new Rotation3d());
var targetCorners =
var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
var imagePoints =
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices());
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
// find circulation (counter/clockwise-ness)
double circulation = 0;
for (int i = 0; i < targetCorners.size(); i++) {
double xDiff = targetCorners.get((i + 1) % 4).x - targetCorners.get(i).x;
double ySum = targetCorners.get((i + 1) % 4).y + targetCorners.get(i).y;
for (int i = 0; i < imagePoints.length; i++) {
double xDiff = imagePoints[(i + 1) % 4].x - imagePoints[i].x;
double ySum = imagePoints[(i + 1) % 4].y + imagePoints[i].y;
circulation += xDiff * ySum;
}
assertTrue(circulation > 0, "2d fiducial points aren't counter-clockwise");
// undo projection distortion
targetCorners = prop.undistort(targetCorners);
var avgCenterRot1 = prop.getPixelRot(OpenCVHelp.averageCorner(targetCorners));
imagePoints =
OpenCVHelp.undistortPoints(prop.getIntrinsics(), prop.getDistCoeffs(), imagePoints);
// test projection results after moving camera
var avgCenterRot1 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints));
cameraPose =
cameraPose.plus(new Transform3d(new Translation3d(), new Rotation3d(0, 0.25, 0.25)));
targetCorners =
camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
imagePoints =
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices());
var avgCenterRot2 = prop.getPixelRot(OpenCVHelp.averageCorner(targetCorners));
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var avgCenterRot2 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints));
var yaw2d = new Rotation2d(avgCenterRot2.getZ());
var pitch2d = new Rotation2d(avgCenterRot2.getY());
var yawDiff = yaw2d.minus(new Rotation2d(avgCenterRot1.getZ()));
var pitchDiff = pitch2d.minus(new Rotation2d(avgCenterRot1.getY()));
assertTrue(yawDiff.getRadians() < 0, "2d points don't follow yaw");
assertTrue(pitchDiff.getRadians() < 0, "2d points don't follow pitch");
var actualRelation = new CameraTargetRelation(cameraPose, target.getPose());
assertEquals(
actualRelation.camToTargPitch.getDegrees(),
pitchDiff.getDegrees() * Math.cos(yaw2d.getRadians()), // adjust for perpsective distortion
pitchDiff.getDegrees()
* Math.cos(yaw2d.getRadians()), // adjust for unaccounted perpsective distortion
kRotDeltaDeg,
"2d pitch doesn't match 3d");
assertEquals(
@@ -184,23 +195,31 @@ public class OpenCVTest {
@Test
public void testSolvePNP_SQUARE() {
// square AprilTag target
var target =
new VisionTargetSim(
new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0);
var cameraPose = new Pose3d(0, 0, 0, new Rotation3d());
var actualRelation = new CameraTargetRelation(cameraPose, target.getPose());
var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
// target relative to camera
var relTarget = camRt.apply(target.getPose());
// simulate solvePNP estimation
var targetCorners =
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices());
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best));
assertSame(actualRelation.camToTarg, estRelation.camToTarg);
// check solvePNP estimation accuracy
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());
assertSame(relTarget.getTranslation(), pnpSim.best.getTranslation());
}
@Test
public void testSolvePNP_SQPNP() {
// (for targets with arbitrary number of non-colinear points > 2)
var target =
new VisionTargetSim(
new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)),
@@ -216,14 +235,20 @@ public class OpenCVTest {
new Translation3d(-1, 0, 0))),
0);
var cameraPose = new Pose3d(0, 0, 0, new Rotation3d());
var actualRelation = new CameraTargetRelation(cameraPose, target.getPose());
var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
// target relative to camera
var relTarget = camRt.apply(target.getPose());
// simulate solvePNP estimation
var targetCorners =
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices());
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var pnpSim =
OpenCVHelp.solvePNP_SQPNP(
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best));
assertSame(actualRelation.camToTarg, estRelation.camToTarg);
// check solvePNP estimation accuracy
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());
assertSame(relTarget.getTranslation(), pnpSim.best.getTranslation());
}
}

15
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

@@ -348,7 +348,7 @@ class VisionSystemSimTest {
Arguments.of(20, -42, 0),
Arguments.of(5, -35, 2),
Arguments.of(6, -35, 0),
Arguments.of(10, -34, 3.2),
Arguments.of(10, -34, 2.4),
Arguments.of(15, -33, 0),
Arguments.of(19.52, -15.98, 1.1));
}
@@ -376,23 +376,28 @@ class VisionSystemSimTest {
cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(160));
cameraSim.setMinTargetAreaPixels(0.0);
visionSysSim.adjustCamera(cameraSim, robotToCamera);
// note that non-fiducial targets have different center point calculation and will
// return slightly inaccurate yaw/pitch values
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 0));
visionSysSim.update(robotPose);
// Note that target 2d yaw/pitch accuracy is hindered by two factors in photonvision:
// 1. These are calculated with the average of the minimum area rectangle, which does not
// actually find the target center because of perspective distortion.
// 2. Yaw and pitch are calculated separately which gives incorrect pitch values.
var res = camera.getLatestResult();
assertTrue(res.hasTargets());
var tgt = res.getBestTarget();
assertEquals(0.0, tgt.getYaw(), kRotDeltaDeg);
assertEquals(0.0, tgt.getYaw(), 0.5);
// Distance calculation using this trigonometry may be wildly incorrect when
// there is not much height difference between the target and the camera.
double distMeas =
PhotonUtils.calculateDistanceToTargetMeters(
robotToCamera.getZ(),
targetPose.getZ(),
Units.degreesToRadians(-testPitch),
Units.degreesToRadians(tgt.getPitch()));
assertEquals(Units.feetToMeters(testDist), distMeas, kTrlDelta);
assertEquals(Units.feetToMeters(testDist), distMeas, 0.15);
}
@Test