From 6a2807007226fd4b682a6bedad35b9d8b3db6db5 Mon Sep 17 00:00:00 2001 From: Viba Mohan Date: Fri, 3 Apr 2026 22:51:12 +0000 Subject: [PATCH 1/3] fix calibration --- constants/calibrated_map.json | 549 +++++++++++++++++++++++++++++ src/localization/position_solver.h | 2 +- 2 files changed, 550 insertions(+), 1 deletion(-) create mode 100644 constants/calibrated_map.json diff --git a/constants/calibrated_map.json b/constants/calibrated_map.json new file mode 100644 index 00000000..a5246bdb --- /dev/null +++ b/constants/calibrated_map.json @@ -0,0 +1,549 @@ +{ + "tags" : [ + { + "ID" : 1, + "pose" : { + "translation" : { + "x" : 11.875921249389648, + "y" : 7.5439453125, + "z" : 0.7925388813018799 + }, + "rotation" : { + "quaternion" : { + "W" : -0.017850790172815323, + "X" : 0.014882062561810017, + "Y" : -0.0007217469974420965, + "Z" : 0.9997296333312988 + } + } + } + }, + { + "ID" : 2, + "pose" : { + "translation" : { + "x" : 11.896167755126953, + "y" : 4.624698162078857, + "z" : 1.1230480670928955 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7027201652526855, + "X" : 0.002001303480938077, + "Y" : 0.014001062139868736, + "Z" : 0.7113259434700012 + } + } + } + }, + { + "ID" : 3, + "pose" : { + "translation" : { + "x" : 11.311864852905273, + "y" : 4.390237808227539, + "z" : 1.1239500045776367 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 1.0 + } + } + } + }, + { + "ID" : 4, + "pose" : { + "translation" : { + "x" : 11.309041023254395, + "y" : 4.0331854820251465, + "z" : 1.1280386447906494 + }, + "rotation" : { + "quaternion" : { + "W" : -0.0057865106500685215, + "X" : -0.00138318061362952, + "Y" : 0.0010927513940259814, + "Z" : 0.9999817609786987 + } + } + } + }, + { + "ID" : 5, + "pose" : { + "translation" : { + "x" : 11.913521766662598, + "y" : 3.4121131896972656, + "z" : 1.1317188739776611 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7072911262512207, + "X" : -0.013083551079034805, + "Y" : -0.005719936918467283, + "Z" : -0.7067782878875732 + } + } + } + }, + { + "ID" : 6, + "pose" : { + "translation" : { + "x" : 11.897409439086914, + "y" : 0.6016051769256592, + "z" : 0.8386548757553101 + }, + "rotation" : { + "quaternion" : { + "W" : -0.02148066647350788, + "X" : 0.06235736235976219, + "Y" : -0.005285942927002907, + "Z" : 0.9978087544441223 + } + } + } + }, + { + "ID" : 7, + "pose" : { + "translation" : { + "x" : 11.934053421020508, + "y" : 0.626187801361084, + "z" : 0.8545701503753662 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9998833537101746, + "X" : -0.0007205287110991776, + "Y" : 0.013563890010118484, + "Z" : -0.006990760564804077 + } + } + } + }, + { + "ID" : 8, + "pose" : { + "translation" : { + "x" : 12.280057907104492, + "y" : 3.425236701965332, + "z" : 1.1187946796417236 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7078094482421875, + "X" : -0.0011965626617893577, + "Y" : -0.00777541333809495, + "Z" : -0.7063596248626709 + } + } + } + }, + { + "ID" : 9, + "pose" : { + "translation" : { + "x" : 12.509306907653809, + "y" : 3.679147720336914, + "z" : 1.1237692832946777 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9997282028198242, + "X" : -0.009091046638786793, + "Y" : 0.008884177543222904, + "Z" : 0.019545089453458786 + } + } + } + }, + { + "ID" : 10, + "pose" : { + "translation" : { + "x" : 12.520866394042969, + "y" : 4.032379150390625, + "z" : 1.113229513168335 + }, + "rotation" : { + "quaternion" : { + "W" : 0.999724268913269, + "X" : -0.014895313419401646, + "Y" : 0.011223175562918186, + "Z" : 0.014271488413214684 + } + } + } + }, + { + "ID" : 11, + "pose" : { + "translation" : { + "x" : 12.234682083129883, + "y" : 4.625340461730957, + "z" : 1.112737774848938 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7041671872138977, + "X" : 0.006034619174897671, + "Y" : 0.014329856261610985, + "Z" : 0.70986407995224 + } + } + } + }, + { + "ID" : 12, + "pose" : { + "translation" : { + "x" : 11.914060592651367, + "y" : 7.510349273681641, + "z" : 0.8055204153060913 + }, + "rotation" : { + "quaternion" : { + "W" : 0.999936044216156, + "X" : 0.007622192148119211, + "Y" : 0.008131369017064571, + "Z" : 0.0019091779831796885 + } + } + } + }, + { + "ID" : 13, + "pose" : { + "translation" : { + "x" : 16.476512908935547, + "y" : 7.4005842208862305, + "z" : 0.4618014693260193 + }, + "rotation" : { + "quaternion" : { + "W" : 0.0057561746798455715, + "X" : 0.010195695795118809, + "Y" : 0.002686823718249798, + "Z" : 0.9999278783798218 + } + } + } + }, + { + "ID" : 14, + "pose" : { + "translation" : { + "x" : 16.750709533691406, + "y" : 7.005283832550049, + "z" : 0.43773651123046875 + }, + "rotation" : { + "quaternion" : { + "W" : 0.042970139533281326, + "X" : 0.021833091974258423, + "Y" : -0.0011738614412024617, + "Z" : 0.9988371133804321 + } + } + } + }, + { + "ID" : 15, + "pose" : { + "translation" : { + "x" : 16.463510513305664, + "y" : 4.317402362823486, + "z" : 0.48823046684265137 + }, + "rotation" : { + "quaternion" : { + "W" : 0.000817999301943928, + "X" : 0.016904184594750404, + "Y" : -0.00532533647492528, + "Z" : 0.9998425841331482 + } + } + } + }, + { + "ID" : 16, + "pose" : { + "translation" : { + "x" : 16.528364181518555, + "y" : 3.866840124130249, + "z" : 0.4711809754371643 + }, + "rotation" : { + "quaternion" : { + "W" : -0.02420216053724289, + "X" : 0.02101813070476055, + "Y" : -0.007978207431733608, + "Z" : 0.9994543194770813 + } + } + } + }, + { + "ID" : 17, + "pose" : { + "translation" : { + "x" : 4.765810012817383, + "y" : 0.672478437423706, + "z" : 0.9881226420402527 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9895285367965698, + "X" : -0.0037361669819802046, + "Y" : -0.12984338402748108, + "Z" : -0.06292884796857834 + } + } + } + }, + { + "ID" : 18, + "pose" : { + "translation" : { + "x" : 4.584910869598389, + "y" : 3.43510365486145, + "z" : 1.2218271493911743 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7005232572555542, + "X" : 0.0031395184341818094, + "Y" : -0.014674943871796131, + "Z" : -0.7134717702865601 + } + } + } + }, + { + "ID" : 19, + "pose" : { + "translation" : { + "x" : 5.077688694000244, + "y" : 3.6394405364990234, + "z" : 1.2120795249938965 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9961698055267334, + "X" : 0.003075927961617708, + "Y" : -0.045074477791786194, + "Z" : -0.07486365735530853 + } + } + } + }, + { + "ID" : 20, + "pose" : { + "translation" : { + "x" : 5.047872543334961, + "y" : 3.9971811771392822, + "z" : 1.2078109979629517 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9983355402946472, + "X" : -0.01595189981162548, + "Y" : -0.04631337150931358, + "Z" : -0.030443765223026276 + } + } + } + }, + { + "ID" : 21, + "pose" : { + "translation" : { + "x" : 4.637393474578857, + "y" : 4.637195110321045, + "z" : 1.2144553661346436 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7111435532569885, + "X" : 0.005097430199384689, + "Y" : 0.008678554557263851, + "Z" : 0.702974796295166 + } + } + } + }, + { + "ID" : 24, + "pose" : { + "translation" : { + "x" : 4.205660820007324, + "y" : 4.622918128967285, + "z" : 1.211899757385254 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7112739086151123, + "X" : 0.011953228153288364, + "Y" : 0.017410162836313248, + "Z" : 0.7025976777076721 + } + } + } + }, + { + "ID" : 25, + "pose" : { + "translation" : { + "x" : 4.030567646026611, + "y" : 4.379627704620361, + "z" : 1.212963342666626 + }, + "rotation" : { + "quaternion" : { + "W" : -0.007267146836966276, + "X" : 0.01194478664547205, + "Y" : 0.006994785275310278, + "Z" : 0.9998778104782104 + } + } + } + }, + { + "ID" : 26, + "pose" : { + "translation" : { + "x" : 4.024738788604736, + "y" : 4.033272743225098, + "z" : 1.22028386592865 + }, + "rotation" : { + "quaternion" : { + "W" : 0.005470994859933853, + "X" : 0.009013883769512177, + "Y" : 0.0020439622458070517, + "Z" : 0.9999423027038574 + } + } + } + }, + { + "ID" : 27, + "pose" : { + "translation" : { + "x" : 4.270040035247803, + "y" : 3.44403338432312, + "z" : 1.216443657875061 + }, + "rotation" : { + "quaternion" : { + "W" : 0.6960598230361938, + "X" : -0.004301766864955425, + "Y" : -0.01133889239281416, + "Z" : -0.7178813815116882 + } + } + } + }, + { + "ID" : 28, + "pose" : { + "translation" : { + "x" : 4.550383567810059, + "y" : 0.6360101699829102, + "z" : 0.9451743364334106 + }, + "rotation" : { + "quaternion" : { + "W" : 0.004774466156959534, + "X" : 0.011016186326742172, + "Y" : -0.0072367750108242035, + "Z" : 0.9999017715454102 + } + } + } + }, + { + "ID" : 29, + "pose" : { + "translation" : { + "x" : 0.2925863265991211, + "y" : 0.7428812980651855, + "z" : 0.7900299429893494 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9975513815879822, + "X" : -0.012037541717290878, + "Y" : -0.05495142564177513, + "Z" : -0.04155464842915535 + } + } + } + }, + { + "ID" : 30, + "pose" : { + "translation" : { + "x" : 0.19485855102539062, + "y" : 1.1195740699768066, + "z" : 0.7814610004425049 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9946686625480652, + "X" : -0.00735628604888916, + "Y" : -0.08462119102478027, + "Z" : -0.05847662314772606 + } + } + } + }, + { + "ID" : 31, + "pose" : { + "translation" : { + "x" : -0.08415508270263672, + "y" : 3.819650650024414, + "z" : 0.6745599508285522 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9999256730079651, + "X" : -0.004718994256108999, + "Y" : -0.005024900659918785, + "Z" : 0.01006208173930645 + } + } + } + }, + { + "ID" : 32, + "pose" : { + "translation" : { + "x" : -0.04279136657714844, + "y" : 4.24408483505249, + "z" : 0.6655440926551819 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9998557567596436, + "X" : -0.0145384781062603, + "Y" : -0.007895582355558872, + "Z" : -0.0038559434469789267 + } + } + } + } + ], + "field" : { + "length" : 16.541, + "width" : 8.069 + }, + "_comment" : "generated by PractiCal 1.0.5 (3) on 4/1/2026" +} \ No newline at end of file diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index 42365bf9..22884ab1 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -23,7 +23,7 @@ const std::vector kapriltag_corners_eigen = { const frc::AprilTagFieldLayout kapriltag_layout = frc::AprilTagFieldLayout::LoadField( - frc::AprilTagField::k2026RebuiltAndyMark); + "/root/bos/constants/calibrated_map.json"); inline auto Variance(const int num_tags_detected, const double distance, const double min_variance, const double scalar) From 9130eaa4b6d424699743f983634b999cd33652c4 Mon Sep 17 00:00:00 2001 From: Viba Mohan Date: Fri, 3 Apr 2026 23:02:10 +0000 Subject: [PATCH 2/3] resolve copilot review --- src/localization/CMakeLists.txt | 2 +- src/localization/joint_solver.h | 2 +- src/localization/multi_tag_solver.h | 4 ++-- src/localization/position_solver.cc | 17 +++++++++++++++++ src/localization/position_solver.h | 9 ++++++--- src/main_bot_main.cc | 9 ++++++--- src/second_bot_main.cc | 6 ++++-- src/test/integration_test/localization_test.cc | 3 ++- 8 files changed, 39 insertions(+), 13 deletions(-) create mode 100644 src/localization/position_solver.cc diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index 24cc2f16..114b09b3 100644 --- a/src/localization/CMakeLists.txt +++ b/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) diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 7dfb91f1..96d57946 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -17,7 +17,7 @@ static constexpr int kmax_tags = 50; class JointSolver { public: JointSolver(const std::vector& camera_constants_, - const frc::AprilTagFieldLayout& layout = kapriltag_layout); + const frc::AprilTagFieldLayout& layout); auto EstimatePosition( const std::map>& all_cam_detections, diff --git a/src/localization/multi_tag_solver.h b/src/localization/multi_tag_solver.h index d6dbe186..bafb64a5 100644 --- a/src/localization/multi_tag_solver.h +++ b/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, const std::vector& tag_corners = kapriltag_corners); MultiTagSolver( camera::camera_constant_t camera_constant, - const frc::AprilTagFieldLayout& layout = kapriltag_layout, + const frc::AprilTagFieldLayout& layout, const std::vector& tag_corners = kapriltag_corners); auto EstimatePosition(const std::vector& detections, bool reject_far_tags = true) diff --git a/src/localization/position_solver.cc b/src/localization/position_solver.cc new file mode 100644 index 00000000..cff5728b --- /dev/null +++ b/src/localization/position_solver.cc @@ -0,0 +1,17 @@ +#include "src/localization/position_solver.h" +#include "absl/flags/flag.h" + +ABSL_FLAG(std::string, apriltag_field_layout_path, // NOLINT + "/bos/constants/field_map_apr_01_20_22_53.json", // NOLINT + "Path to the JSON file containing AprilTag field layout"); + +namespace localization { + +auto GetAprilTagFieldLayout() -> const frc::AprilTagFieldLayout& { + static const frc::AprilTagFieldLayout layout = + frc::AprilTagFieldLayout::LoadField( + absl::GetFlag(FLAGS_apriltag_field_layout_path)); + return layout; +} + +} // namespace localization diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index 22884ab1..107b338a 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -21,9 +21,12 @@ const std::vector 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( - "/root/bos/constants/calibrated_map.json"); +/** + * 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) diff --git a/src/main_bot_main.cc b/src/main_bot_main.cc index 834514a5..edb8b1a9 100644 --- a/src/main_bot_main.cc +++ b/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( - 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( - 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( - 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); diff --git a/src/second_bot_main.cc b/src/second_bot_main.cc index 99f6604a..1ce54de0 100644 --- a/src/second_bot_main.cc +++ b/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( - 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( - 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); diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index d0692c7d..6914150e 100644 --- a/src/test/integration_test/localization_test.cc +++ b/src/test/integration_test/localization_test.cc @@ -140,7 +140,8 @@ auto main(int argc, char** argv) -> int { std::make_unique( frame.cols, frame.rows, utils::ReadIntrinsics(camera_constant.intrinsics_path.value())), - std::make_unique(camera_constant), + std::make_unique( + camera_constant, localization::GetAprilTagFieldLayout()), camera_constant.extrinsics_path.value(), base_port + static_cast(i), true); } From fa04576cf86a4210d73f023d9ef04374ca7075e0 Mon Sep 17 00:00:00 2001 From: "carrotmercinary@gmail.com" <167659798+yasen5@users.noreply.github.com> Date: Sat, 4 Apr 2026 01:12:44 +0000 Subject: [PATCH 3/3] Fix build --- src/localization/joint_solver.h | 5 +++-- src/localization/multi_tag_solver.h | 4 ++-- src/localization/position_solver.cc | 9 ++++---- src/localization/square_solver.cc | 8 ++----- src/localization/square_solver.h | 4 ++-- .../integration_test/localization_test.cc | 22 ++++++++----------- 6 files changed, 22 insertions(+), 30 deletions(-) diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 96d57946..b5f11b8d 100644 --- a/src/localization/joint_solver.h +++ b/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_constants_, - const frc::AprilTagFieldLayout& layout); + JointSolver( + const std::vector& camera_constants_, + const frc::AprilTagFieldLayout& layout = GetAprilTagFieldLayout()); auto EstimatePosition( const std::map>& all_cam_detections, diff --git a/src/localization/multi_tag_solver.h b/src/localization/multi_tag_solver.h index bafb64a5..578d75fd 100644 --- a/src/localization/multi_tag_solver.h +++ b/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, + const frc::AprilTagFieldLayout& layout = GetAprilTagFieldLayout(), const std::vector& tag_corners = kapriltag_corners); MultiTagSolver( camera::camera_constant_t camera_constant, - const frc::AprilTagFieldLayout& layout, + const frc::AprilTagFieldLayout& layout = GetAprilTagFieldLayout(), const std::vector& tag_corners = kapriltag_corners); auto EstimatePosition(const std::vector& detections, bool reject_far_tags = true) diff --git a/src/localization/position_solver.cc b/src/localization/position_solver.cc index cff5728b..e97a59a8 100644 --- a/src/localization/position_solver.cc +++ b/src/localization/position_solver.cc @@ -1,16 +1,15 @@ #include "src/localization/position_solver.h" #include "absl/flags/flag.h" -ABSL_FLAG(std::string, apriltag_field_layout_path, // NOLINT - "/bos/constants/field_map_apr_01_20_22_53.json", // NOLINT +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 = - frc::AprilTagFieldLayout::LoadField( - absl::GetFlag(FLAGS_apriltag_field_layout_path)); + static const frc::AprilTagFieldLayout layout{ + absl::GetFlag(FLAGS_apriltag_field_layout_path)}; return layout; } diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index 4373675a..dd5e605f 100644 --- a/src/localization/square_solver.cc +++ b/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_; diff --git a/src/localization/square_solver.h b/src/localization/square_solver.h index 75c2d9e5..4dfb9ac0 100644 --- a/src/localization/square_solver.h +++ b/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 tag_corners = kapriltag_corners); SquareSolver(camera::camera_constant_t camera_constant, - frc::AprilTagFieldLayout layout = kapriltag_layout, + frc::AprilTagFieldLayout layout = GetAprilTagFieldLayout(), std::vector tag_corners = kapriltag_corners); auto EstimatePosition(const std::vector& detections, diff --git a/src/test/integration_test/localization_test.cc b/src/test/integration_test/localization_test.cc index 6914150e..f65f4f08 100644 --- a/src/test/integration_test/localization_test.cc +++ b/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_name, - std::make_unique(camera_folder.string(), - std::nullopt, speed))); + camera_name, std::make_unique( + camera_folder.string(), std::nullopt, speed))); camera::CameraSource& camera_source = *camera_sources.back(); auto frame = camera_source.GetFrame(); @@ -140,8 +137,7 @@ auto main(int argc, char** argv) -> int { std::make_unique( frame.cols, frame.rows, utils::ReadIntrinsics(camera_constant.intrinsics_path.value())), - std::make_unique( - camera_constant, localization::GetAprilTagFieldLayout()), + std::make_unique(camera_constant), camera_constant.extrinsics_path.value(), base_port + static_cast(i), true); }