Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41: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());
}
}