Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make ComputerVisionUtil use 3D geometry classes (#4528)

Browse Source 
Closes #4189.
This commit is contained in:
Tyler Veness
2022-10-26 22:20:08 -07:00
committed by GitHub
parent cfb84a6083
commit 8bc3b04f5b
5 changed files with 159 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpimath/src/main/native/cpp/ComputerVisionUtil.cpp
View File

@@ -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);
}

40
wpimath/src/main/native/include/frc/ComputerVisionUtil.h
View File

@@ -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