commit 9c3db182b769f00b898fd8db8026c20b917a1a1d
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Feb 09 22:02:18 2024 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Feb 17 13:28:17 2024 -0800
tree 84f33b6f1fd201d8362b8f6751e9b8752c80dbf2
parent d44ffaad357f5854dd2c14b9f08f9be9af40824d

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>

frc971/vision/BUILD

diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 02ab5e2..d96822b 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -386,3 +386,14 @@
         "@com_github_google_glog//:glog",
     ],
 )
+
+cc_library(
+    name = "target_map_utils",
+    srcs = ["target_map_utils.cc"],
+    hdrs = ["target_map_utils.h"],
+    visibility = ["//visibility:public"],
+    deps = [
+        "//frc971/vision:target_map_fbs",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)

frc971/vision/target_map_utils.cc

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

frc971/vision/target_map_utils.h

diff --git a/frc971/vision/target_map_utils.h b/frc971/vision/target_map_utils.h
new file mode 100644
index 0000000..36b4e30
--- /dev/null
+++ b/frc971/vision/target_map_utils.h
@@ -0,0 +1,14 @@
+#ifndef FRC971_VISION_TARGET_MAP_UTILS_H_
+#define FRC971_VISION_TARGET_MAP_UTILS_H_
+
+#include <Eigen/Dense>
+
+#include "frc971/vision/target_map_generated.h"
+
+namespace frc971::vision {
+// Converts a TargetPoseFbs into a transformation matrix.
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+    const frc971::vision::TargetPoseFbs *pose);
+}  // namespace frc971::vision
+
+#endif  // FRC971_VISION_TARGET_MAP_UTILS_H_