| James Kuszmaul | 9c3db18 | 2024-02-09 22:02:18 -0800 | [diff] [blame] | 1 | #include "frc971/vision/target_map_utils.h" |
| James Kuszmaul | 18008f8 | 2023-02-23 20:52:50 -0800 | [diff] [blame] | 2 | |
| James Kuszmaul | 9c3db18 | 2024-02-09 22:02:18 -0800 | [diff] [blame] | 3 | namespace frc971::vision { |
| James Kuszmaul | 18008f8 | 2023-02-23 20:52:50 -0800 | [diff] [blame] | 4 | Eigen::Matrix<double, 4, 4> PoseToTransform( |
| 5 | const frc971::vision::TargetPoseFbs *pose) { |
| 6 | const frc971::vision::Position *position = pose->position(); |
| 7 | const frc971::vision::Quaternion *quaternion = pose->orientation(); |
| 8 | return (Eigen::Translation3d( |
| 9 | Eigen::Vector3d(position->x(), position->y(), position->z())) * |
| 10 | Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(), |
| 11 | quaternion->z())) |
| 12 | .matrix(); |
| 13 | } |
| James Kuszmaul | 9c3db18 | 2024-02-09 22:02:18 -0800 | [diff] [blame] | 14 | } // namespace frc971::vision |