From 8bc3b04f5b9becefe6fb0c7e99497495a3fe9715 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 26 Oct 2022 22:20:08 -0700 Subject: [PATCH] [wpimath] Make ComputerVisionUtil use 3D geometry classes (#4528) Closes #4189. --- .../wpi/first/math/ComputerVisionUtil.java | 62 +++++++------- .../main/native/cpp/ComputerVisionUtil.cpp | 37 +++++---- .../native/include/frc/ComputerVisionUtil.h | 40 ++++----- .../first/math/ComputerVisionUtilTest.java | 83 ++++++++++--------- .../native/cpp/ComputerVisionUtilTest.cpp | 75 +++++++++-------- 5 files changed, 159 insertions(+), 138 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java index e297634127..61c9b1521a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java @@ -4,10 +4,11 @@ package edu.wpi.first.math; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; public final class ComputerVisionUtil { private ComputerVisionUtil() { @@ -59,31 +60,34 @@ public final class ComputerVisionUtil { * @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and * Photon returns CW-positive. * @param gyroAngle The current robot gyro angle, likely from odometry. - * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param fieldToTarget A Pose3d representing the target position in the field coordinate system. * @param cameraToRobot The position of the robot relative to the camera. If the camera was * mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be - * Transform2d(Units.inchesToMeters(3), Units.inchesToMeters(0), Units.degreesToRadians(0)). + * new Transform3d(Units.inchesToMeters(3), Units.inchesToMeters(0), Units.inchesToMeters(0), + * new Rotation3d(Units.degreesToRadians(0))). * @return The position of the robot in the field. */ - public static Pose2d estimateFieldToRobot( + public static Pose3d estimateFieldToRobot( double cameraHeightMeters, double targetHeightMeters, double cameraPitchRadians, double targetPitchRadians, Rotation2d targetYaw, Rotation2d gyroAngle, - Pose2d fieldToTarget, - Transform2d cameraToRobot) { + Pose3d fieldToTarget, + Transform3d cameraToRobot) { + final var distanceAlongGround = + calculateDistanceToTarget( + cameraHeightMeters, + targetHeightMeters, + cameraPitchRadians, + targetPitchRadians, + targetYaw.getRadians()); + final var range = Math.hypot(distanceAlongGround, targetHeightMeters - cameraHeightMeters); return estimateFieldToRobot( estimateCameraToTarget( - new Translation2d( - calculateDistanceToTarget( - cameraHeightMeters, - targetHeightMeters, - cameraPitchRadians, - targetPitchRadians, - targetYaw.getRadians()), - targetYaw), + new Translation3d( + range, new Rotation3d(0.0, targetPitchRadians, targetYaw.getRadians())), fieldToTarget, gyroAngle), fieldToTarget, @@ -99,33 +103,35 @@ public final class ComputerVisionUtil { * @param fieldToTarget The position of the target in the field. * @param cameraToRobot The position of the robot relative to the camera. If the camera was * mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be - * Transform2d(Units.inchesToMeters(3), Units.inchesToMeters(0), Units.degreesToRadians(0)). + * new Transform3d(Units.inchesToMeters(3), Units.inchesToMeters(0), Units.inchesToMeters(0), + * new Rotation3d(Units.degreesToRadians(0))). * @return The position of the robot in the field. */ - public static Pose2d estimateFieldToRobot( - Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) { + public static Pose3d estimateFieldToRobot( + Transform3d cameraToTarget, Pose3d fieldToTarget, Transform3d cameraToRobot) { return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot); } /** - * Estimates a {@link Transform2d} that maps the camera position to the target position, using the + * Estimates a {@link Transform3d} that maps the camera position to the target position, using the * robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system * -- that is, it should read zero degrees when pointed towards the opposing alliance station, and * increase as the robot rotates CCW. * - * @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target + * @param cameraToTargetTranslation A Translation3d that encodes the x/y position of the target * relative to the camera. - * @param fieldToTarget A Pose2d representing the target position in the field coordinate system. + * @param fieldToTarget A Pose3d representing the target position in the field coordinate system. * @param gyroAngle The current robot gyro angle, likely from odometry. - * @return A Transform2d that takes us from the camera to the target. + * @return A Transform3d that takes us from the camera to the target. */ - public static Transform2d estimateCameraToTarget( - Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) { + public static Transform3d estimateCameraToTarget( + Translation3d cameraToTargetTranslation, Pose3d fieldToTarget, Rotation2d gyroAngle) { // Map our camera at the origin out to our target, in the robot reference // frame. Gyro angle is needed because there's a circle of possible camera // poses for which the camera has the same yaw from camera to target. - return new Transform2d( - cameraToTargetTranslation, gyroAngle.unaryMinus().minus(fieldToTarget.getRotation())); + return new Transform3d( + cameraToTargetTranslation, + new Rotation3d(0.0, 0.0, -gyroAngle.getRadians()).minus(fieldToTarget.getRotation())); } /** @@ -137,7 +143,7 @@ public final class ComputerVisionUtil { * @param fieldToTarget The position of the target in the field. * @return The position of the camera in the field. */ - public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) { + public static Pose3d estimateFieldToCamera(Transform3d cameraToTarget, Pose3d fieldToTarget) { var targetToCamera = cameraToTarget.inverse(); return fieldToTarget.transformBy(targetToCamera); } diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp index 079b11b9c2..f8c2640c3f 100644 --- a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp +++ b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp @@ -4,6 +4,7 @@ #include "frc/ComputerVisionUtil.h" +#include "frc/geometry/Rotation3d.h" #include "units/math.h" namespace frc { @@ -18,40 +19,44 @@ units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, units::math::cos(targetYaw)); } -frc::Pose2d EstimateFieldToRobot( +frc::Pose3d EstimateFieldToRobot( units::meter_t cameraHeight, units::meter_t targetHeight, units::radian_t cameraPitch, units::radian_t targetPitch, const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, - const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) { + const frc::Pose3d& fieldToTarget, const frc::Transform3d& cameraToRobot) { + auto distanceAlongGround = + CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch, + targetPitch, targetYaw.Radians()); + auto range = + units::math::hypot(distanceAlongGround, targetHeight - cameraHeight); return EstimateFieldToRobot( EstimateCameraToTarget( - frc::Translation2d{ - CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch, - targetPitch, targetYaw.Radians()), - targetYaw}, + Translation3d{range, + Rotation3d{0_rad, targetPitch, targetYaw.Radians()}}, fieldToTarget, gyroAngle), fieldToTarget, cameraToRobot); } -frc::Pose2d EstimateFieldToRobot(const frc::Transform2d& cameraToTarget, - const frc::Pose2d& fieldToTarget, - const frc::Transform2d& cameraToRobot) { +frc::Pose3d EstimateFieldToRobot(const frc::Transform3d& cameraToTarget, + const frc::Pose3d& fieldToTarget, + const frc::Transform3d& cameraToRobot) { return EstimateFieldToCamera(cameraToTarget, fieldToTarget) .TransformBy(cameraToRobot); } -frc::Transform2d EstimateCameraToTarget( - const frc::Translation2d& cameraToTargetTranslation, - const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) { +frc::Transform3d EstimateCameraToTarget( + const frc::Translation3d& cameraToTargetTranslation, + const frc::Pose3d& fieldToTarget, const frc::Rotation2d& gyroAngle) { // Map our camera at the origin out to our target, in the robot reference // frame. Gyro angle is needed because there's a circle of possible camera // poses for which the camera has the same yaw from camera to target. - return frc::Transform2d{cameraToTargetTranslation, - -gyroAngle - fieldToTarget.Rotation()}; + return Transform3d{cameraToTargetTranslation, + Rotation3d{0_rad, 0_rad, -gyroAngle.Radians()} - + fieldToTarget.Rotation()}; } -frc::Pose2d EstimateFieldToCamera(const frc::Transform2d& cameraToTarget, - const frc::Pose2d& fieldToTarget) { +frc::Pose3d EstimateFieldToCamera(const frc::Transform3d& cameraToTarget, + const frc::Pose3d& fieldToTarget) { auto targetToCamera = cameraToTarget.Inverse(); return fieldToTarget.TransformBy(targetToCamera); } diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h index 6ee9225232..d2eb4f218b 100644 --- a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h +++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h @@ -6,10 +6,10 @@ #include -#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Pose3d.h" #include "frc/geometry/Rotation2d.h" -#include "frc/geometry/Transform2d.h" -#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Transform3d.h" +#include "frc/geometry/Translation3d.h" #include "units/angle.h" #include "units/length.h" @@ -53,20 +53,20 @@ units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight, * @param targetYaw The observed yaw of the target. Note that this *must* be * CCW-positive, and Photon returns CW-positive. * @param gyroAngle The current robot gyro angle, likely from odometry. - * @param fieldToTarget A Pose2d representing the target position in the field + * @param fieldToTarget A Pose3d representing the target position in the field * coordinate system. * @param cameraToRobot The position of the robot relative to the camera. If the * camera was mounted 3 inches behind the "origin" (usually * physical center) of the robot, this would be - * frc::Transform2d{3_in, 0_in, 0_deg}. + * frc::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}. * @return The position of the robot in the field. */ WPILIB_DLLEXPORT -frc::Pose2d EstimateFieldToRobot( +frc::Pose3d EstimateFieldToRobot( units::meter_t cameraHeight, units::meter_t targetHeight, units::radian_t cameraPitch, units::radian_t targetPitch, const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle, - const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot); + const frc::Pose3d& fieldToTarget, const frc::Transform3d& cameraToRobot); /** * Estimates the pose of the robot in the field coordinate system, given the @@ -78,33 +78,33 @@ frc::Pose2d EstimateFieldToRobot( * @param cameraToRobot The position of the robot relative to the camera. If * the camera was mounted 3 inches behind the "origin" * (usually physical center) of the robot, this would be - * frc::Transform2d{3_in, 0_in, 0_deg}. + * frc::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}. * @return The position of the robot in the field. */ WPILIB_DLLEXPORT -frc::Pose2d EstimateFieldToRobot(const frc::Transform2d& cameraToTarget, - const frc::Pose2d& fieldToTarget, - const frc::Transform2d& cameraToRobot); +frc::Pose3d EstimateFieldToRobot(const frc::Transform3d& cameraToTarget, + const frc::Pose3d& fieldToTarget, + const frc::Transform3d& cameraToRobot); /** - * Estimates a Transform2d that maps the camera position to the target position, + * Estimates a Transform3d that maps the camera position to the target position, * using the robot's gyro. Note that the gyro angle provided *must* line up with * the field coordinate system -- that is, it should read zero degrees when * pointed towards the opposing alliance station, and increase as the robot * rotates CCW. * - * @param cameraToTargetTranslation A Translation2d that encodes the x/y + * @param cameraToTargetTranslation A Translation3d that encodes the x/y * position of the target relative to the * camera. - * @param fieldToTarget A Pose2d representing the target position in the field + * @param fieldToTarget A Pose3d representing the target position in the field * coordinate system. * @param gyroAngle The current robot gyro angle, likely from odometry. - * @return A Transform2d that takes us from the camera to the target. + * @return A Transform3d that takes us from the camera to the target. */ WPILIB_DLLEXPORT -frc::Transform2d EstimateCameraToTarget( - const frc::Translation2d& cameraToTargetTranslation, - const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle); +frc::Transform3d EstimateCameraToTarget( + const frc::Translation3d& cameraToTargetTranslation, + const frc::Pose3d& fieldToTarget, const frc::Rotation2d& gyroAngle); /** * Estimates the pose of the camera in the field coordinate system, given the @@ -117,7 +117,7 @@ frc::Transform2d EstimateCameraToTarget( * @return The position of the camera in the field. */ WPILIB_DLLEXPORT -frc::Pose2d EstimateFieldToCamera(const frc::Transform2d& cameraToTarget, - const frc::Pose2d& fieldToTarget); +frc::Pose3d EstimateFieldToCamera(const frc::Transform3d& cameraToTarget, + const frc::Pose3d& fieldToTarget); } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java index ea3f8d58eb..5a6d949a82 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java @@ -4,10 +4,11 @@ package edu.wpi.first.math; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Test; @@ -15,69 +16,68 @@ import org.junit.jupiter.api.Test; class ComputerVisionUtilTest { @Test void testCalculateDistanceToTarget() { - var camHeight = 1; + var cameraHeight = 1; var targetHeight = 3; - var camPitch = Units.degreesToRadians(0); + var cameraPitch = Units.degreesToRadians(0); var targetPitch = Units.degreesToRadians(30); var targetYaw = Units.degreesToRadians(0); - var dist = + + var distanceAlongGround = ComputerVisionUtil.calculateDistanceToTarget( - camHeight, targetHeight, camPitch, targetPitch, targetYaw); + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + Assertions.assertEquals(3.464, distanceAlongGround, 0.01); - Assertions.assertEquals(3.464, dist, 0.01); - - camHeight = 1; + cameraHeight = 1; targetHeight = 2; - camPitch = Units.degreesToRadians(20); + cameraPitch = Units.degreesToRadians(20); targetPitch = Units.degreesToRadians(-10); - dist = + distanceAlongGround = ComputerVisionUtil.calculateDistanceToTarget( - camHeight, targetHeight, camPitch, targetPitch, targetYaw); - Assertions.assertEquals(5.671, dist, 0.01); + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + Assertions.assertEquals(5.671, distanceAlongGround, 0.01); - camHeight = 3; + cameraHeight = 3; targetHeight = 1; - camPitch = Units.degreesToRadians(0); + cameraPitch = Units.degreesToRadians(0); targetPitch = Units.degreesToRadians(-30); - dist = + distanceAlongGround = ComputerVisionUtil.calculateDistanceToTarget( - camHeight, targetHeight, camPitch, targetPitch, targetYaw); + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + Assertions.assertEquals(3.464, distanceAlongGround, 0.01); - Assertions.assertEquals(3.464, dist, 0.01); - - camHeight = 1; + cameraHeight = 1; targetHeight = 3; - camPitch = Units.degreesToRadians(0); + cameraPitch = Units.degreesToRadians(0); targetPitch = Units.degreesToRadians(30); targetYaw = Units.degreesToRadians(30); - dist = + distanceAlongGround = ComputerVisionUtil.calculateDistanceToTarget( - camHeight, targetHeight, camPitch, targetPitch, targetYaw); - - Assertions.assertEquals(4, dist, 0.01); + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + Assertions.assertEquals(4, distanceAlongGround, 0.01); } @Test void testEstimateFieldToRobot() { - var camHeight = 1; + var cameraHeight = 1; var targetHeight = 3; - var camPitch = 0; + var cameraPitch = 0; var targetPitch = Units.degreesToRadians(30); var targetYaw = new Rotation2d(); var gyroAngle = new Rotation2d(); - var fieldToTarget = new Pose2d(); - var cameraToRobot = new Transform2d(); + var fieldToTarget = new Pose3d(); + var cameraToRobot = new Transform3d(); + var distanceAlongGround = + ComputerVisionUtil.calculateDistanceToTarget( + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw.getRadians()); + var range = Math.hypot(distanceAlongGround, targetHeight - cameraHeight); var fieldToRobot = ComputerVisionUtil.estimateFieldToRobot( ComputerVisionUtil.estimateCameraToTarget( - new Translation2d( - ComputerVisionUtil.calculateDistanceToTarget( - camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()), - targetYaw), + new Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())), fieldToTarget, gyroAngle), fieldToTarget, @@ -85,17 +85,19 @@ class ComputerVisionUtilTest { Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1); Assertions.assertEquals(0, fieldToRobot.getY(), 0.1); - Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1); + Assertions.assertEquals(2.0, fieldToRobot.getZ(), 0.1); + Assertions.assertEquals(0, Units.radiansToDegrees(fieldToRobot.getRotation().getZ()), 0.1); gyroAngle = Rotation2d.fromDegrees(-30); + distanceAlongGround = + ComputerVisionUtil.calculateDistanceToTarget( + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw.getRadians()); + range = Math.hypot(distanceAlongGround, targetHeight - cameraHeight); fieldToRobot = ComputerVisionUtil.estimateFieldToRobot( ComputerVisionUtil.estimateCameraToTarget( - new Translation2d( - ComputerVisionUtil.calculateDistanceToTarget( - camHeight, targetHeight, camPitch, targetPitch, targetYaw.getRadians()), - targetYaw), + new Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())), fieldToTarget, gyroAngle), fieldToTarget, @@ -103,6 +105,7 @@ class ComputerVisionUtilTest { Assertions.assertEquals(-3.0, fieldToRobot.getX(), 0.1); Assertions.assertEquals(1.732, fieldToRobot.getY(), 0.1); - Assertions.assertEquals(-30.0, fieldToRobot.getRotation().getDegrees(), 0.1); + Assertions.assertEquals(2.0, fieldToRobot.getZ(), 0.1); + Assertions.assertEquals(-30.0, Units.radiansToDegrees(fieldToRobot.getRotation().getZ()), 0.1); } } diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp index c10baef785..209407d383 100644 --- a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp +++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp @@ -11,80 +11,87 @@ #include "units/length.h" TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) { - auto camHeight = 1_m; + auto cameraHeight = 1_m; auto targetHeight = 3_m; - auto camPitch = 0_deg; + auto cameraPitch = 0_deg; auto targetPitch = 30_deg; auto targetYaw = 0_deg; - auto dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, - targetPitch, targetYaw); - EXPECT_NEAR(3.464, dist.value(), 0.01); + auto distanceAlongGround = frc::CalculateDistanceToTarget( + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01); - camHeight = 1_m; + cameraHeight = 1_m; targetHeight = 2_m; - camPitch = 20_deg; + cameraPitch = 20_deg; targetPitch = -10_deg; - dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, - targetPitch, targetYaw); - EXPECT_NEAR(5.671, dist.value(), 0.01); + distanceAlongGround = frc::CalculateDistanceToTarget( + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + EXPECT_NEAR(5.671, distanceAlongGround.value(), 0.01); - camHeight = 3_m; + cameraHeight = 3_m; targetHeight = 1_m; - camPitch = 0_deg; + cameraPitch = 0_deg; targetPitch = -30_deg; - dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, - targetPitch, targetYaw); - EXPECT_NEAR(3.464, dist.value(), 0.01); + distanceAlongGround = frc::CalculateDistanceToTarget( + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01); - camHeight = 1_m; + cameraHeight = 1_m; targetHeight = 3_m; - camPitch = 0_deg; + cameraPitch = 0_deg; targetPitch = 30_deg; targetYaw = 30_deg; - dist = frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, - targetPitch, targetYaw); - EXPECT_NEAR(4, dist.value(), 0.01); + distanceAlongGround = frc::CalculateDistanceToTarget( + cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw); + EXPECT_NEAR(4, distanceAlongGround.value(), 0.01); } TEST(ComputerVisionUtilTest, EstimateFieldToRobot) { - auto camHeight = 1_m; + auto cameraHeight = 1_m; auto targetHeight = 3_m; - auto camPitch = 0_deg; + auto cameraPitch = 0_deg; auto targetPitch = 30_deg; frc::Rotation2d targetYaw; frc::Rotation2d gyroAngle; - frc::Pose2d fieldToTarget; - frc::Transform2d cameraToRobot; + frc::Pose3d fieldToTarget; + frc::Transform3d cameraToRobot; + auto distanceAlongGround = + frc::CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch, + targetPitch, targetYaw.Radians()); + auto range = + units::math::hypot(distanceAlongGround, targetHeight - cameraHeight); auto fieldToRobot = frc::EstimateFieldToRobot( frc::EstimateCameraToTarget( - frc::Translation2d{ - frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, - targetPitch, targetYaw.Radians()), - targetYaw}, + frc::Translation3d{ + range, frc::Rotation3d{0_rad, targetPitch, targetYaw.Radians()}}, fieldToTarget, gyroAngle), fieldToTarget, cameraToRobot); EXPECT_NEAR(-3.464, fieldToRobot.X().value(), 0.1); EXPECT_NEAR(0, fieldToRobot.Y().value(), 0.1); - EXPECT_NEAR(0, fieldToRobot.Rotation().Degrees().value(), 0.1); + EXPECT_NEAR(2.0, fieldToRobot.Z().value(), 0.1); + EXPECT_NEAR(0, fieldToRobot.Rotation().Z().value(), 0.1); gyroAngle = -30_deg; + distanceAlongGround = + frc::CalculateDistanceToTarget(cameraHeight, targetHeight, cameraPitch, + targetPitch, targetYaw.Radians()); + range = units::math::hypot(distanceAlongGround, targetHeight - cameraHeight); fieldToRobot = frc::EstimateFieldToRobot( frc::EstimateCameraToTarget( - frc::Translation2d{ - frc::CalculateDistanceToTarget(camHeight, targetHeight, camPitch, - targetPitch, targetYaw.Radians()), - targetYaw}, + frc::Translation3d{ + range, frc::Rotation3d{0_rad, targetPitch, targetYaw.Radians()}}, fieldToTarget, gyroAngle), fieldToTarget, cameraToRobot); EXPECT_NEAR(-3.0, fieldToRobot.X().value(), 0.1); EXPECT_NEAR(1.732, fieldToRobot.Y().value(), 0.1); - EXPECT_NEAR(-30.0, fieldToRobot.Rotation().Degrees().value(), 0.1); + EXPECT_NEAR(2.0, fieldToRobot.Z().value(), 0.1); + EXPECT_NEAR(-30.0, units::degree_t{fieldToRobot.Rotation().Z()}.value(), 0.1); }