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 61c9b1521a..77c4abc809 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
@@ -5,10 +5,7 @@
package edu.wpi.first.math;
import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-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() {
@@ -16,135 +13,26 @@ public final class ComputerVisionUtil {
}
/**
- * Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates
- * range to a target using the target's elevation. This method can produce more stable results
- * than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method
- * requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and
- * for there to exist a height differential between goal and camera. The larger this differential,
- * the more accurate the distance estimate will be.
+ * Returns the robot's pose in the field coordinate system given an object's field-relative pose,
+ * the transformation from the camera's pose to the object's pose (obtained via computer vision),
+ * and the transformation from the robot's pose to the camera's pose.
*
- * Units can be converted using the {@link edu.wpi.first.math.util.Units} class.
+ *
The object could be a target or a fiducial marker.
*
- * @param cameraHeightMeters The physical height of the camera off the floor in meters.
- * @param targetHeightMeters The physical height of the target off the floor in meters. This
- * should be the height of whatever is being targeted (i.e. if the targeting region is set to
- * top, this should be the height of the top of the target).
- * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
- * Positive values up.
- * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
- * values up.
- * @param targetYawRadians The yaw of the target in the camera's lens in radians.
- * @return The estimated distance to the target in meters.
+ * @param objectInField An object's field-relative pose.
+ * @param cameraToObject The transformation from the camera's pose to the object's pose. This
+ * comes from computer vision.
+ * @param robotToCamera The transformation from the robot's pose to the camera's pose. This can
+ * either be a constant for a rigidly mounted camera, or variable if the camera is mounted to
+ * a turret. If the camera was mounted 3 inches behind the "origin" (usually physical center)
+ * of the robot, this would be new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0, new
+ * Rotation3d()).
+ * @return The robot's field-relative pose.
*/
- public static double calculateDistanceToTarget(
- double cameraHeightMeters,
- double targetHeightMeters,
- double cameraPitchRadians,
- double targetPitchRadians,
- double targetYawRadians) {
- return (targetHeightMeters - cameraHeightMeters)
- / (Math.tan(cameraPitchRadians + targetPitchRadians) * Math.cos(targetYawRadians));
- }
-
- /**
- * Estimate the position of the robot in the field.
- *
- * @param cameraHeightMeters The physical height of the camera off the floor in meters.
- * @param targetHeightMeters The physical height of the target off the floor in meters. This
- * should be the height of whatever is being targeted (i.e. if the targeting region is set to
- * top, this should be the height of the top of the target).
- * @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
- * Positive values up.
- * @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
- * values up.
- * @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 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
- * 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 Pose3d estimateFieldToRobot(
- double cameraHeightMeters,
- double targetHeightMeters,
- double cameraPitchRadians,
- double targetPitchRadians,
- Rotation2d targetYaw,
- Rotation2d gyroAngle,
- 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 Translation3d(
- range, new Rotation3d(0.0, targetPitchRadians, targetYaw.getRadians())),
- fieldToTarget,
- gyroAngle),
- fieldToTarget,
- cameraToRobot);
- }
-
- /**
- * Estimates the pose of the robot in the field coordinate system, given the position of the
- * target relative to the camera, the target relative to the field, and the robot relative to the
- * camera.
- *
- * @param cameraToTarget The position of the target relative to the camera.
- * @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
- * 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 Pose3d estimateFieldToRobot(
- Transform3d cameraToTarget, Pose3d fieldToTarget, Transform3d cameraToRobot) {
- return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot);
- }
-
- /**
- * 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 Translation3d that encodes the x/y position of the target
- * relative to the camera.
- * @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 Transform3d that takes us from the camera to the target.
- */
- 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 Transform3d(
- cameraToTargetTranslation,
- new Rotation3d(0.0, 0.0, -gyroAngle.getRadians()).minus(fieldToTarget.getRotation()));
- }
-
- /**
- * Estimates the pose of the camera in the field coordinate system, given the position of the
- * target relative to the camera, and the target relative to the field. This *only* tracks the
- * position of the camera, not the position of the robot itself.
- *
- * @param cameraToTarget The position of the target relative to the camera.
- * @param fieldToTarget The position of the target in the field.
- * @return The position of the camera in the field.
- */
- public static Pose3d estimateFieldToCamera(Transform3d cameraToTarget, Pose3d fieldToTarget) {
- var targetToCamera = cameraToTarget.inverse();
- return fieldToTarget.transformBy(targetToCamera);
+ public static Pose3d objectToRobotPose(
+ Pose3d objectInField, Transform3d cameraToObject, Transform3d robotToCamera) {
+ final var objectToCamera = cameraToObject.inverse();
+ final var cameraToRobot = robotToCamera.inverse();
+ return objectInField.plus(objectToCamera).plus(cameraToRobot);
}
}
diff --git a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
index f8c2640c3f..97968dce90 100644
--- a/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
+++ b/wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
@@ -4,61 +4,16 @@
#include "frc/ComputerVisionUtil.h"
-#include "frc/geometry/Rotation3d.h"
#include "units/math.h"
namespace frc {
-units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
- units::meter_t targetHeight,
- units::radian_t cameraPitch,
- units::radian_t targetPitch,
- units::radian_t targetYaw) {
- return (targetHeight - cameraHeight) /
- (units::math::tan(cameraPitch + targetPitch) *
- units::math::cos(targetYaw));
-}
-
-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::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(
- Translation3d{range,
- Rotation3d{0_rad, targetPitch, targetYaw.Radians()}},
- fieldToTarget, gyroAngle),
- fieldToTarget, cameraToRobot);
-}
-
-frc::Pose3d EstimateFieldToRobot(const frc::Transform3d& cameraToTarget,
- const frc::Pose3d& fieldToTarget,
- const frc::Transform3d& cameraToRobot) {
- return EstimateFieldToCamera(cameraToTarget, fieldToTarget)
- .TransformBy(cameraToRobot);
-}
-
-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 Transform3d{cameraToTargetTranslation,
- Rotation3d{0_rad, 0_rad, -gyroAngle.Radians()} -
- fieldToTarget.Rotation()};
-}
-
-frc::Pose3d EstimateFieldToCamera(const frc::Transform3d& cameraToTarget,
- const frc::Pose3d& fieldToTarget) {
- auto targetToCamera = cameraToTarget.Inverse();
- return fieldToTarget.TransformBy(targetToCamera);
+frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+ const frc::Transform3d& cameraToObject,
+ const frc::Transform3d& robotToCamera) {
+ const auto objectToCamera = cameraToObject.Inverse();
+ const auto cameraToRobot = robotToCamera.Inverse();
+ return objectInField + objectToCamera + cameraToRobot;
}
} // namespace frc
diff --git a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
index d2eb4f218b..8744f46c24 100644
--- a/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
+++ b/wpimath/src/main/native/include/frc/ComputerVisionUtil.h
@@ -7,117 +7,31 @@
#include
#include "frc/geometry/Pose3d.h"
-#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Transform3d.h"
-#include "frc/geometry/Translation3d.h"
-#include "units/angle.h"
-#include "units/length.h"
namespace frc {
/**
- * Algorithm from
- * https://docs.limelightvision.io/en/latest/cs_estimating_distance.html
- * Estimates range to a target using the target's elevation. This method can
- * produce more stable results than SolvePNP when well tuned, if the full 6d
- * robot pose is not required.
+ * Returns the robot's pose in the field coordinate system given an object's
+ * field-relative pose, the transformation from the camera's pose to the
+ * object's pose (obtained via computer vision), and the transformation from the
+ * robot's pose to the camera's pose.
*
- * @param cameraHeight The height of the camera off the floor.
- * @param targetHeight The height of the target off the floor.
- * @param cameraPitch The pitch of the camera from the horizontal plane.
- * Positive values up.
- * @param targetPitch The pitch of the target in the camera's lens. Positive
- * values up.
- * @param targetYaw The yaw of the target in the camera's lens.
- * @return The estimated distance to the target.
+ * The object could be a target or a fiducial marker.
+ *
+ * @param objectInField An object's field-relative pose.
+ * @param cameraToObject The transformation from the camera's pose to the
+ * object's pose. This comes from computer vision.
+ * @param robotToCamera The transformation from the robot's pose to the camera's
+ * pose. This can either be a constant for a rigidly mounted camera, or
+ * variable if the camera is mounted to a turret. If the camera was mounted 3
+ * inches behind the "origin" (usually physical center) of the robot, this
+ * would be frc::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}.
+ * @return The robot's field-relative pose.
*/
WPILIB_DLLEXPORT
-units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
- units::meter_t targetHeight,
- units::radian_t cameraPitch,
- units::radian_t targetPitch,
- units::radian_t targetYaw);
-
-/**
- * Estimate the position of the robot in the field.
- *
- * @param cameraHeight The physical height of the camera off the floor.
- * @param targetHeight The physical height of the target off the floor. This
- * should be the height of whatever is being targeted (i.e.
- * if the targeting region is set to top, this should be the
- * height of the top of the target).
- * @param cameraPitch The pitch of the camera from the horizontal plane.
- * Positive values up.
- * @param targetPitch The pitch of the target in the camera's lens. Positive
- * values up.
- * @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 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::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}.
- * @return The position of the robot in the field.
- */
-WPILIB_DLLEXPORT
-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::Pose3d& fieldToTarget, const frc::Transform3d& cameraToRobot);
-
-/**
- * Estimates the pose of the robot in the field coordinate system, given the
- * position of the target relative to the camera, the target relative to the
- * field, and the robot relative to the camera.
- *
- * @param cameraToTarget The position of the target relative to the camera.
- * @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
- * frc::Transform3d{3_in, 0_in, 0_in, frc::Rotation3d{}}.
- * @return The position of the robot in the field.
- */
-WPILIB_DLLEXPORT
-frc::Pose3d EstimateFieldToRobot(const frc::Transform3d& cameraToTarget,
- const frc::Pose3d& fieldToTarget,
- const frc::Transform3d& cameraToRobot);
-
-/**
- * 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 Translation3d that encodes the x/y
- * position of the target relative to the
- * camera.
- * @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 Transform3d that takes us from the camera to the target.
- */
-WPILIB_DLLEXPORT
-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
- * position of the target relative to the camera, and the target relative to
- * the field. This *only* tracks the position of the camera, not the position
- * of the robot itself.
- *
- * @param cameraToTarget The position of the target relative to the camera.
- * @param fieldToTarget The position of the target in the field.
- * @return The position of the camera in the field.
- */
-WPILIB_DLLEXPORT
-frc::Pose3d EstimateFieldToCamera(const frc::Transform3d& cameraToTarget,
- const frc::Pose3d& fieldToTarget);
+frc::Pose3d ObjectToRobotPose(const frc::Pose3d& objectInField,
+ const frc::Transform3d& cameraToObject,
+ const frc::Transform3d& robotToCamera);
} // 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 5a6d949a82..29278475d7 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/ComputerVisionUtilTest.java
@@ -4,108 +4,30 @@
package edu.wpi.first.math;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
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;
class ComputerVisionUtilTest {
@Test
- void testCalculateDistanceToTarget() {
- var cameraHeight = 1;
- var targetHeight = 3;
- var cameraPitch = Units.degreesToRadians(0);
- var targetPitch = Units.degreesToRadians(30);
- var targetYaw = Units.degreesToRadians(0);
+ void testObjectToRobotPose() {
+ var robot = new Pose3d(1.0, 2.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(30.0)));
+ var cameraToObject =
+ new Transform3d(
+ new Translation3d(1.0, 1.0, 1.0),
+ new Rotation3d(0.0, Units.degreesToRadians(-20.0), Units.degreesToRadians(45.0)));
+ var robotToCamera =
+ new Transform3d(
+ new Translation3d(1.0, 0.0, 2.0),
+ new Rotation3d(0.0, 0.0, Units.degreesToRadians(25.0)));
+ Pose3d object = robot.plus(robotToCamera).plus(cameraToObject);
- var distanceAlongGround =
- ComputerVisionUtil.calculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- Assertions.assertEquals(3.464, distanceAlongGround, 0.01);
-
- cameraHeight = 1;
- targetHeight = 2;
- cameraPitch = Units.degreesToRadians(20);
- targetPitch = Units.degreesToRadians(-10);
-
- distanceAlongGround =
- ComputerVisionUtil.calculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- Assertions.assertEquals(5.671, distanceAlongGround, 0.01);
-
- cameraHeight = 3;
- targetHeight = 1;
- cameraPitch = Units.degreesToRadians(0);
- targetPitch = Units.degreesToRadians(-30);
-
- distanceAlongGround =
- ComputerVisionUtil.calculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- Assertions.assertEquals(3.464, distanceAlongGround, 0.01);
-
- cameraHeight = 1;
- targetHeight = 3;
- cameraPitch = Units.degreesToRadians(0);
- targetPitch = Units.degreesToRadians(30);
- targetYaw = Units.degreesToRadians(30);
-
- distanceAlongGround =
- ComputerVisionUtil.calculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- Assertions.assertEquals(4, distanceAlongGround, 0.01);
- }
-
- @Test
- void testEstimateFieldToRobot() {
- var cameraHeight = 1;
- var targetHeight = 3;
- var cameraPitch = 0;
- var targetPitch = Units.degreesToRadians(30);
- var targetYaw = new Rotation2d();
- var gyroAngle = new Rotation2d();
- 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 Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())),
- fieldToTarget,
- gyroAngle),
- fieldToTarget,
- cameraToRobot);
-
- Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1);
- Assertions.assertEquals(0, fieldToRobot.getY(), 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 Translation3d(range, new Rotation3d(0.0, targetPitch, targetYaw.getRadians())),
- fieldToTarget,
- gyroAngle),
- fieldToTarget,
- cameraToRobot);
-
- Assertions.assertEquals(-3.0, fieldToRobot.getX(), 0.1);
- Assertions.assertEquals(1.732, fieldToRobot.getY(), 0.1);
- Assertions.assertEquals(2.0, fieldToRobot.getZ(), 0.1);
- Assertions.assertEquals(-30.0, Units.radiansToDegrees(fieldToRobot.getRotation().getZ()), 0.1);
+ assertEquals(
+ robot, ComputerVisionUtil.objectToRobotPose(object, cameraToObject, robotToCamera));
}
}
diff --git a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
index 209407d383..e397af1338 100644
--- a/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
+++ b/wpimath/src/test/native/cpp/ComputerVisionUtilTest.cpp
@@ -3,95 +3,16 @@
// the WPILib BSD license file in the root directory of this project.
#include "frc/ComputerVisionUtil.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Rotation2d.h"
-#include "frc/geometry/Transform2d.h"
#include "gtest/gtest.h"
-#include "units/angle.h"
-#include "units/length.h"
-TEST(ComputerVisionUtilTest, CalculateDistanceToTarget) {
- auto cameraHeight = 1_m;
- auto targetHeight = 3_m;
- auto cameraPitch = 0_deg;
- auto targetPitch = 30_deg;
- auto targetYaw = 0_deg;
+TEST(ComputerVisionUtilTest, ObjectToRobotPose) {
+ frc::Pose3d robot{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 30_deg}};
+ frc::Transform3d cameraToObject{frc::Translation3d{1_m, 1_m, 1_m},
+ frc::Rotation3d{0_deg, -20_deg, 45_deg}};
+ frc::Transform3d robotToCamera{frc::Translation3d{1_m, 0_m, 2_m},
+ frc::Rotation3d{0_deg, 0_deg, 25_deg}};
+ frc::Pose3d object = robot + robotToCamera + cameraToObject;
- auto distanceAlongGround = frc::CalculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01);
-
- cameraHeight = 1_m;
- targetHeight = 2_m;
- cameraPitch = 20_deg;
- targetPitch = -10_deg;
-
- distanceAlongGround = frc::CalculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- EXPECT_NEAR(5.671, distanceAlongGround.value(), 0.01);
-
- cameraHeight = 3_m;
- targetHeight = 1_m;
- cameraPitch = 0_deg;
- targetPitch = -30_deg;
-
- distanceAlongGround = frc::CalculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- EXPECT_NEAR(3.464, distanceAlongGround.value(), 0.01);
-
- cameraHeight = 1_m;
- targetHeight = 3_m;
- cameraPitch = 0_deg;
- targetPitch = 30_deg;
- targetYaw = 30_deg;
-
- distanceAlongGround = frc::CalculateDistanceToTarget(
- cameraHeight, targetHeight, cameraPitch, targetPitch, targetYaw);
- EXPECT_NEAR(4, distanceAlongGround.value(), 0.01);
-}
-
-TEST(ComputerVisionUtilTest, EstimateFieldToRobot) {
- auto cameraHeight = 1_m;
- auto targetHeight = 3_m;
- auto cameraPitch = 0_deg;
- auto targetPitch = 30_deg;
- frc::Rotation2d targetYaw;
- frc::Rotation2d gyroAngle;
- 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::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(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::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(2.0, fieldToRobot.Z().value(), 0.1);
- EXPECT_NEAR(-30.0, units::degree_t{fieldToRobot.Rotation().Z()}.value(), 0.1);
+ EXPECT_EQ(robot,
+ frc::ObjectToRobotPose(object, cameraToObject, robotToCamera));
}