mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-27 02:01:42 +00:00
f18fd41ac3
The ComputerVisionUtil class was added before AprilTag support was announced. Now that it has, the functions for estimating a pose from range and yaw are no longer needed; it's just better to get the pose directly from the AprilTag. The coordinate system on some function arguments was confusing or didn't match the NWU convention the rest of the library uses. It's easier to remove the functions now and add them back after they're fixed since the fixes aren't trivial. The range function was removed because it uses pitch and yaw in the camera's spherical coordinate system, which is obsoleted by AprilTags. AprilTags give you a 6DOF pose directly, so range can be obtained via Pose2d.getTranslation().getDistance(). Fixes #4757.
20 lines
674 B
C++
20 lines
674 B
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "frc/ComputerVisionUtil.h"
|
|
|
|
#include "units/math.h"
|
|
|
|
namespace frc {
|
|
|
|
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
|