mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
[wpimath] Make ComputerVisionUtil use 3D geometry classes (#4528)
Closes #4189.
This commit is contained in:
Tyler Veness
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -6,10 +6,10 @@
|
||||
|
||||
#include <wpi/SymbolExports.h>
|
||||
|
||||
#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
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user