| James Kuszmaul | 18008f8 | 2023-02-23 20:52:50 -0800 | [diff] [blame^] | 1 | #include "y2023/localizer/utils.h" |
| 2 | |
| 3 | namespace y2023::localizer { |
| 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 | } |
| 14 | } // namespace y2023::localizer |