Move y2023/localizer/utils.h to frc971/vision
These are entirely year-generic, no reason to keep them year-specific.
Change-Id: Ie3636613074e21c578afe0c9f136f6553759b63c
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/vision/target_map_utils.cc b/frc971/vision/target_map_utils.cc
new file mode 100644
index 0000000..232c2b6
--- /dev/null
+++ b/frc971/vision/target_map_utils.cc
@@ -0,0 +1,14 @@
+#include "frc971/vision/target_map_utils.h"
+
+namespace frc971::vision {
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose) {
+ const frc971::vision::Position *position = pose->position();
+ const frc971::vision::Quaternion *quaternion = pose->orientation();
+ return (Eigen::Translation3d(
+ Eigen::Vector3d(position->x(), position->y(), position->z())) *
+ Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
+ quaternion->z()))
+ .matrix();
+}
+} // namespace frc971::vision