fix calibration

src/localization/CMakeLists.txt

@@ -1,4 +1,4 @@
 cmake_minimum_required(VERSION 3.10)
 
-add_library(localization position_sender.cc gpu_apriltag_detector.cc opencv_apriltag_detector.cc run_localization.cc nvidia_apriltag_detector.cc square_solver.cc joint_solver.cc multi_tag_solver.cc position_receiver.cc)
+add_library(localization position_sender.cc position_solver.cc gpu_apriltag_detector.cc opencv_apriltag_detector.cc run_localization.cc nvidia_apriltag_detector.cc square_solver.cc joint_solver.cc multi_tag_solver.cc position_receiver.cc)
 target_link_libraries(localization PRIVATE 971apriltag utils camera wpilibc wpiutil)

src/localization/joint_solver.h

@@ -16,8 +16,9 @@ struct CameraMatrices {
 static constexpr int kmax_tags = 50;
 class JointSolver {
  public:
-  JointSolver(const std::vector<camera::camera_constant_t>& camera_constants_,
-              const frc::AprilTagFieldLayout& layout = kapriltag_layout);
+  JointSolver(
+      const std::vector<camera::camera_constant_t>& camera_constants_,
+      const frc::AprilTagFieldLayout& layout = GetAprilTagFieldLayout());
   auto EstimatePosition(
       const std::map<std::string, std::vector<tag_detection_t>>&
           all_cam_detections,

src/localization/multi_tag_solver.h

@@ -14,11 +14,11 @@ class MultiTagSolver : public IPositionSolver {
  public:
   MultiTagSolver(
       const std::string& intrinsics_path, const std::string& extrinsics_path,
-      const frc::AprilTagFieldLayout& layout = kapriltag_layout,
+      const frc::AprilTagFieldLayout& layout = GetAprilTagFieldLayout(),
       const std::vector<cv::Point3d>& tag_corners = kapriltag_corners);
   MultiTagSolver(
       camera::camera_constant_t camera_constant,
-      const frc::AprilTagFieldLayout& layout = kapriltag_layout,
+      const frc::AprilTagFieldLayout& layout = GetAprilTagFieldLayout(),
       const std::vector<cv::Point3d>& tag_corners = kapriltag_corners);
   auto EstimatePosition(const std::vector<tag_detection_t>& detections,
                         bool reject_far_tags = true)

src/localization/position_solver.cc

@@ -0,0 +1,16 @@
+#include "src/localization/position_solver.h"
+#include "absl/flags/flag.h"
+
+ABSL_FLAG(std::string, apriltag_field_layout_path,  // NOLINT
+          "/bos/constants/calibrated_map.json",     // NOLINT
+          "Path to the JSON file containing AprilTag field layout");
+
+namespace localization {
+
+auto GetAprilTagFieldLayout() -> const frc::AprilTagFieldLayout& {
+  static const frc::AprilTagFieldLayout layout{
+      absl::GetFlag(FLAGS_apriltag_field_layout_path)};
+  return layout;
+}
+
+}  // namespace localization

src/localization/position_solver.h

@@ -21,9 +21,12 @@ const std::vector<Eigen::Vector3d> kapriltag_corners_eigen = {
     {ktag_size / 2, -ktag_size / 2, 0},
     {-ktag_size / 2, -ktag_size / 2, 0}};
 
-const frc::AprilTagFieldLayout kapriltag_layout =
-    frc::AprilTagFieldLayout::LoadField(
-        frc::AprilTagField::k2026RebuiltAndyMark);
+/**
+ * Returns a reference to the AprilTag field layout.
+ * Loads from the path specified by --apriltag_field_layout_path flag.
+ * Loaded once and cached (function-local static).
+ */
+auto GetAprilTagFieldLayout() -> const frc::AprilTagFieldLayout&;
 
 inline auto Variance(const int num_tags_detected, const double distance,
                      const double min_variance, const double scalar)

src/localization/square_solver.cc

@@ -74,9 +74,7 @@ auto SquareSolver::EstimatePosition(
     cv::Mat camera_to_tag = utils::MakeTransform(rvec, tvec);
     cv::Mat tag_to_camera = camera_to_tag.inv();
     cv::Mat field_to_tag = utils::EigenToCvMat(
-        localization::kapriltag_layout.GetTagPose(detection.tag_id)
-            .value()
-            .ToMatrix());
+        layout_.GetTagPose(detection.tag_id).value().ToMatrix());
     frc::Pose3d robot_pose(utils::CvMatToEigen(
         ((field_to_tag * rotate_yaw_wpilib_) * tag_to_camera) *
         camera_to_robot_));
@@ -130,9 +128,7 @@ auto SquareSolver::EstimatePositionNew(
     cv::Mat camera_to_tag = utils::MakeTransform(rvec, tvec);
     cv::Mat tag_to_camera = camera_to_tag.inv();
     cv::Mat field_to_tag = utils::EigenToCvMat(
-        localization::kapriltag_layout.GetTagPose(detection.tag_id)
-            .value()
-            .ToMatrix());
+        layout_.GetTagPose(detection.tag_id).value().ToMatrix());
     utils::ChangeBasis(field_to_tag, utils::WPI_TO_CV);
     cv::Mat field_to_robot =
         field_to_tag * rotate_yaw_cv_ * tag_to_camera * camera_to_robot_;

src/localization/square_solver.h

@@ -13,10 +13,10 @@ class SquareSolver : public IPositionSolver {
  public:
   SquareSolver(const std::string& intrinsics_path,
                const std::string& extrinsics_path,
-               frc::AprilTagFieldLayout layout = kapriltag_layout,
+               frc::AprilTagFieldLayout layout = GetAprilTagFieldLayout(),
                std::vector<cv::Point3d> tag_corners = kapriltag_corners);
   SquareSolver(camera::camera_constant_t camera_constant,
-               frc::AprilTagFieldLayout layout = kapriltag_layout,
+               frc::AprilTagFieldLayout layout = GetAprilTagFieldLayout(),
                std::vector<cv::Point3d> tag_corners = kapriltag_corners);
 
   auto EstimatePosition(const std::vector<tag_detection_t>& detections,

src/main_bot_main.cc

@@ -42,7 +42,8 @@ auto main() -> int {
           utils::ReadIntrinsics(
               camera_constants.at("main_bot_front").intrinsics_path.value())),
       std::make_unique<localization::MultiTagSolver>(
-          camera_constants.at("main_bot_front")),
+          camera_constants.at("main_bot_front"),
+          localization::GetAprilTagFieldLayout()),
       camera_constants.at("main_bot_front").extrinsics_path.value(), 4971,
       false);
 
@@ -53,7 +54,8 @@ auto main() -> int {
           utils::ReadIntrinsics(
               camera_constants.at("main_bot_left").intrinsics_path.value())),
       std::make_unique<localization::MultiTagSolver>(
-          camera_constants.at("main_bot_left")),
+          camera_constants.at("main_bot_left"),
+          localization::GetAprilTagFieldLayout()),
       camera_constants.at("main_bot_left").extrinsics_path.value(), 5802,
       false);
 
@@ -64,7 +66,8 @@ auto main() -> int {
           utils::ReadIntrinsics(
               camera_constants.at("main_bot_right").intrinsics_path.value())),
       std::make_unique<localization::MultiTagSolver>(
-          camera_constants.at("main_bot_right")),
+          camera_constants.at("main_bot_right"),
+          localization::GetAprilTagFieldLayout()),
       camera_constants.at("main_bot_right").extrinsics_path.value(), 5803,
       false);
 

src/second_bot_main.cc

@@ -52,7 +52,8 @@ auto main() -> int {
           utils::ReadIntrinsics(
               camera_constants.at("second_bot_left").intrinsics_path.value())),
       std::make_unique<localization::MultiTagSolver>(
-          camera_constants.at("second_bot_left")),
+          camera_constants.at("second_bot_left"),
+          localization::GetAprilTagFieldLayout()),
       camera_constants.at("second_bot_left").extrinsics_path.value(), 5803,
       false);
 
@@ -63,7 +64,8 @@ auto main() -> int {
           utils::ReadIntrinsics(
               camera_constants.at("second_bot_right").intrinsics_path.value())),
       std::make_unique<localization::MultiTagSolver>(
-          camera_constants.at("second_bot_right")),
+          camera_constants.at("second_bot_right"),
+          localization::GetAprilTagFieldLayout()),
       camera_constants.at("second_bot_right").extrinsics_path.value(), 5804,
       false);
 

src/test/integration_test/localization_test.cc

@@ -37,8 +37,7 @@ auto HasRegularFiles(const std::filesystem::path& path) -> bool {
     std::string extension = entry.path().extension().string();
     std::transform(extension.begin(), extension.end(), extension.begin(),
                    [](unsigned char c) { return std::tolower(c); });
-    if (extension == ".png" || extension == ".jpg" ||
-        extension == ".jpeg") {
+    if (extension == ".png" || extension == ".jpg" || extension == ".jpeg") {
       return true;
     }
   }
@@ -65,15 +64,14 @@ auto FindCameraFolders(const std::filesystem::path& path)
 auto ResolveCameraName(const std::string& directory_name,
                        const camera::camera_constants_t& constants)
     -> std::string {
-  std::string resolved_name =
-      directory_name.rfind("main_bot_", 0) == 0
-          ? directory_name
-          : "main_bot_" + directory_name;
+  std::string resolved_name = directory_name.rfind("main_bot_", 0) == 0
+                                  ? directory_name
+                                  : "main_bot_" + directory_name;
 
   if (!constants.contains(resolved_name)) {
     LOG(FATAL) << "Could not resolve camera constants name for directory: "
-               << directory_name << ", expected constants entry: "
-               << resolved_name;
+               << directory_name
+               << ", expected constants entry: " << resolved_name;
   }
 
   return resolved_name;
@@ -124,9 +122,8 @@ auto main(int argc, char** argv) -> int {
     }
 
     camera_sources.push_back(std::make_unique<camera::CameraSource>(
-        camera_name,
-        std::make_unique<camera::DiskCamera>(camera_folder.string(),
-                                             std::nullopt, speed)));
+        camera_name, std::make_unique<camera::DiskCamera>(
+                         camera_folder.string(), std::nullopt, speed)));
 
     camera::CameraSource& camera_source = *camera_sources.back();
     auto frame = camera_source.GetFrame();