diff --git a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py index 00664785e..9f98f4f2b 100644 --- a/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py +++ b/photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py @@ -24,9 +24,32 @@ class RotTrlTransform3d: def getRotation(self) -> Rotation3d: return self.rot - def apply(self, trlToApply: Translation3d) -> Translation3d: + def applyTranslation(self, trlToApply: Translation3d) -> Translation3d: return trlToApply.rotateBy(self.rot) + self.trl + def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d: + return rotToApply + self.rot + + def applyPose(self, poseToApply: Pose3d) -> Pose3d: + return Pose3d( + self.applyTranslation(poseToApply.translation()), + self.applyRotation(poseToApply.rotation()), + ) + + def applyTrls(self, rots: list[Rotation3d]) -> list[Rotation3d]: + retVal: list[Rotation3d] = [] + for rot in rots: + retVal.append(self.applyRotation(rot)) + return retVal + @classmethod def makeRelativeTo(cls, pose: Pose3d) -> Self: return cls(pose.rotation(), pose.translation()).inverse() + + @classmethod + def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self: + return cls( + last.rotation() - initial.rotation(), + last.translation() + - initial.translation().rotateBy(last.rotation() - initial.rotation()), + ) diff --git a/photon-lib/py/photonlibpy/estimation/targetModel.py b/photon-lib/py/photonlibpy/estimation/targetModel.py index 652863403..ff1f2b8ed 100644 --- a/photon-lib/py/photonlibpy/estimation/targetModel.py +++ b/photon-lib/py/photonlibpy/estimation/targetModel.py @@ -107,7 +107,7 @@ class TargetModel: retVal = [] for vert in self.vertices: - retVal.append(basisChange.apply(vert)) + retVal.append(basisChange.applyTranslation(vert)) return retVal diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index 6869e594f..f4058d46b 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -226,8 +226,8 @@ class SimCameraProperties: def getVisibleLine( self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d ) -> typing.Tuple[float | None, float | None]: - relA = camRt.apply(a) - relB = camRt.apply(b) + relA = camRt.applyTranslation(a) + relB = camRt.applyTranslation(b) if relA.X() <= 0.0 and relB.X() <= 0.0: return (None, None) @@ -279,7 +279,7 @@ class SimCameraProperties: ipts[i] = None break - if not ipts[i]: + if ipts[i] is None: continue for j in range(i - 1, 0 - 1): diff --git a/photon-lib/py/test/openCVHelp_test.py b/photon-lib/py/test/openCVHelp_test.py new file mode 100644 index 000000000..a0bd30bed --- /dev/null +++ b/photon-lib/py/test/openCVHelp_test.py @@ -0,0 +1,205 @@ +import math + +import ntcore as nt +import pytest +from photonlibpy.estimation import RotTrlTransform3d, TargetModel +from photonlibpy.estimation.openCVHelp import OpenCVHelp +from photonlibpy.photonCamera import setVersionCheckEnabled +from photonlibpy.simulation import SimCameraProperties, VisionTargetSim +from wpimath.geometry import Pose3d, Rotation3d, Translation3d + + +@pytest.fixture(autouse=True) +def setupCommon() -> None: + nt.NetworkTableInstance.getDefault().startServer() + setVersionCheckEnabled(False) + + +def test_TrlConvert(): + trl = Translation3d(0.75, -0.4, 0.1) + tvec = OpenCVHelp.translationToTVec([trl]) + result = OpenCVHelp.tVecToTranslation(tvec[0]) + + assert result.X() == pytest.approx(trl.X(), 0.005) + assert result.Y() == pytest.approx(trl.Y(), 0.005) + assert result.Z() == pytest.approx(trl.Z(), 0.005) + + +def test_RotConvert(): + rot = Rotation3d(0.5, 1, -1) + rvec = OpenCVHelp.rotationToRVec(rot) + result = OpenCVHelp.rVecToRotation(rvec[0]) + + assert result.X() == pytest.approx(rot.X(), 0.25) + assert result.Y() == pytest.approx(rot.Y(), 0.25) + assert result.Z() == pytest.approx(rot.Z(), 0.25) + + +def test_Projection(): + prop = SimCameraProperties() + + target = VisionTargetSim( + Pose3d(Translation3d(1.0, 0.0, 0.0), Rotation3d(0.0, 0.0, math.pi)), + TargetModel.AprilTag16h5(), + 4774, + ) + + cameraPose = Pose3d(Translation3d(), Rotation3d()) + + camRt = RotTrlTransform3d.makeRelativeTo(cameraPose) + imagePoints = OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices() + ) + + # find circulation (counter/clockwise-ness) + circulation = 0.0 + for i in range(0, len(imagePoints)): + xDiff = imagePoints[(i + 1) % 4][0][0] - imagePoints[i][0][0] + ySum = imagePoints[(i + 1) % 4][0][1] + imagePoints[i][0][1] + circulation += xDiff * ySum + + assert circulation > 0, "2d fiducial points aren't counter-clockwise" + + # TODO Uncomment after OpenCVHelp.undistortPoints is implemented + # # undo projection distortion + # imagePoints = OpenCVHelp.undistortPoints( + # prop.getIntrinsics(), prop.getDistCoeffs(), imagePoints + # ) + # # test projection results after moving camera + # avgCenterRot1 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints)) + # cameraPose = cameraPose + Transform3d(Translation3d(), Rotation3d(0.0, 0.25, 0.25)) + # camRt = RotTrlTransform3d.makeRelativeTo(cameraPose) + # imagePoints = OpenCVHelp.projectPoints( + # prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices() + # ) + # avgCenterRot2 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints)) + + # yaw2d = Rotation2d(avgCenterRot2.Z()) + # pitch2d = Rotation2d(avgCenterRot2.Y()) + # yawDiff = yaw2d - Rotation2d(avgCenterRot1.Z()) + # pitchDiff = pitch2d - Rotation2d(avgCenterRot2.Y()) + # assert yawDiff.radians() < 0.0, "2d points don't follow yaw" + # assert pitchDiff.radians() < 0.0, "2d points don't follow pitch" + + # actualRelation = CameraTargetRelation(cameraPose, target.getPose()) + + # assert actualRelation.camToTargPitch.degrees() == pytest.approx( + # pitchDiff.degrees() + # * math.cos(yaw2d.radians()), # adjust for unaccounted perspective distortion + # abs=0.25, + # ), "2d pitch doesn't match 3d" + # assert actualRelation.camToTargYaw.degrees() == pytest.approx( + # yawDiff.degrees(), abs=0.25 + # ), "2d yaw doesn't match 3d" + + +def test_SolvePNP_SQUARE(): + prop = SimCameraProperties() + + # square AprilTag target + target = VisionTargetSim( + Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)), + TargetModel.AprilTag16h5(), + 4774, + ) + cameraPose = Pose3d(Translation3d(), Rotation3d()) + camRt = RotTrlTransform3d.makeRelativeTo(cameraPose) + + # target relative to camera + relTarget = camRt.applyPose(target.getPose()) + + # simulate solvePNP estimation + targetCorners = OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices() + ) + + pnpSim = OpenCVHelp.solvePNP_Square( + prop.getIntrinsics(), + prop.getDistCoeffs(), + target.getModel().vertices, + targetCorners, + ) + + assert pnpSim is not None + + # check solvePNP estimation accuracy + assert relTarget.rotation().X() == pytest.approx( + pnpSim.best.rotation().X(), abs=0.25 + ) + assert relTarget.rotation().Y() == pytest.approx( + pnpSim.best.rotation().Y(), abs=0.25 + ) + assert relTarget.rotation().Z() == pytest.approx( + pnpSim.best.rotation().Z(), abs=0.25 + ) + + assert relTarget.translation().X() == pytest.approx( + pnpSim.best.translation().X(), abs=0.005 + ) + assert relTarget.translation().Y() == pytest.approx( + pnpSim.best.translation().Y(), abs=0.005 + ) + assert relTarget.translation().Z() == pytest.approx( + pnpSim.best.translation().Z(), abs=0.005 + ) + + +def test_SolvePNP_SQPNP(): + prop = SimCameraProperties() + + # (for targets with arbitrary number of non-colinear points > 2) + target = VisionTargetSim( + Pose3d(Translation3d(5.0, 0.5, 1.0), Rotation3d(0.0, 0.0, math.pi)), + TargetModel( + verts=[ + Translation3d(0.0, 0.0, 0.0), + Translation3d(1.0, 0.0, 0.0), + Translation3d(0.0, 1.0, 0.0), + Translation3d(0.0, 0.0, 1.0), + Translation3d(0.125, 0.25, 0.5), + Translation3d(0.0, 0.0, -1.0), + Translation3d(0.0, -1.0, 0.0), + Translation3d(-1.0, 0.0, 0.0), + ] + ), + 4774, + ) + cameraPose = Pose3d(Translation3d(), Rotation3d()) + camRt = RotTrlTransform3d.makeRelativeTo(cameraPose) + # target relative to camera + relTarget = camRt.applyPose(target.getPose()) + + # simulate solvePNP estimation + targetCorners = OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices() + ) + + pnpSim = OpenCVHelp.solvePNP_SQPNP( + prop.getIntrinsics(), + prop.getDistCoeffs(), + target.getModel().vertices, + targetCorners, + ) + + assert pnpSim is not None + + # check solvePNP estimation accuracy + assert relTarget.rotation().X() == pytest.approx( + pnpSim.best.rotation().X(), abs=0.25 + ) + assert relTarget.rotation().Y() == pytest.approx( + pnpSim.best.rotation().Y(), abs=0.25 + ) + assert relTarget.rotation().Z() == pytest.approx( + pnpSim.best.rotation().Z(), abs=0.25 + ) + + assert relTarget.translation().X() == pytest.approx( + pnpSim.best.translation().X(), abs=0.005 + ) + assert relTarget.translation().Y() == pytest.approx( + pnpSim.best.translation().Y(), abs=0.005 + ) + assert relTarget.translation().Z() == pytest.approx( + pnpSim.best.translation().Z(), abs=0.005 + ) diff --git a/photon-lib/py/test/photonCamera_test.py b/photon-lib/py/test/photonCamera_test.py new file mode 100644 index 000000000..33729cdcd --- /dev/null +++ b/photon-lib/py/test/photonCamera_test.py @@ -0,0 +1,36 @@ +import pytest +from photonlibpy import Packet +from photonlibpy.targeting import PhotonPipelineResult + + +@pytest.fixture(autouse=True) +def setupCommon() -> None: + pass + + +def test_Empty() -> None: + packet = Packet(b"1") + PhotonPipelineResult() + packet.setData(bytes(0)) + PhotonPipelineResult.photonStruct.unpack(packet) + # There is no need for an assert as we are checking + # if this throws an exception (it should not) + + +@pytest.mark.parametrize( + "robotStart, coprocStart, robotRestart, coprocRestart", + [ + [1, 10, 30, 30], + [10, 2, 30, 30], + [10, 10, 30, 30], + # Reboot just the robot + [1, 1, 10, 30], + # Reboot just the coproc + [1, 1, 30, 10], + ], +) +def test_RestartingRobotandCoproc( + robotStart: int, coprocStart: int, robotRestart: int, coprocRestart: int +): + # Python doesn't have a TimeSyncClient so we can't run this yet + pass diff --git a/photon-lib/py/test/simCameraProperties_test.py b/photon-lib/py/test/simCameraProperties_test.py new file mode 100644 index 000000000..7db73a67f --- /dev/null +++ b/photon-lib/py/test/simCameraProperties_test.py @@ -0,0 +1,105 @@ +import math + +import numpy as np +import pytest +from photonlibpy.estimation import RotTrlTransform3d +from photonlibpy.simulation import SimCameraProperties +from wpimath.geometry import Rotation2d, Translation3d + + +@pytest.fixture(autouse=True) +def scp() -> SimCameraProperties: + props = SimCameraProperties() + props.setCalibration(1000, 1000, fovDiag=Rotation2d(math.radians(90.0))) + return props + + +def test_Constructor() -> None: + SimCameraProperties() + with pytest.raises(Exception): + SimCameraProperties("4774") + + +def test_GetPixelYaw(scp) -> None: + rot = scp.getPixelYaw(scp.getResWidth() / 2) + assert rot.degrees() == pytest.approx(0.0, abs=1.0) + rot = scp.getPixelYaw(0.0) + # FOV is square + assert rot.degrees() == pytest.approx(45.0 / math.sqrt(2.0), abs=5.0) + rot = scp.getPixelYaw(scp.getResWidth()) + assert rot.degrees() == pytest.approx(-45.0 / math.sqrt(2.0), abs=5.0) + + +def test_GetPixelPitch(scp) -> None: + rot = scp.getPixelPitch(scp.getResHeight() / 2) + assert rot.degrees() == pytest.approx(0.0, abs=1.0) + rot = scp.getPixelPitch(0.0) + # FOV is square + assert rot.degrees() == pytest.approx(-45.0 / math.sqrt(2.0), abs=5.0) + rot = scp.getPixelPitch(scp.getResHeight()) + assert rot.degrees() == pytest.approx(45.0 / math.sqrt(2.0), abs=5.0) + + +def test_GetPixelRot(scp) -> None: + rot = scp.getPixelRot(np.array([scp.getResWidth() / 2.0, scp.getResHeight() / 2.0])) + assert rot.x_degrees == pytest.approx(0.0, abs=5) + assert rot.y_degrees == pytest.approx(0.0, abs=5) + assert rot.z_degrees == pytest.approx(0.0, abs=5) + rot = scp.getPixelRot(np.array([0.0, 0.0])) + assert rot.x_degrees == pytest.approx(0.0, abs=5) + assert rot.y_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5) + assert rot.z_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5) + rot = scp.getPixelRot(np.array([scp.getResWidth(), scp.getResHeight()])) + assert rot.x_degrees == pytest.approx(0.0, abs=5) + assert rot.y_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5) + assert rot.z_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5) + + +def test_GetCorrectedPixelRot(scp) -> None: + rot = scp.getCorrectedPixelRot( + np.array([scp.getResWidth() / 2.0, scp.getResHeight() / 2.0]) + ) + assert rot.x_degrees == pytest.approx(0.0, abs=5) + assert rot.y_degrees == pytest.approx(0.0, abs=5) + assert rot.z_degrees == pytest.approx(0.0, abs=5) + rot = scp.getCorrectedPixelRot(np.array([0.0, 0.0])) + assert rot.x_degrees == pytest.approx(0.0, abs=5) + assert rot.y_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5) + assert rot.z_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5) + rot = scp.getCorrectedPixelRot(np.array([scp.getResWidth(), scp.getResHeight()])) + assert rot.x_degrees == pytest.approx(0.0, abs=5) + assert rot.y_degrees == pytest.approx(45.0 / math.sqrt(2.0), abs=5) + assert rot.z_degrees == pytest.approx(-45.0 / math.sqrt(2.0), abs=5) + + +def test_GetVisibleLine(scp) -> None: + camRt = RotTrlTransform3d() + a = Translation3d() + b = Translation3d() + retval = scp.getVisibleLine(camRt, a, b) + assert retval == (None, None) + + a = Translation3d(-5.0, -0.1, 0) + b = Translation3d(5.0, 0.1, 0) + retval = scp.getVisibleLine(camRt, a, b) + assert retval == (0.5, 0.5) + + +def test_EstPixelNoise(scp) -> None: + with pytest.raises(Exception): + scp.test_EstPixelNoise(np.array([0, 0])) + with pytest.raises(Exception): + scp.test_EstPixelNoise(np.array([[0], [0]])) + + pts = np.array([[[0, 0]], [[0, 0]]]) + + # No noise parameters set + noisy = scp.estPixelNoise(pts) + for n, p in zip(noisy, pts): + assert n.all() == p.all() + + # Noise parameters set + scp.setCalibError(1.0, 1.0) + noisy = scp.estPixelNoise(pts) + for n, p in zip(noisy, pts): + assert n.any() != p.any() diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index c9f4757e2..25acb52f4 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -20,13 +20,11 @@ from wpimath.units import feetToMeters, meters @pytest.fixture(autouse=True) def setupCommon() -> None: - nt.NetworkTableInstance.getDefault().startServer() setVersionCheckEnabled(False) def test_VisibilityCupidShuffle() -> None: - targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) visionSysSim = VisionSystemSim("Test") @@ -84,7 +82,6 @@ def test_VisibilityCupidShuffle() -> None: def test_NotVisibleVert1() -> None: - targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) visionSysSim = VisionSystemSim("Test") @@ -112,7 +109,6 @@ def test_NotVisibleVert1() -> None: def test_NotVisibleVert2() -> None: - targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi)) robotToCamera = Transform3d( @@ -162,7 +158,6 @@ def test_NotVisibleTargetSize() -> None: def test_NotVisibleTooFarLeds() -> None: - targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi)) visionSysSim = VisionSystemSim("Test") @@ -190,7 +185,6 @@ def test_NotVisibleTooFarLeds() -> None: "expected_yaw", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23] ) def test_YawAngles(expected_yaw) -> None: - targetPose = Pose3d( Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 3.0 * math.pi / 4.0) ) @@ -220,7 +214,6 @@ def test_YawAngles(expected_yaw) -> None: "expected_pitch", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23] ) def test_PitchAngles(expected_pitch) -> None: - targetPose = Pose3d( Translation3d(15.98, 0.0, 0.0), Rotation3d(0, 0, 3.0 * math.pi / 4.0) ) @@ -482,3 +475,79 @@ def test_PoseEstimation() -> None: assert pose2.Y() == pytest.approx(robotPose.Y(), abs=0.01) assert pose2.Z() == pytest.approx(0.0, abs=0.01) assert pose2.rotation().Z() == pytest.approx(math.radians(5.0), abs=0.01) + + +def test_PoseEstimationRotated() -> None: + robotToCamera = Transform3d( + Translation3d(6.0 * 0.0254, 6.0 * 0.0254, 6.0 * 0.0254), + Rotation3d(0.0, math.radians(-30.0), math.radians(25.5)), + ) + + visionSysSim = VisionSystemSim("Test") + camera = PhotonCamera("camera") + cameraSim = PhotonCameraSim(camera) + visionSysSim.addCamera(cameraSim, robotToCamera) + cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(90.0)) + cameraSim.setMinTargetAreaPixels(20.0) + + tagList: list[AprilTag] = [] + at0 = AprilTag() + at0.ID = 0 + at0.pose = Pose3d(12.0, 3.0, 1.0, Rotation3d(0.0, 0.0, math.pi)) + tagList.append(at0) + at1 = AprilTag() + at1.ID = 1 + at1.pose = Pose3d(12.0, 1.0, -1.0, Rotation3d(0.0, 0.0, math.pi)) + tagList.append(at1) + at2 = AprilTag() + at2.ID = 2 + at2.pose = Pose3d(11.0, 0.0, 2.0, Rotation3d(0.0, 0.0, math.pi)) + tagList.append(at2) + + fieldLength: meters = 54.0 + fieldWidth: meters = 27.0 + layout = AprilTagFieldLayout(tagList, fieldLength, fieldWidth) + robotPose = Pose2d(Translation2d(5.0, 1.0), Rotation2d.fromDegrees(-5.0)) + visionSysSim.addVisionTargets( + [VisionTargetSim(tagList[0].pose, TargetModel.AprilTag36h11(), 0)] + ) + + visionSysSim.update(robotPose) + + camEigen = cameraSim.prop.getIntrinsics() + distEigen = cameraSim.prop.getDistCoeffs() + + camResults = camera.getLatestResult() + targets = camResults.getTargets() + results = VisionEstimation.estimateCamPosePNP( + camEigen, distEigen, targets, layout, TargetModel.AprilTag36h11() + ) + assert results is not None + pose: Pose3d = Pose3d() + results.best + pose = pose.transformBy(robotToCamera.inverse()) + assert pose.X() == pytest.approx(5.0, abs=0.01) + assert pose.Y() == pytest.approx(1.0, abs=0.01) + assert pose.Z() == pytest.approx(0.0, abs=0.01) + assert pose.rotation().Z() == pytest.approx(math.radians(-5.0), abs=0.01) + + visionSysSim.addVisionTargets( + [VisionTargetSim(tagList[1].pose, TargetModel.AprilTag36h11(), 1)] + ) + visionSysSim.addVisionTargets( + [VisionTargetSim(tagList[2].pose, TargetModel.AprilTag36h11(), 2)] + ) + visionSysSim.update(robotPose) + + camResults2 = camera.getLatestResult() + targets2 = camResults2.getTargets() + results2 = VisionEstimation.estimateCamPosePNP( + camEigen, distEigen, targets2, layout, TargetModel.AprilTag36h11() + ) + assert results2 is not None + pose2 = Pose3d() + results2.best + pose2 = pose2.transformBy(robotToCamera.inverse()) + + assert pose2.X() == pytest.approx(robotPose.X(), abs=0.01) + assert pose2.Y() == pytest.approx(robotPose.Y(), abs=0.01) + assert pose2.Z() == pytest.approx(0.0, abs=0.01) + assert pose2.rotation().Z() == pytest.approx(math.radians(-5.0), abs=0.01)