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

[photon-lib] Cleanup simulation Rotation3d usage (#982)

Browse Source
This commit is contained in:
amquake
2023-10-25 18:19:48 -07:00
committed by GitHub
parent 63147786b9
commit 5b2be119e7
4 changed files with 29 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -181,20 +181,13 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
new Transform3d(
new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
// match expected AprilTag coordinate system
// TODO cleanup coordinate systems in wpilib 2024
var apriltagTrl =
CoordinateSystem.convert(
camToTag.getTranslation(), CoordinateSystem.NWU(), CoordinateSystem.EDN());
var apriltagRot =
CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
.plus(
CoordinateSystem.convert(
camToTag.getRotation(),
CoordinateSystem.NWU(),
CoordinateSystem.EDN()));
apriltagRot = new Rotation3d(0, Math.PI, 0).plus(apriltagRot);
camToTag = new Transform3d(apriltagTrl, apriltagRot);
camToTag =
CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
// (AprilTag expects Z axis going into tag)
camToTag =
new Transform3d(
camToTag.getTranslation(),
new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation()));
tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0);
}
}