From 538c9d376428c14fb0cbe6bf04f9645adc359fd0 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 20 Apr 2026 22:40:14 +0000 Subject: [PATCH 01/47] fixup joint solve test Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 2 +- src/test/unit_test/CMakeLists.txt | 5 ++++- src/test/unit_test/forward_test.cc | 4 ++++ src/test/unit_test/joint_solve_test.cc | 17 +++++++++++++++-- 4 files changed, 24 insertions(+), 4 deletions(-) create mode 100644 src/test/unit_test/forward_test.cc diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 2f9ff596..d52b73e8 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -132,7 +132,7 @@ auto JointSolver::EstimatePosition( if (counter % 10000 == 0 && !printed) { printed = true; - std::cout << "loss: " << loss << std::endl; + // std::cout << "loss: " << loss << std::endl; } const Eigen::Vector3d d_projection( diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index cf8055b3..dbe09225 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -12,9 +12,12 @@ target_link_libraries(square_solve_test PRIVATE localization utils GTest::gtest_ add_executable(general_solver_test general_solver_test.cc) target_link_libraries(general_solver_test PRIVATE localization utils GTest::gtest_main) -add_executable(matrix matrix.cc) +add_executable(matrix matrix.cc) target_link_libraries(matrix PRIVATE localization utils GTest::gtest_main) +add_executable(forward_test forward_test.cc) +target_link_libraries(forward_test PRIVATE localization utils GTest::gtest_main) + include(GoogleTest) gtest_discover_tests(multi_tag_test) gtest_discover_tests(square_solve_test) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc new file mode 100644 index 00000000..90790ab6 --- /dev/null +++ b/src/test/unit_test/forward_test.cc @@ -0,0 +1,4 @@ +#include + +TEST(ForwardTest, Basic) { // NOLINT +} diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 0bc15d77..f9be2e63 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -1,5 +1,6 @@ #include #include "src/localization/joint_solver.h" +#include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" @@ -14,11 +15,23 @@ std::vector detections( TEST(JointSolveTest, EstimatePosition) { // NOLINT camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("dev_orin"); + camera::GetCameraConstants().at("second_bot_left"); + std::string image_path = "/bos-logs/log181/left/9.627390.jpg"; + localization::JointSolver joint_solver({camera_constant}); localization::SquareSolver square_solver(camera_constant); + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); + + cv::Mat image = cv::imread(image_path); + camera::timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + + auto detections = detector->GetTagDetections(timestamped_frame); + + auto square_solver_solution = square_solver.EstimatePosition({detections})[0]; - auto square_solver_solution = square_solver.EstimatePosition(detections)[0]; utils::PrintTransformationMatrix( utils::EigenToCvMat(square_solver_solution.pose.ToMatrix())); std::cout << square_solver_solution.pose.ToMatrix() << std::endl; From b7b8a2c87651713b910e739317e579acc99aefd9 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Tue, 21 Apr 2026 22:24:08 +0000 Subject: [PATCH 02/47] add xad Signed-off-by: Charlie Huang --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index fc7850f8..5cd48039 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) find_package(vpi REQUIRED) find_package(libuvc REQUIRED) +find_package(XAD REQUIRED) add_subdirectory(third_party/json) add_subdirectory(third_party/971apriltag) From ede4f928787dcc4c8a9414e2427d272ea58551f6 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 23 Apr 2026 05:59:53 +0000 Subject: [PATCH 03/47] wip Signed-off-by: Charlie Huang --- constants/second_bot/right_intrinsics.json | 10 +-- src/localization/position_solver.h | 7 ++ src/test/unit_test/forward_test.cc | 91 ++++++++++++++++++++++ src/utils/constants_from_json.cc | 8 +- 4 files changed, 109 insertions(+), 7 deletions(-) diff --git a/constants/second_bot/right_intrinsics.json b/constants/second_bot/right_intrinsics.json index 672dae0d..e9dcb014 100644 --- a/constants/second_bot/right_intrinsics.json +++ b/constants/second_bot/right_intrinsics.json @@ -3,9 +3,9 @@ "cy": 421.9852711165147, "fx": 896.4156102015556, "fy": 896.5943754377341, - "k1": 0.037473669453764555, - "k2": -0.08209649993614554, - "k3": 0.02896141051361048, - "p1": 0.00537699080347837, - "p2": 0.00035625234846317763 + "k1": 0.0, + "k2": 0.0, + "k3": 0.0, + "p1": 0.0, + "p2": 0.0 } diff --git a/src/localization/position_solver.h b/src/localization/position_solver.h index ae984f8f..7911d4d1 100644 --- a/src/localization/position_solver.h +++ b/src/localization/position_solver.h @@ -21,6 +21,13 @@ const std::vector kapriltag_corners_eigen = { {ktag_size / 2, -ktag_size / 2, 0}, {-ktag_size / 2, -ktag_size / 2, 0}}; +const std::vector kapriltag_corners_eigen_homogenized = { + {0, ktag_size / 2, -ktag_size / 2, 1}, // Bottom left + {0, -ktag_size / 2, -ktag_size / 2, 1}, // Bottom right + {0, -ktag_size / 2, ktag_size / 2, 1}, // Top right + {0, ktag_size / 2, ktag_size / 2, 1}, // Top left +}; + const frc::AprilTagFieldLayout kapriltag_layout = frc::AprilTagFieldLayout::LoadField( frc::AprilTagField::k2026RebuiltAndyMark); diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 90790ab6..48238746 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -1,4 +1,95 @@ #include +#include "src/localization/joint_solver.h" +#include "src/localization/opencv_apriltag_detector.h" +#include "src/localization/square_solver.h" +#include "src/utils/camera_utils.h" +#include "src/utils/constants_from_json.h" +#include "src/utils/transform.h" + +// clang-format off +const Eigen::Matrix PI = + (Eigen::Matrix() << + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0).finished(); + +const Eigen::Matrix rotate_yaw = + (Eigen::Matrix() << + -1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1).finished(); +// clang-format on + +auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, + const Eigen::Matrix3d& camera_matrix) -> Eigen::Vector3d { + auto feild_to_camera = camera_pose.ToMatrix(); + auto feild_to_tag = tag_pose.ToMatrix(); + auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + + Eigen::Vector3d projected_points = + camera_matrix * PI * camera_to_tag * + localization::kapriltag_corners_eigen_homogenized[0]; + projected_points /= projected_points[0]; + return projected_points; +} TEST(ForwardTest, Basic) { // NOLINT + camera::camera_constant_t camera_constant = + camera::GetCameraConstants().at("second_bot_right"); + camera_constant.extrinsics_path = + "/bos/constants/misc/dev_orin_extrinsics.json"; + CHECK(camera_constant.intrinsics_path.has_value()); + + std::string image_path = "/bos-logs/log181/left/9.627390.jpg"; + + localization::SquareSolver square_solver(camera_constant); + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); + + // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners + // feild_to_camera = feild_to_tag * 180_yaw * camera_to_tag.inv + // feild_to_camera * camera_to_tag = feild_to_tag * 180_yaw + // camera_to_tag = feild_to_camera.inv * feild_to_tag * 180_yaw + + cv::Mat image = cv::imread(image_path); + camera::timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + localization::tag_detection_t detection = + detector->GetTagDetections(timestamped_frame)[0]; + LOG(INFO) << "detected_tag_corners\n" + << detection.corners[0] << "\n" + << detection.corners[1] << "\n" + << detection.corners[2] << "\n" + << detection.corners[3]; + + localization::position_estimate_t square_solver_solution = + square_solver.EstimatePosition({detection})[0]; + + auto camera_matrix = utils::CameraMatrixFromJson( + utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); + LOG(INFO) << "camera_matrix\n" << camera_matrix; + + // auto feild_to_camera = square_solver_solution.pose.ToMatrix(); + // LOG(INFO) << "feild_to_camera\n" << feild_to_camera; + // + // auto feild_to_tag = + // localization::kapriltag_layout.GetTagPose(detection.tag_id)->ToMatrix(); + // LOG(INFO) << "feild_to_tag\n" << feild_to_tag; + // + // + // auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + // + // Eigen::Vector3d projected_points = + // camera_matrix * PI * camera_to_tag * + // localization::kapriltag_corners_eigen_homogenized[0]; + // projected_points /= projected_points[0]; + + auto projected_points = ProjectPoints( + square_solver_solution.pose, + localization::kapriltag_layout.GetTagPose(detection.tag_id).value(), + camera_matrix); + + LOG(INFO) << "Projected points\n" << projected_points; } diff --git a/src/utils/constants_from_json.cc b/src/utils/constants_from_json.cc index 7b8b601a..21277f5b 100644 --- a/src/utils/constants_from_json.cc +++ b/src/utils/constants_from_json.cc @@ -45,8 +45,12 @@ template <> auto CameraMatrixFromJson(nlohmann::json intrinsics) -> Eigen::Matrix3d { Eigen::Matrix3d K; - K << intrinsics["fx"], 0, intrinsics["cx"], 0, intrinsics["fy"], - intrinsics["cy"], 0, 0, 1; + // clang-format off + K << + 1, 0, 0, + static_cast(intrinsics["cx"]), -static_cast(intrinsics["fx"]), 0, + static_cast(intrinsics["cy"]), 0, -static_cast(intrinsics["fy"]); + // clang-format on return K; } From 55178a436154de009b849d57b23a0a0ad890e04a Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 23 Apr 2026 06:31:25 +0000 Subject: [PATCH 04/47] forward test TODO fix camera_matrix eigen Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 107 ++++++++++++++++------------- 1 file changed, 60 insertions(+), 47 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 48238746..60548a5b 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -1,4 +1,5 @@ #include +#include #include "src/localization/joint_solver.h" #include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" @@ -6,6 +7,21 @@ #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" +#define IMAGE_STRIDE 4 +#define LOG_PATH "/bos-logs/log181/left" + +namespace fs = std::filesystem; + +using camera::camera_constant_t; +using camera::GetCameraConstants; +using camera::timestamped_frame_t; +using localization::OpenCVAprilTagDetector; +using localization::position_estimate_t; +using localization::SquareSolver; +using localization::tag_detection_t; +using utils::CameraMatrixFromJson; +using utils::ReadIntrinsics; + // clang-format off const Eigen::Matrix PI = (Eigen::Matrix() << @@ -22,30 +38,39 @@ const Eigen::Matrix rotate_yaw = // clang-format on auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, - const Eigen::Matrix3d& camera_matrix) -> Eigen::Vector3d { + const Eigen::Matrix3d& camera_matrix, int corner_index) + -> Eigen::Vector3d { auto feild_to_camera = camera_pose.ToMatrix(); auto feild_to_tag = tag_pose.ToMatrix(); auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; Eigen::Vector3d projected_points = camera_matrix * PI * camera_to_tag * - localization::kapriltag_corners_eigen_homogenized[0]; + localization::kapriltag_corners_eigen_homogenized[corner_index]; projected_points /= projected_points[0]; return projected_points; } +// TODO: Tolerance is quite high. Find out what causes the loss in precision +void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, + double tolerance = 1) { + EXPECT_NEAR(image_point.x, projected_points[1], tolerance); + EXPECT_NEAR(image_point.y, projected_points[2], tolerance); +} + TEST(ForwardTest, Basic) { // NOLINT - camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("second_bot_right"); + camera_constant_t camera_constant = + GetCameraConstants().at("second_bot_right"); camera_constant.extrinsics_path = "/bos/constants/misc/dev_orin_extrinsics.json"; CHECK(camera_constant.intrinsics_path.has_value()); - std::string image_path = "/bos-logs/log181/left/9.627390.jpg"; - - localization::SquareSolver square_solver(camera_constant); - auto detector = std::make_unique( + SquareSolver square_solver(camera_constant); + const auto detector = std::make_unique( camera_constant.frame_width.value(), camera_constant.frame_height.value(), + ReadIntrinsics(camera_constant.intrinsics_path.value())); + + const auto camera_matrix = CameraMatrixFromJson( utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners @@ -53,43 +78,31 @@ TEST(ForwardTest, Basic) { // NOLINT // feild_to_camera * camera_to_tag = feild_to_tag * 180_yaw // camera_to_tag = feild_to_camera.inv * feild_to_tag * 180_yaw - cv::Mat image = cv::imread(image_path); - camera::timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - localization::tag_detection_t detection = - detector->GetTagDetections(timestamped_frame)[0]; - LOG(INFO) << "detected_tag_corners\n" - << detection.corners[0] << "\n" - << detection.corners[1] << "\n" - << detection.corners[2] << "\n" - << detection.corners[3]; - - localization::position_estimate_t square_solver_solution = - square_solver.EstimatePosition({detection})[0]; - - auto camera_matrix = utils::CameraMatrixFromJson( - utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); - LOG(INFO) << "camera_matrix\n" << camera_matrix; - - // auto feild_to_camera = square_solver_solution.pose.ToMatrix(); - // LOG(INFO) << "feild_to_camera\n" << feild_to_camera; - // - // auto feild_to_tag = - // localization::kapriltag_layout.GetTagPose(detection.tag_id)->ToMatrix(); - // LOG(INFO) << "feild_to_tag\n" << feild_to_tag; - // - // - // auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; - // - // Eigen::Vector3d projected_points = - // camera_matrix * PI * camera_to_tag * - // localization::kapriltag_corners_eigen_homogenized[0]; - // projected_points /= projected_points[0]; - - auto projected_points = ProjectPoints( - square_solver_solution.pose, - localization::kapriltag_layout.GetTagPose(detection.tag_id).value(), - camera_matrix); - - LOG(INFO) << "Projected points\n" << projected_points; + std::vector image_paths; + for (const auto& file : fs::directory_iterator(LOG_PATH)) { + image_paths.push_back(file.path()); + } + std::sort(image_paths.begin(), image_paths.end()); + + for (size_t i = 0; i < image_paths.size(); i += IMAGE_STRIDE) { + cv::Mat image = cv::imread(image_paths[i]); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector->GetTagDetections(timestamped_frame); + if (detections.empty()) { + continue; + } + auto detection = detections[0]; + + auto square_solver_solution = + square_solver.EstimatePosition({detection})[0]; + + for (int j = 0; j < 4; j++) { + auto projected_points = ProjectPoints( + square_solver_solution.pose, + localization::kapriltag_layout.GetTagPose(detection.tag_id).value(), + camera_matrix, j); + CheckIsEqual(detection.corners[j], projected_points); + } + } } From 4a886246c63548906cee7a8d859c986a9b247e90 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 23 Apr 2026 06:33:38 +0000 Subject: [PATCH 05/47] not fixing joint solve Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index d52b73e8..af746478 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -45,8 +45,8 @@ JointSolver::JointSolver( for (const camera::camera_constant_t& camera_constant : camera_constants_) { const nlohmann::json intrinsics_json = utils::ReadIntrinsics(camera_constant.intrinsics_path.value()); - const auto camera_matrix = - utils::CameraMatrixFromJson(intrinsics_json); + const auto camera_matrix = utils::CameraMatrixFromJson( + intrinsics_json); // TOOD CameraMatrixFromJson now returns inwpilib coords. Joint solver is broken anyway so this is fine for now. const Eigen::Matrix image_to_camera = camera_matrix * pi; const Eigen::Matrix4d camera_to_robot = utils::ExtrinsicsJsonToCameraToRobot( From 39a0508da57f32aabfb6f328fe5b5d6c51c6aaf6 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 23 Apr 2026 20:40:05 +0000 Subject: [PATCH 06/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 116 +++++++++++++++++++++++------ 1 file changed, 93 insertions(+), 23 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 60548a5b..9934f807 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -15,6 +15,7 @@ namespace fs = std::filesystem; using camera::camera_constant_t; using camera::GetCameraConstants; using camera::timestamped_frame_t; +using localization::kapriltag_layout; using localization::OpenCVAprilTagDetector; using localization::position_estimate_t; using localization::SquareSolver; @@ -37,6 +38,30 @@ const Eigen::Matrix rotate_yaw = 0, 0, 0, 1).finished(); // clang-format on +class ForwardTest : public ::testing::Test { + protected: + void SetUp() override { + camera_constant_ = GetCameraConstants().at("second_bot_right"); + camera_constant_.extrinsics_path = + "/bos/constants/misc/dev_orin_extrinsics.json"; + + square_solver_ = std::make_unique(camera_constant_); + detector_ = std::make_unique( + camera_constant_.frame_height.value(), + camera_constant_.frame_height.value(), + ReadIntrinsics(camera_constant_.intrinsics_path.value())); + camera_matrix_ = CameraMatrixFromJson( + ReadIntrinsics(camera_constant_.intrinsics_path.value())); + } + + void TearDown() override {} + + camera_constant_t camera_constant_; + std::unique_ptr square_solver_; + std::unique_ptr detector_; + Eigen::Matrix3d camera_matrix_; +}; + auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, const Eigen::Matrix3d& camera_matrix, int corner_index) -> Eigen::Vector3d { @@ -47,8 +72,41 @@ auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, Eigen::Vector3d projected_points = camera_matrix * PI * camera_to_tag * localization::kapriltag_corners_eigen_homogenized[corner_index]; - projected_points /= projected_points[0]; - return projected_points; + auto normalized_points = projected_points / projected_points[0]; + return normalized_points; +} + +auto CalculateDerivative(const frc::Pose3d& camera_pose, + const frc::Pose3d& tag_pose, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, int corner_index) + -> Eigen::Matrix4d { + auto feild_to_camera = camera_pose.ToMatrix(); + auto feild_to_tag = tag_pose.ToMatrix(); + auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + + Eigen::Vector3d projected_point = + camera_matrix * PI * camera_to_tag * + localization::kapriltag_corners_eigen_homogenized[corner_index]; + auto normalized_point = projected_point / projected_point[0]; + + auto normalized_points_d = normalized_point - image_point; + + // clang-format off + auto projected_point_d = (Eigen::Vector3d() << + -normalized_points_d[1] * projected_point[1] / (projected_point[0] * projected_point[0]) + -normalized_points_d[2] * projected_point[2] / (projected_point[0] * projected_point[0]), + normalized_points_d[1] / projected_point[0], + normalized_points_d[2] / projected_point[0] + ).finished(); + // clang-format on + + auto camera_matrix_xd = camera_matrix.transpose() * projected_point_d; + auto PI_xd = PI.transpose() * camera_matrix_xd; + auto camera_to_tag_d = + PI_xd * localization::kapriltag_corners_eigen_homogenized[corner_index] + .transpose(); + return camera_to_tag_d; } // TODO: Tolerance is quite high. Find out what causes the loss in precision @@ -58,21 +116,7 @@ void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, EXPECT_NEAR(image_point.y, projected_points[2], tolerance); } -TEST(ForwardTest, Basic) { // NOLINT - camera_constant_t camera_constant = - GetCameraConstants().at("second_bot_right"); - camera_constant.extrinsics_path = - "/bos/constants/misc/dev_orin_extrinsics.json"; - CHECK(camera_constant.intrinsics_path.has_value()); - - SquareSolver square_solver(camera_constant); - const auto detector = std::make_unique( - camera_constant.frame_width.value(), camera_constant.frame_height.value(), - ReadIntrinsics(camera_constant.intrinsics_path.value())); - - const auto camera_matrix = CameraMatrixFromJson( - utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); - +TEST_F(ForwardTest, ProjectTest) { // NOLINT // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners // feild_to_camera = feild_to_tag * 180_yaw * camera_to_tag.inv // feild_to_camera * camera_to_tag = feild_to_tag * 180_yaw @@ -88,21 +132,47 @@ TEST(ForwardTest, Basic) { // NOLINT cv::Mat image = cv::imread(image_paths[i]); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector->GetTagDetections(timestamped_frame); + auto detections = detector_->GetTagDetections(timestamped_frame); if (detections.empty()) { continue; } auto detection = detections[0]; auto square_solver_solution = - square_solver.EstimatePosition({detection})[0]; + square_solver_->EstimatePosition({detection})[0]; for (int j = 0; j < 4; j++) { - auto projected_points = ProjectPoints( - square_solver_solution.pose, - localization::kapriltag_layout.GetTagPose(detection.tag_id).value(), - camera_matrix, j); + auto projected_points = + ProjectPoints(square_solver_solution.pose, + kapriltag_layout.GetTagPose(detection.tag_id).value(), + camera_matrix_, j); CheckIsEqual(detection.corners[j], projected_points); } } } + +TEST_F(ForwardTest, TestDerrivative) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); + for (int camera_index = 0; camera_index < 4; camera_index++) { + auto image_point = + (Eigen::Vector3d() << 1, detection.corners[camera_index].x, + detection.corners[camera_index].y) + .finished(); + + camera_to_tag_d += CalculateDerivative( + square_solver_solution.pose, + kapriltag_layout.GetTagPose(detection.tag_id).value(), camera_matrix_, + image_point, camera_index); + } + + LOG(INFO) << "derrivative\n" << camera_to_tag_d; +} From ef320f828975a228e095279f986b0d71e1c04618 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 24 Apr 2026 17:35:24 +0000 Subject: [PATCH 07/47] transform3d Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 60 ++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 9934f807..8f9dc727 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -23,6 +23,47 @@ using localization::tag_detection_t; using utils::CameraMatrixFromJson; using utils::ReadIntrinsics; +using transform3d_t = struct Transfrom3d { + // Translation in meters, rotation in radians + double t_x; + double t_y; + double t_z; + double r_x; + double r_y; + double r_z; + + Transfrom3d(frc::Pose3d pose) + : t_x(pose.Translation().X().value()), + t_y(pose.Translation().Y().value()), + t_z(pose.Translation().Z().value()), + r_x(pose.Rotation().X().value()), + r_y(pose.Rotation().Y().value()), + r_z(pose.Rotation().Z().value()) {} + + Transfrom3d(frc::Transform3d pose) + : t_x(pose.Translation().X().value()), + t_y(pose.Translation().Y().value()), + t_z(pose.Translation().Z().value()), + r_x(pose.Rotation().X().value()), + r_y(pose.Rotation().Y().value()), + r_z(pose.Rotation().Z().value()) {} + + auto ToEigen() -> Eigen::Matrix4d { + Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); + + // Rotation: ZYX euler convention (common for robot poses) + Eigen::Matrix3d rot = (Eigen::AngleAxisd(r_z, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(r_y, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(r_x, Eigen::Vector3d::UnitX())) + .toRotationMatrix(); + + m.block<3, 3>(0, 0) = rot; + m.block<3, 1>(0, 3) = Eigen::Vector3d{t_x, t_y, t_z}; + + return m; + } +}; + // clang-format off const Eigen::Matrix PI = (Eigen::Matrix() << @@ -176,3 +217,22 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT LOG(INFO) << "derrivative\n" << camera_to_tag_d; } + +TEST_F(ForwardTest, TestTranfrom3d) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + transform3d_t transform(square_solver_solution.pose); + + LOG(INFO) << "pose.ToMatrix()\n" << square_solver_solution.pose.ToMatrix(); + LOG(INFO) << "transform.ToEigen()\n" << transform.ToEigen(); + + EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( + transform.ToEigen(), 1e-9)); +} From 08e4ad22a26c6002e754925ae5aec542512274b7 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 05:35:42 +0000 Subject: [PATCH 08/47] wip Signed-off-by: Charlie Huang --- src/localization/square_solver.cc | 16 +- src/test/unit_test/CMakeLists.txt | 2 +- src/test/unit_test/forward_test.cc | 284 ++++++++++++++++++++++++----- 3 files changed, 249 insertions(+), 53 deletions(-) diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index d8da439d..3ac00446 100644 --- a/src/localization/square_solver.cc +++ b/src/localization/square_solver.cc @@ -62,7 +62,19 @@ auto SquareSolver::EstimatePositionAmbiguous( cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, distortion_coefficients_, rvecs, tvecs, false, - cv::SOLVEPNP_IPPE_SQUARE); + cv::SOLVEPNP_ITERATIVE); + + cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, + distortion_coefficients_, rvecs, tvecs, true, + cv::SOLVEPNP_ITERATIVE); + + cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, + distortion_coefficients_, rvecs, tvecs, true, + cv::SOLVEPNP_ITERATIVE); + + cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, + distortion_coefficients_, rvecs, tvecs, true, + cv::SOLVEPNP_ITERATIVE); if (rvecs.size() < 2 || tvecs.size() < 2) { continue; @@ -115,7 +127,7 @@ auto SquareSolver::EstimatePosition( try { cv::solvePnP(tag_corners_, detection.corners, camera_matrix_, distortion_coefficients_, rvec, tvec, false, - cv::SOLVEPNP_IPPE_SQUARE); + cv::SOLVEPNP_ITERATIVE); } catch (std::exception& e) { LOG(WARNING) << "Caught solve pnp exception:\n" << e.what(); return {}; diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index dbe09225..f0bee837 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -16,7 +16,7 @@ add_executable(matrix matrix.cc) target_link_libraries(matrix PRIVATE localization utils GTest::gtest_main) add_executable(forward_test forward_test.cc) -target_link_libraries(forward_test PRIVATE localization utils GTest::gtest_main) +target_link_libraries(forward_test PRIVATE localization utils GTest::gtest_main XAD::xad) include(GoogleTest) gtest_discover_tests(multi_tag_test) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 8f9dc727..15cb3c4e 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -1,4 +1,6 @@ #include +#include +#include #include #include "src/localization/joint_solver.h" #include "src/localization/opencv_apriltag_detector.h" @@ -8,7 +10,10 @@ #include "src/utils/transform.h" #define IMAGE_STRIDE 4 -#define LOG_PATH "/bos-logs/log181/left" +#define LOG_PATH "/bos-logs/log181/right" +#define LR 0.01 +#define EPOCHS 100 +#define NORMALIZATION 100 namespace fs = std::filesystem; @@ -23,24 +28,63 @@ using localization::tag_detection_t; using utils::CameraMatrixFromJson; using utils::ReadIntrinsics; -using transform3d_t = struct Transfrom3d { +using transform3d_derrivative_t = struct Transfrom3dDerrivative { + double t_x = 0; + double t_y = 0; + double t_z = 0; + double r_x = 0; + double r_y = 0; + double r_z = 0; + + auto operator+(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x + other.t_x, + .t_y = t_y + other.t_y, + .t_z = t_z + other.t_z, + .r_x = r_x + other.r_x, + .r_y = r_y + other.r_y, + .r_z = r_z + other.r_z}; + } +}; + +auto operator<<(std::ostream& os, const Transfrom3dDerrivative& v) + -> std::ostream& { + os << "tx: " << v.t_x << "\n"; + os << "ty: " << v.t_y << "\n"; + os << "tz: " << v.t_z << "\n"; + + os << "rx: " << v.r_x << "\n"; + os << "ry: " << v.r_y << "\n"; + os << "rz: " << v.r_z << "\n"; + return os; +} + +struct DifferntiableTransform3d { + using mode = xad::adj; + using tape_type = mode::tape_type; + using AD = mode::active_type; + // Translation in meters, rotation in radians - double t_x; - double t_y; - double t_z; - double r_x; - double r_y; - double r_z; - - Transfrom3d(frc::Pose3d pose) + AD t_x; + AD t_y; + AD t_z; + AD r_x; + AD r_y; + AD r_z; + std::array, 4> matrix; + tape_type tape; + + DifferntiableTransform3d(frc::Pose3d pose) : t_x(pose.Translation().X().value()), t_y(pose.Translation().Y().value()), t_z(pose.Translation().Z().value()), r_x(pose.Rotation().X().value()), r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} + r_z(pose.Rotation().Z().value()) { + RegisterInputs(); + RegisterOutputs(); + } - Transfrom3d(frc::Transform3d pose) + DifferntiableTransform3d(frc::Transform3d pose) : t_x(pose.Translation().X().value()), t_y(pose.Translation().Y().value()), t_z(pose.Translation().Z().value()), @@ -48,22 +92,114 @@ using transform3d_t = struct Transfrom3d { r_y(pose.Rotation().Y().value()), r_z(pose.Rotation().Z().value()) {} - auto ToEigen() -> Eigen::Matrix4d { - Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); + void Update(transform3d_derrivative_t derrivative) { + t_x -= derrivative.t_x * LR; + t_y -= derrivative.t_y * LR; + t_z -= derrivative.t_z * LR; - // Rotation: ZYX euler convention (common for robot poses) - Eigen::Matrix3d rot = (Eigen::AngleAxisd(r_z, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(r_y, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(r_x, Eigen::Vector3d::UnitX())) - .toRotationMatrix(); + r_x -= derrivative.r_x * LR; + r_y -= derrivative.r_y * LR; + r_z -= derrivative.r_z * LR; + } - m.block<3, 3>(0, 0) = rot; - m.block<3, 1>(0, 3) = Eigen::Vector3d{t_x, t_y, t_z}; + void RegisterInputs() { + tape.registerInput(r_x); + tape.registerInput(r_y); + tape.registerInput(r_z); - return m; + tape.registerInput(t_x); + tape.registerInput(t_y); + tape.registerInput(t_z); + } + + void RegisterOutputs() { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + tape.registerOutput(matrix[i][j]); + } + } + } + + auto ToEigen() -> Eigen::Matrix4d { + // clang-format off + return (Eigen::Matrix() << + matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), + matrix[1][0].value(), matrix[1][1].value(), matrix[1][2].value(), matrix[1][3].value(), + matrix[2][0].value(), matrix[2][1].value(), matrix[2][2].value(), matrix[2][3].value(), + matrix[3][0].value(), matrix[3][1].value(), matrix[3][2].value(), matrix[3][3].value()) + .finished(); + // clang-format on + } + void CalculateMatrix() { + tape.newRecording(); + RegisterInputs(); + + AD cos_x = xad::cos(r_x); + AD cos_y = xad::cos(r_y); + AD cos_z = xad::cos(r_z); + + AD sin_x = xad::sin(r_x); + AD sin_y = xad::sin(r_y); + AD sin_z = xad::sin(r_z); + + // Rotation + matrix[0][0] = cos_z * cos_y; + matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; + matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; + + matrix[1][0] = sin_z * cos_y; + matrix[1][1] = sin_z * sin_y * sin_x + cos_z * cos_x; + matrix[1][2] = sin_z * sin_y * cos_x - cos_z * sin_x; + + matrix[2][0] = -sin_y; + matrix[2][1] = cos_y * sin_x; + matrix[2][2] = cos_y * cos_x; + + // Translation + matrix[0][3] = t_x; + matrix[1][3] = t_y; + matrix[2][3] = t_z; + + matrix[3][0] = 0; + matrix[3][1] = 0; + matrix[3][2] = 0; + matrix[3][3] = 1; + } + + auto BackPropagate(const Eigen::Matrix4d& next_derrivative) + -> transform3d_derrivative_t { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + tape.registerOutput(matrix[i][j]); + derivative(matrix[i][j]) = next_derrivative(i, j); + } + } + tape.computeAdjoints(); + transform3d_derrivative_t derrivative{ + .t_x = xad::derivative(t_x), + .t_y = xad::derivative(t_y), + .t_z = xad::derivative(t_z), + .r_x = xad::derivative(r_x), + .r_y = xad::derivative(r_y), + .r_z = xad::derivative(r_z), + }; + tape.clearDerivatives(); + return derrivative; } }; +auto operator<<(std::ostream& os, const DifferntiableTransform3d& v) + -> std::ostream& { + os << "tx: " << v.t_x << "\n"; + os << "ty: " << v.t_y << "\n"; + os << "tz: " << v.t_z << "\n"; + + os << "rx: " << v.r_x << "\n"; + os << "ry: " << v.r_y << "\n"; + os << "rz: " << v.r_z << "\n"; + return os; +} + // clang-format off const Eigen::Matrix PI = (Eigen::Matrix() << @@ -93,6 +229,8 @@ class ForwardTest : public ::testing::Test { ReadIntrinsics(camera_constant_.intrinsics_path.value())); camera_matrix_ = CameraMatrixFromJson( ReadIntrinsics(camera_constant_.intrinsics_path.value())); + normalized_camera_matrix_ = camera_matrix_ / NORMALIZATION; + normalized_camera_matrix_(0, 0) = 1; } void TearDown() override {} @@ -101,6 +239,7 @@ class ForwardTest : public ::testing::Test { std::unique_ptr square_solver_; std::unique_ptr detector_; Eigen::Matrix3d camera_matrix_; + Eigen::Matrix3d normalized_camera_matrix_; }; auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, @@ -110,21 +249,31 @@ auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, auto feild_to_tag = tag_pose.ToMatrix(); auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; - Eigen::Vector3d projected_points = + Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_tag * localization::kapriltag_corners_eigen_homogenized[corner_index]; - auto normalized_points = projected_points / projected_points[0]; - return normalized_points; + auto normalized_point = projected_point / projected_point[0]; + return normalized_point; } -auto CalculateDerivative(const frc::Pose3d& camera_pose, - const frc::Pose3d& tag_pose, +auto CalculateLoss(const Eigen::Matrix4d& camera_to_tag, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, int corner_index) + -> double { + Eigen::Vector3d projected_point = + camera_matrix * PI * camera_to_tag * + localization::kapriltag_corners_eigen_homogenized[corner_index]; + auto normalized_point = projected_point / projected_point[0]; + + auto normalized_points_d = normalized_point - image_point; + + return normalized_points_d.array().square().sum(); +} + +auto CalculateDerivative(const Eigen::Matrix4d& camera_to_tag, const Eigen::Matrix3d& camera_matrix, const Eigen::Vector3d& image_point, int corner_index) -> Eigen::Matrix4d { - auto feild_to_camera = camera_pose.ToMatrix(); - auto feild_to_tag = tag_pose.ToMatrix(); - auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_tag * @@ -135,15 +284,17 @@ auto CalculateDerivative(const frc::Pose3d& camera_pose, // clang-format off auto projected_point_d = (Eigen::Vector3d() << - -normalized_points_d[1] * projected_point[1] / (projected_point[0] * projected_point[0]) - -normalized_points_d[2] * projected_point[2] / (projected_point[0] * projected_point[0]), + (-normalized_points_d[1] * projected_point[1]) / (projected_point[0] * projected_point[0]) + + (-normalized_points_d[2] * projected_point[1]) / (projected_point[0] * projected_point[0]), normalized_points_d[1] / projected_point[0], normalized_points_d[2] / projected_point[0] ).finished(); // clang-format on auto camera_matrix_xd = camera_matrix.transpose() * projected_point_d; + auto PI_xd = PI.transpose() * camera_matrix_xd; + auto camera_to_tag_d = PI_xd * localization::kapriltag_corners_eigen_homogenized[corner_index] .transpose(); @@ -187,7 +338,15 @@ TEST_F(ForwardTest, ProjectTest) { // NOLINT ProjectPoints(square_solver_solution.pose, kapriltag_layout.GetTagPose(detection.tag_id).value(), camera_matrix_, j); - CheckIsEqual(detection.corners[j], projected_points); + + auto projected_points_normalized = + ProjectPoints(square_solver_solution.pose, + kapriltag_layout.GetTagPose(detection.tag_id).value(), + normalized_camera_matrix_, j); + + CheckIsEqual(detection.corners[j], projected_points, 1); + CheckIsEqual(detection.corners[j] / NORMALIZATION, + projected_points_normalized, 1.0 / NORMALIZATION); } } } @@ -202,20 +361,40 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT auto square_solver_solution = square_solver_->EstimatePosition({detection})[0]; - Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); - for (int camera_index = 0; camera_index < 4; camera_index++) { - auto image_point = - (Eigen::Vector3d() << 1, detection.corners[camera_index].x, - detection.corners[camera_index].y) - .finished(); - - camera_to_tag_d += CalculateDerivative( - square_solver_solution.pose, - kapriltag_layout.GetTagPose(detection.tag_id).value(), camera_matrix_, - image_point, camera_index); - } + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + auto feild_to_camera = square_solver_solution.pose.ToMatrix(); + + Eigen::Matrix4d camera_to_tag = + feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + + // Noise + camera_to_tag(0, 0) += 0.1; + camera_to_tag(0, 3) += 0.1; + camera_to_tag(0, 2) += 0.1; + camera_to_tag(1, 3) += 0.1; + + double loss = 0; + for (int epoch = 0; epoch < EPOCHS; epoch++) { + Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); + loss = 0; + for (int corner_index = 0; corner_index < 4; corner_index++) { + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + camera_to_tag_d += CalculateDerivative( + camera_to_tag, normalized_camera_matrix_, image_point, corner_index); - LOG(INFO) << "derrivative\n" << camera_to_tag_d; + loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, + image_point, corner_index); + } + camera_to_tag -= camera_to_tag_d * LR; + LOG(INFO) << loss; + } + ASSERT_LT(loss, 0.001); } TEST_F(ForwardTest, TestTranfrom3d) { // NOLINT @@ -228,11 +407,16 @@ TEST_F(ForwardTest, TestTranfrom3d) { // NOLINT auto square_solver_solution = square_solver_->EstimatePosition({detection})[0]; - transform3d_t transform(square_solver_solution.pose); - - LOG(INFO) << "pose.ToMatrix()\n" << square_solver_solution.pose.ToMatrix(); - LOG(INFO) << "transform.ToEigen()\n" << transform.ToEigen(); + DifferntiableTransform3d transform(square_solver_solution.pose); + transform.CalculateMatrix(); EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( transform.ToEigen(), 1e-9)); + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + EXPECT_NEAR(square_solver_solution.pose.ToMatrix()(i, j), + transform.matrix[i][j].value(), 1e-9); + } + } } From aee456a4afccc8a4b3733f9eced088f67d00c9ce Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 06:02:35 +0000 Subject: [PATCH 09/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 114 +++++++++++++++++++++++++---- 1 file changed, 99 insertions(+), 15 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 15cb3c4e..f13f9d09 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -71,7 +71,7 @@ struct DifferntiableTransform3d { AD r_y; AD r_z; std::array, 4> matrix; - tape_type tape; + // tape_type tape; DifferntiableTransform3d(frc::Pose3d pose) : t_x(pose.Translation().X().value()), @@ -92,6 +92,15 @@ struct DifferntiableTransform3d { r_y(pose.Rotation().Y().value()), r_z(pose.Rotation().Z().value()) {} + DifferntiableTransform3d(Eigen::Matrix4d matrix) + : t_x(matrix(0, 3)), t_y(matrix(1, 3)), t_z(matrix(2, 3)) { + Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); + Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); + r_x = euler(2); + r_y = euler(1); + r_z = euler(0); + } + void Update(transform3d_derrivative_t derrivative) { t_x -= derrivative.t_x * LR; t_y -= derrivative.t_y * LR; @@ -103,19 +112,19 @@ struct DifferntiableTransform3d { } void RegisterInputs() { - tape.registerInput(r_x); - tape.registerInput(r_y); - tape.registerInput(r_z); - - tape.registerInput(t_x); - tape.registerInput(t_y); - tape.registerInput(t_z); + // tape.registerInput(r_x); + // tape.registerInput(r_y); + // tape.registerInput(r_z); + // + // tape.registerInput(t_x); + // tape.registerInput(t_y); + // tape.registerInput(t_z); } void RegisterOutputs() { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); + // tape.registerOutput(matrix[i][j]); } } } @@ -131,7 +140,7 @@ struct DifferntiableTransform3d { // clang-format on } void CalculateMatrix() { - tape.newRecording(); + // tape.newRecording(); RegisterInputs(); AD cos_x = xad::cos(r_x); @@ -170,11 +179,11 @@ struct DifferntiableTransform3d { -> transform3d_derrivative_t { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); + // tape.registerOutput(matrix[i][j]); derivative(matrix[i][j]) = next_derrivative(i, j); } } - tape.computeAdjoints(); + // tape.computeAdjoints(); transform3d_derrivative_t derrivative{ .t_x = xad::derivative(t_x), .t_y = xad::derivative(t_y), @@ -183,7 +192,7 @@ struct DifferntiableTransform3d { .r_y = xad::derivative(r_y), .r_z = xad::derivative(r_z), }; - tape.clearDerivatives(); + // tape.clearDerivatives(); return derrivative; } }; @@ -392,12 +401,41 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT image_point, corner_index); } camera_to_tag -= camera_to_tag_d * LR; - LOG(INFO) << loss; + // LOG(INFO) << loss; } ASSERT_LT(loss, 0.001); } -TEST_F(ForwardTest, TestTranfrom3d) { // NOLINT +TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); + DifferntiableTransform3d transform_from_eigen( + square_solver_solution.pose.ToMatrix()); + + const double tolerance = 1e-7; + ASSERT_NEAR(transform_from_pose.t_x.value(), transform_from_eigen.t_x.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.t_y.value(), transform_from_eigen.t_y.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), + tolerance); + + ASSERT_NEAR(transform_from_pose.r_x.value(), transform_from_eigen.r_x.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.r_y.value(), transform_from_eigen.r_y.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), + tolerance); +} +TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; @@ -420,3 +458,49 @@ TEST_F(ForwardTest, TestTranfrom3d) { // NOLINT } } } + +TEST_F(ForwardTest, TestBackpropagation) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + auto feild_to_camera = square_solver_solution.pose.ToMatrix(); + + Eigen::Matrix4d camera_to_tag = + feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + + // Noise + camera_to_tag(0, 0) += 0.1; + camera_to_tag(0, 3) += 0.1; + camera_to_tag(0, 2) += 0.1; + camera_to_tag(1, 3) += 0.1; + + double loss = 0; + for (int epoch = 0; epoch < EPOCHS; epoch++) { + Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); + loss = 0; + for (int corner_index = 0; corner_index < 4; corner_index++) { + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + camera_to_tag_d += CalculateDerivative( + camera_to_tag, normalized_camera_matrix_, image_point, corner_index); + + loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, + image_point, corner_index); + } + camera_to_tag -= camera_to_tag_d * LR; + // LOG(INFO) << loss; + } + ASSERT_LT(loss, 0.001); +} From f3205a4c3dcb0451b9bf2922fd2cc8ca6404e6b5 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 06:37:04 +0000 Subject: [PATCH 10/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 94 +++++++++++++++++------------- 1 file changed, 54 insertions(+), 40 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index f13f9d09..ff589a18 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -12,7 +12,7 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" #define LR 0.01 -#define EPOCHS 100 +#define EPOCHS 1000 #define NORMALIZATION 100 namespace fs = std::filesystem; @@ -28,6 +28,10 @@ using localization::tag_detection_t; using utils::CameraMatrixFromJson; using utils::ReadIntrinsics; +using mode = xad::adj; +using tape_type = mode::tape_type; +using AD = mode::active_type; + using transform3d_derrivative_t = struct Transfrom3dDerrivative { double t_x = 0; double t_y = 0; @@ -59,9 +63,6 @@ auto operator<<(std::ostream& os, const Transfrom3dDerrivative& v) } struct DifferntiableTransform3d { - using mode = xad::adj; - using tape_type = mode::tape_type; - using AD = mode::active_type; // Translation in meters, rotation in radians AD t_x; @@ -71,7 +72,6 @@ struct DifferntiableTransform3d { AD r_y; AD r_z; std::array, 4> matrix; - // tape_type tape; DifferntiableTransform3d(frc::Pose3d pose) : t_x(pose.Translation().X().value()), @@ -79,10 +79,7 @@ struct DifferntiableTransform3d { t_z(pose.Translation().Z().value()), r_x(pose.Rotation().X().value()), r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) { - RegisterInputs(); - RegisterOutputs(); - } + r_z(pose.Rotation().Z().value()) {} DifferntiableTransform3d(frc::Transform3d pose) : t_x(pose.Translation().X().value()), @@ -111,25 +108,25 @@ struct DifferntiableTransform3d { r_z -= derrivative.r_z * LR; } - void RegisterInputs() { - // tape.registerInput(r_x); - // tape.registerInput(r_y); - // tape.registerInput(r_z); - // - // tape.registerInput(t_x); - // tape.registerInput(t_y); - // tape.registerInput(t_z); + void RegisterInputs(tape_type& tape) { + tape.registerInput(r_x); + tape.registerInput(r_y); + tape.registerInput(r_z); + + tape.registerInput(t_x); + tape.registerInput(t_y); + tape.registerInput(t_z); } - void RegisterOutputs() { + void RegisterOutputs(tape_type& tape) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { - // tape.registerOutput(matrix[i][j]); + tape.registerOutput(matrix[i][j]); } } } - auto ToEigen() -> Eigen::Matrix4d { + auto ToEigen() -> Eigen::Matrix4d const { // clang-format off return (Eigen::Matrix() << matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), @@ -140,9 +137,6 @@ struct DifferntiableTransform3d { // clang-format on } void CalculateMatrix() { - // tape.newRecording(); - RegisterInputs(); - AD cos_x = xad::cos(r_x); AD cos_y = xad::cos(r_y); AD cos_z = xad::cos(r_z); @@ -175,15 +169,15 @@ struct DifferntiableTransform3d { matrix[3][3] = 1; } - auto BackPropagate(const Eigen::Matrix4d& next_derrivative) + auto BackPropagate(const Eigen::Matrix4d& next_derrivative, tape_type& tape) -> transform3d_derrivative_t { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { - // tape.registerOutput(matrix[i][j]); + tape.registerOutput(matrix[i][j]); derivative(matrix[i][j]) = next_derrivative(i, j); } } - // tape.computeAdjoints(); + tape.computeAdjoints(); transform3d_derrivative_t derrivative{ .t_x = xad::derivative(t_x), .t_y = xad::derivative(t_y), @@ -192,9 +186,17 @@ struct DifferntiableTransform3d { .r_y = xad::derivative(r_y), .r_z = xad::derivative(r_z), }; - // tape.clearDerivatives(); return derrivative; } + void Apply(const transform3d_derrivative_t& derrivative) { + t_x -= derrivative.t_x * LR; + t_y -= derrivative.t_y * LR; + t_z -= derrivative.t_z * LR; + + r_x -= derrivative.r_x * LR; + r_y -= derrivative.r_y * LR; + r_z -= derrivative.r_z * LR; + } }; auto operator<<(std::ostream& os, const DifferntiableTransform3d& v) @@ -401,7 +403,6 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT image_point, corner_index); } camera_to_tag -= camera_to_tag_d * LR; - // LOG(INFO) << loss; } ASSERT_LT(loss, 0.001); } @@ -474,33 +475,46 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT auto feild_to_camera = square_solver_solution.pose.ToMatrix(); - Eigen::Matrix4d camera_to_tag = - feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + DifferntiableTransform3d camera_to_tag(feild_to_camera.inverse() * + feild_to_tag * rotate_yaw); // Noise - camera_to_tag(0, 0) += 0.1; - camera_to_tag(0, 3) += 0.1; - camera_to_tag(0, 2) += 0.1; - camera_to_tag(1, 3) += 0.1; + camera_to_tag.t_x += 0.1; + camera_to_tag.t_y += 0.1; + camera_to_tag.t_z += 0.1; + + camera_to_tag.r_x += 0.1; + camera_to_tag.r_y += 0.1; + camera_to_tag.r_z += 0.1; + + tape_type tape; double loss = 0; for (int epoch = 0; epoch < EPOCHS; epoch++) { - Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); loss = 0; + transform3d_derrivative_t derrivative; + for (int corner_index = 0; corner_index < 4; corner_index++) { + camera_to_tag.CalculateMatrix(); + camera_to_tag.RegisterInputs(tape); auto image_point = (Eigen::Vector3d() << 1, detection.corners[corner_index].x / NORMALIZATION, detection.corners[corner_index].y / NORMALIZATION) .finished(); - camera_to_tag_d += CalculateDerivative( - camera_to_tag, normalized_camera_matrix_, image_point, corner_index); + auto camera_to_tag_d = CalculateDerivative(camera_to_tag.ToEigen(), + normalized_camera_matrix_, + image_point, corner_index); + camera_to_tag.RegisterOutputs(tape); + derrivative = + derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); - loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, + loss += CalculateLoss(camera_to_tag.ToEigen(), normalized_camera_matrix_, image_point, corner_index); + tape.newRecording(); } - camera_to_tag -= camera_to_tag_d * LR; - // LOG(INFO) << loss; + LOG(INFO) << loss; + camera_to_tag.Apply(derrivative); } ASSERT_LT(loss, 0.001); } From 96d5f74af423b4ce031edebd1082f046bebb83f1 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 06:37:23 +0000 Subject: [PATCH 11/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index ff589a18..8ba2b2b7 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -513,7 +513,6 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT image_point, corner_index); tape.newRecording(); } - LOG(INFO) << loss; camera_to_tag.Apply(derrivative); } ASSERT_LT(loss, 0.001); From 88adebc870fe161f20f6a3a1afd0ebfc9b88df04 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 07:07:14 +0000 Subject: [PATCH 12/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 8ba2b2b7..17b324f4 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -497,19 +497,20 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT for (int corner_index = 0; corner_index < 4; corner_index++) { camera_to_tag.CalculateMatrix(); camera_to_tag.RegisterInputs(tape); + const auto camera_to_tag_eigen = camera_to_tag.ToEigen(); auto image_point = (Eigen::Vector3d() << 1, detection.corners[corner_index].x / NORMALIZATION, detection.corners[corner_index].y / NORMALIZATION) .finished(); - auto camera_to_tag_d = CalculateDerivative(camera_to_tag.ToEigen(), - normalized_camera_matrix_, - image_point, corner_index); + auto camera_to_tag_d = + CalculateDerivative(camera_to_tag_eigen, normalized_camera_matrix_, + image_point, corner_index); camera_to_tag.RegisterOutputs(tape); derrivative = derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); - loss += CalculateLoss(camera_to_tag.ToEigen(), normalized_camera_matrix_, + loss += CalculateLoss(camera_to_tag_eigen, normalized_camera_matrix_, image_point, corner_index); tape.newRecording(); } From 48fa97c3d41638ecc53f0474ef8da08a2001029a Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 07:15:06 +0000 Subject: [PATCH 13/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 17b324f4..edf65492 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -7,12 +7,13 @@ #include "src/localization/square_solver.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" +#include "src/utils/timer.h" #include "src/utils/transform.h" #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" -#define LR 0.01 -#define EPOCHS 1000 +#define LR 0.05 +#define EPOCHS 500 #define NORMALIZATION 100 namespace fs = std::filesystem; @@ -461,7 +462,7 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT } TEST_F(ForwardTest, TestBackpropagation) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + cv::Mat image = cv::imread("/bos-logs/log181/right/5.647740.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; auto detections = detector_->GetTagDetections(timestamped_frame); @@ -490,6 +491,7 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT tape_type tape; double loss = 0; + utils::Timer solve_timer("solve", false); for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; transform3d_derrivative_t derrivative; @@ -516,5 +518,6 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT } camera_to_tag.Apply(derrivative); } + ASSERT_LT(solve_timer.Stop(), 1.0 / 250); ASSERT_LT(loss, 0.001); } From 69dc8dc218a9e38a974321af4e88fa9a7f9fa244 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 07:21:45 +0000 Subject: [PATCH 14/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index edf65492..5b775fed 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -13,7 +13,7 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" #define LR 0.05 -#define EPOCHS 500 +#define EPOCHS 1000 #define NORMALIZATION 100 namespace fs = std::filesystem; @@ -462,10 +462,11 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT } TEST_F(ForwardTest, TestBackpropagation) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/5.647740.jpg"); + cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; auto detections = detector_->GetTagDetections(timestamped_frame); + LOG(INFO) << detections.size(); auto detection = detections[0]; auto square_solver_solution = @@ -519,5 +520,5 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT camera_to_tag.Apply(derrivative); } ASSERT_LT(solve_timer.Stop(), 1.0 / 250); - ASSERT_LT(loss, 0.001); + ASSERT_LT(loss, 0.01); } From 02e3b8e96dc5a8d3d8755db22b1aba2583ef20a6 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 07:35:32 +0000 Subject: [PATCH 15/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 343 +++++++++++++++-------------- 1 file changed, 178 insertions(+), 165 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 5b775fed..71aab05b 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -231,8 +231,6 @@ class ForwardTest : public ::testing::Test { protected: void SetUp() override { camera_constant_ = GetCameraConstants().at("second_bot_right"); - camera_constant_.extrinsics_path = - "/bos/constants/misc/dev_orin_extrinsics.json"; square_solver_ = std::make_unique(camera_constant_); detector_ = std::make_unique( @@ -243,6 +241,11 @@ class ForwardTest : public ::testing::Test { ReadIntrinsics(camera_constant_.intrinsics_path.value())); normalized_camera_matrix_ = camera_matrix_ / NORMALIZATION; normalized_camera_matrix_(0, 0) = 1; + + camera_to_robot_ = + utils::ExtrinsicsJsonToCameraToRobot( + utils::ReadExtrinsics(camera_constant_.extrinsics_path.value())) + .ToMatrix(); } void TearDown() override {} @@ -252,14 +255,17 @@ class ForwardTest : public ::testing::Test { std::unique_ptr detector_; Eigen::Matrix3d camera_matrix_; Eigen::Matrix3d normalized_camera_matrix_; + Eigen::Matrix4d camera_to_robot_; }; auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, - const Eigen::Matrix3d& camera_matrix, int corner_index) + const Eigen::Matrix3d& camera_matrix, + const Eigen::Matrix4d& camera_to_robot, int corner_index) -> Eigen::Vector3d { auto feild_to_camera = camera_pose.ToMatrix(); auto feild_to_tag = tag_pose.ToMatrix(); - auto camera_to_tag = feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + auto camera_to_tag = + camera_to_robot * feild_to_camera.inverse() * feild_to_tag * rotate_yaw; Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_tag * @@ -322,9 +328,16 @@ void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, TEST_F(ForwardTest, ProjectTest) { // NOLINT // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners + // // feild_to_camera = feild_to_tag * 180_yaw * camera_to_tag.inv // feild_to_camera * camera_to_tag = feild_to_tag * 180_yaw // camera_to_tag = feild_to_camera.inv * feild_to_tag * 180_yaw + // + // feild_to_robot = feild_to_tag * 180_yaw * camera_to_tag.inv * camera_to_robot + // feild_to_robot * camera_to_robot.inv = feild_to_tag * 180_yaw * camera_to_tag.inv + // feild_to_robot * camera_to_robot.inv * camera_to_tag = feild_to_tag * 180_yaw + // camera_to_robot.inv * camera_to_tag = feild_to_robot.inv * feild_to_tag * 180_yaw + // camera_to_tag = camera_to_robot * feild_to_robot.inv * feild_to_tag * 180_yaw std::vector image_paths; for (const auto& file : fs::directory_iterator(LOG_PATH)) { @@ -349,12 +362,12 @@ TEST_F(ForwardTest, ProjectTest) { // NOLINT auto projected_points = ProjectPoints(square_solver_solution.pose, kapriltag_layout.GetTagPose(detection.tag_id).value(), - camera_matrix_, j); + camera_matrix_, camera_to_robot_, j); auto projected_points_normalized = ProjectPoints(square_solver_solution.pose, kapriltag_layout.GetTagPose(detection.tag_id).value(), - normalized_camera_matrix_, j); + normalized_camera_matrix_, camera_to_robot_, j); CheckIsEqual(detection.corners[j], projected_points, 1); CheckIsEqual(detection.corners[j] / NORMALIZATION, @@ -363,162 +376,162 @@ TEST_F(ForwardTest, ProjectTest) { // NOLINT } } -TEST_F(ForwardTest, TestDerrivative) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); - timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); - auto detection = detections[0]; - - auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; - - auto feild_to_tag = - kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); - - auto feild_to_camera = square_solver_solution.pose.ToMatrix(); - - Eigen::Matrix4d camera_to_tag = - feild_to_camera.inverse() * feild_to_tag * rotate_yaw; - - // Noise - camera_to_tag(0, 0) += 0.1; - camera_to_tag(0, 3) += 0.1; - camera_to_tag(0, 2) += 0.1; - camera_to_tag(1, 3) += 0.1; - - double loss = 0; - for (int epoch = 0; epoch < EPOCHS; epoch++) { - Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); - loss = 0; - for (int corner_index = 0; corner_index < 4; corner_index++) { - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) - .finished(); - - camera_to_tag_d += CalculateDerivative( - camera_to_tag, normalized_camera_matrix_, image_point, corner_index); - - loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, - image_point, corner_index); - } - camera_to_tag -= camera_to_tag_d * LR; - } - ASSERT_LT(loss, 0.001); -} - -TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); - timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); - auto detection = detections[0]; - - auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; - - DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); - DifferntiableTransform3d transform_from_eigen( - square_solver_solution.pose.ToMatrix()); - - const double tolerance = 1e-7; - ASSERT_NEAR(transform_from_pose.t_x.value(), transform_from_eigen.t_x.value(), - tolerance); - ASSERT_NEAR(transform_from_pose.t_y.value(), transform_from_eigen.t_y.value(), - tolerance); - ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), - tolerance); - - ASSERT_NEAR(transform_from_pose.r_x.value(), transform_from_eigen.r_x.value(), - tolerance); - ASSERT_NEAR(transform_from_pose.r_y.value(), transform_from_eigen.r_y.value(), - tolerance); - ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), - tolerance); -} -TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); - timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); - auto detection = detections[0]; - - auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; - - DifferntiableTransform3d transform(square_solver_solution.pose); - transform.CalculateMatrix(); - - EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( - transform.ToEigen(), 1e-9)); - - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - EXPECT_NEAR(square_solver_solution.pose.ToMatrix()(i, j), - transform.matrix[i][j].value(), 1e-9); - } - } -} - -TEST_F(ForwardTest, TestBackpropagation) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); - timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); - LOG(INFO) << detections.size(); - auto detection = detections[0]; - - auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; - - auto feild_to_tag = - kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); - - auto feild_to_camera = square_solver_solution.pose.ToMatrix(); - - DifferntiableTransform3d camera_to_tag(feild_to_camera.inverse() * - feild_to_tag * rotate_yaw); - - // Noise - camera_to_tag.t_x += 0.1; - camera_to_tag.t_y += 0.1; - camera_to_tag.t_z += 0.1; - - camera_to_tag.r_x += 0.1; - camera_to_tag.r_y += 0.1; - camera_to_tag.r_z += 0.1; - - tape_type tape; - - double loss = 0; - utils::Timer solve_timer("solve", false); - for (int epoch = 0; epoch < EPOCHS; epoch++) { - loss = 0; - transform3d_derrivative_t derrivative; - - for (int corner_index = 0; corner_index < 4; corner_index++) { - camera_to_tag.CalculateMatrix(); - camera_to_tag.RegisterInputs(tape); - const auto camera_to_tag_eigen = camera_to_tag.ToEigen(); - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) - .finished(); - - auto camera_to_tag_d = - CalculateDerivative(camera_to_tag_eigen, normalized_camera_matrix_, - image_point, corner_index); - camera_to_tag.RegisterOutputs(tape); - derrivative = - derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); - - loss += CalculateLoss(camera_to_tag_eigen, normalized_camera_matrix_, - image_point, corner_index); - tape.newRecording(); - } - camera_to_tag.Apply(derrivative); - } - ASSERT_LT(solve_timer.Stop(), 1.0 / 250); - ASSERT_LT(loss, 0.01); -} +// TEST_F(ForwardTest, TestDerrivative) { // NOLINT +// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image), .timestamp = 0, .invalid = false}; +// auto detections = detector_->GetTagDetections(timestamped_frame); +// auto detection = detections[0]; +// +// auto square_solver_solution = +// square_solver_->EstimatePosition({detection})[0]; +// +// auto feild_to_tag = +// kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); +// +// auto feild_to_camera = square_solver_solution.pose.ToMatrix(); +// +// Eigen::Matrix4d camera_to_tag = +// feild_to_camera.inverse() * feild_to_tag * rotate_yaw; +// +// // Noise +// camera_to_tag(0, 0) += 0.1; +// camera_to_tag(0, 3) += 0.1; +// camera_to_tag(0, 2) += 0.1; +// camera_to_tag(1, 3) += 0.1; +// +// double loss = 0; +// for (int epoch = 0; epoch < EPOCHS; epoch++) { +// Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); +// loss = 0; +// for (int corner_index = 0; corner_index < 4; corner_index++) { +// auto image_point = (Eigen::Vector3d() << 1, +// detection.corners[corner_index].x / NORMALIZATION, +// detection.corners[corner_index].y / NORMALIZATION) +// .finished(); +// +// camera_to_tag_d += CalculateDerivative( +// camera_to_tag, normalized_camera_matrix_, image_point, corner_index); +// +// loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, +// image_point, corner_index); +// } +// camera_to_tag -= camera_to_tag_d * LR; +// } +// ASSERT_LT(loss, 0.001); +// } +// +// TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT +// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image), .timestamp = 0, .invalid = false}; +// auto detections = detector_->GetTagDetections(timestamped_frame); +// auto detection = detections[0]; +// +// auto square_solver_solution = +// square_solver_->EstimatePosition({detection})[0]; +// +// DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); +// DifferntiableTransform3d transform_from_eigen( +// square_solver_solution.pose.ToMatrix()); +// +// const double tolerance = 1e-7; +// ASSERT_NEAR(transform_from_pose.t_x.value(), transform_from_eigen.t_x.value(), +// tolerance); +// ASSERT_NEAR(transform_from_pose.t_y.value(), transform_from_eigen.t_y.value(), +// tolerance); +// ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), +// tolerance); +// +// ASSERT_NEAR(transform_from_pose.r_x.value(), transform_from_eigen.r_x.value(), +// tolerance); +// ASSERT_NEAR(transform_from_pose.r_y.value(), transform_from_eigen.r_y.value(), +// tolerance); +// ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), +// tolerance); +// } +// TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT +// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image), .timestamp = 0, .invalid = false}; +// auto detections = detector_->GetTagDetections(timestamped_frame); +// auto detection = detections[0]; +// +// auto square_solver_solution = +// square_solver_->EstimatePosition({detection})[0]; +// +// DifferntiableTransform3d transform(square_solver_solution.pose); +// transform.CalculateMatrix(); +// +// EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( +// transform.ToEigen(), 1e-9)); +// +// for (int i = 0; i < 4; i++) { +// for (int j = 0; j < 4; j++) { +// EXPECT_NEAR(square_solver_solution.pose.ToMatrix()(i, j), +// transform.matrix[i][j].value(), 1e-9); +// } +// } +// } +// +// TEST_F(ForwardTest, TestBackpropagation) { // NOLINT +// cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image), .timestamp = 0, .invalid = false}; +// auto detections = detector_->GetTagDetections(timestamped_frame); +// LOG(INFO) << detections.size(); +// auto detection = detections[0]; +// +// auto square_solver_solution = +// square_solver_->EstimatePosition({detection})[0]; +// +// auto feild_to_tag = +// kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); +// +// auto feild_to_camera = square_solver_solution.pose.ToMatrix(); +// +// DifferntiableTransform3d camera_to_tag(feild_to_camera.inverse() * +// feild_to_tag * rotate_yaw); +// +// // Noise +// camera_to_tag.t_x += 0.1; +// camera_to_tag.t_y += 0.1; +// camera_to_tag.t_z += 0.1; +// +// camera_to_tag.r_x += 0.1; +// camera_to_tag.r_y += 0.1; +// camera_to_tag.r_z += 0.1; +// +// tape_type tape; +// +// double loss = 0; +// utils::Timer solve_timer("solve", false); +// for (int epoch = 0; epoch < EPOCHS; epoch++) { +// loss = 0; +// transform3d_derrivative_t derrivative; +// +// for (int corner_index = 0; corner_index < 4; corner_index++) { +// camera_to_tag.CalculateMatrix(); +// camera_to_tag.RegisterInputs(tape); +// const auto camera_to_tag_eigen = camera_to_tag.ToEigen(); +// auto image_point = (Eigen::Vector3d() << 1, +// detection.corners[corner_index].x / NORMALIZATION, +// detection.corners[corner_index].y / NORMALIZATION) +// .finished(); +// +// auto camera_to_tag_d = +// CalculateDerivative(camera_to_tag_eigen, normalized_camera_matrix_, +// image_point, corner_index); +// camera_to_tag.RegisterOutputs(tape); +// derrivative = +// derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); +// +// loss += CalculateLoss(camera_to_tag_eigen, normalized_camera_matrix_, +// image_point, corner_index); +// tape.newRecording(); +// } +// camera_to_tag.Apply(derrivative); +// } +// ASSERT_LT(solve_timer.Stop(), 1.0 / 250); +// ASSERT_LT(loss, 0.01); +// } From fe58c059bf1d93d9944f4ecef74d820c2907dfb6 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 07:40:15 +0000 Subject: [PATCH 16/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 92 +++++++++++++++--------------- 1 file changed, 46 insertions(+), 46 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 71aab05b..cdb6e5ef 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -262,10 +262,10 @@ auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, const Eigen::Matrix3d& camera_matrix, const Eigen::Matrix4d& camera_to_robot, int corner_index) -> Eigen::Vector3d { - auto feild_to_camera = camera_pose.ToMatrix(); + auto feild_to_robot = camera_pose.ToMatrix(); auto feild_to_tag = tag_pose.ToMatrix(); auto camera_to_tag = - camera_to_robot * feild_to_camera.inverse() * feild_to_tag * rotate_yaw; + camera_to_robot * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_tag * @@ -376,50 +376,50 @@ TEST_F(ForwardTest, ProjectTest) { // NOLINT } } -// TEST_F(ForwardTest, TestDerrivative) { // NOLINT -// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); -// timestamped_frame_t timestamped_frame{ -// .frame = std::move(image), .timestamp = 0, .invalid = false}; -// auto detections = detector_->GetTagDetections(timestamped_frame); -// auto detection = detections[0]; -// -// auto square_solver_solution = -// square_solver_->EstimatePosition({detection})[0]; -// -// auto feild_to_tag = -// kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); -// -// auto feild_to_camera = square_solver_solution.pose.ToMatrix(); -// -// Eigen::Matrix4d camera_to_tag = -// feild_to_camera.inverse() * feild_to_tag * rotate_yaw; -// -// // Noise -// camera_to_tag(0, 0) += 0.1; -// camera_to_tag(0, 3) += 0.1; -// camera_to_tag(0, 2) += 0.1; -// camera_to_tag(1, 3) += 0.1; -// -// double loss = 0; -// for (int epoch = 0; epoch < EPOCHS; epoch++) { -// Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); -// loss = 0; -// for (int corner_index = 0; corner_index < 4; corner_index++) { -// auto image_point = (Eigen::Vector3d() << 1, -// detection.corners[corner_index].x / NORMALIZATION, -// detection.corners[corner_index].y / NORMALIZATION) -// .finished(); -// -// camera_to_tag_d += CalculateDerivative( -// camera_to_tag, normalized_camera_matrix_, image_point, corner_index); -// -// loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, -// image_point, corner_index); -// } -// camera_to_tag -= camera_to_tag_d * LR; -// } -// ASSERT_LT(loss, 0.001); -// } +TEST_F(ForwardTest, TestDerrivative) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + auto feild_to_robot = square_solver_solution.pose.ToMatrix(); + + Eigen::Matrix4d camera_to_tag = + camera_to_robot_ * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; + + // Noise + camera_to_tag(0, 0) += 0.1; + camera_to_tag(0, 3) += 0.1; + camera_to_tag(0, 2) += 0.1; + camera_to_tag(1, 3) += 0.1; + + double loss = 0; + for (int epoch = 0; epoch < EPOCHS; epoch++) { + Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); + loss = 0; + for (int corner_index = 0; corner_index < 4; corner_index++) { + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + camera_to_tag_d += CalculateDerivative( + camera_to_tag, normalized_camera_matrix_, image_point, corner_index); + + loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, + image_point, corner_index); + } + camera_to_tag -= camera_to_tag_d * LR; + } + ASSERT_LT(loss, 0.001); +} // // TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT // cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); From 7530d645092fc62f4bd566ecf5c58b66e006d77d Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 25 Apr 2026 07:43:45 +0000 Subject: [PATCH 17/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 231 +++++++++++++++-------------- 1 file changed, 116 insertions(+), 115 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index cdb6e5ef..9056d56f 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -420,118 +420,119 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT } ASSERT_LT(loss, 0.001); } -// -// TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT -// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); -// timestamped_frame_t timestamped_frame{ -// .frame = std::move(image), .timestamp = 0, .invalid = false}; -// auto detections = detector_->GetTagDetections(timestamped_frame); -// auto detection = detections[0]; -// -// auto square_solver_solution = -// square_solver_->EstimatePosition({detection})[0]; -// -// DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); -// DifferntiableTransform3d transform_from_eigen( -// square_solver_solution.pose.ToMatrix()); -// -// const double tolerance = 1e-7; -// ASSERT_NEAR(transform_from_pose.t_x.value(), transform_from_eigen.t_x.value(), -// tolerance); -// ASSERT_NEAR(transform_from_pose.t_y.value(), transform_from_eigen.t_y.value(), -// tolerance); -// ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), -// tolerance); -// -// ASSERT_NEAR(transform_from_pose.r_x.value(), transform_from_eigen.r_x.value(), -// tolerance); -// ASSERT_NEAR(transform_from_pose.r_y.value(), transform_from_eigen.r_y.value(), -// tolerance); -// ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), -// tolerance); -// } -// TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT -// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); -// timestamped_frame_t timestamped_frame{ -// .frame = std::move(image), .timestamp = 0, .invalid = false}; -// auto detections = detector_->GetTagDetections(timestamped_frame); -// auto detection = detections[0]; -// -// auto square_solver_solution = -// square_solver_->EstimatePosition({detection})[0]; -// -// DifferntiableTransform3d transform(square_solver_solution.pose); -// transform.CalculateMatrix(); -// -// EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( -// transform.ToEigen(), 1e-9)); -// -// for (int i = 0; i < 4; i++) { -// for (int j = 0; j < 4; j++) { -// EXPECT_NEAR(square_solver_solution.pose.ToMatrix()(i, j), -// transform.matrix[i][j].value(), 1e-9); -// } -// } -// } -// -// TEST_F(ForwardTest, TestBackpropagation) { // NOLINT -// cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); -// timestamped_frame_t timestamped_frame{ -// .frame = std::move(image), .timestamp = 0, .invalid = false}; -// auto detections = detector_->GetTagDetections(timestamped_frame); -// LOG(INFO) << detections.size(); -// auto detection = detections[0]; -// -// auto square_solver_solution = -// square_solver_->EstimatePosition({detection})[0]; -// -// auto feild_to_tag = -// kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); -// -// auto feild_to_camera = square_solver_solution.pose.ToMatrix(); -// -// DifferntiableTransform3d camera_to_tag(feild_to_camera.inverse() * -// feild_to_tag * rotate_yaw); -// -// // Noise -// camera_to_tag.t_x += 0.1; -// camera_to_tag.t_y += 0.1; -// camera_to_tag.t_z += 0.1; -// -// camera_to_tag.r_x += 0.1; -// camera_to_tag.r_y += 0.1; -// camera_to_tag.r_z += 0.1; -// -// tape_type tape; -// -// double loss = 0; -// utils::Timer solve_timer("solve", false); -// for (int epoch = 0; epoch < EPOCHS; epoch++) { -// loss = 0; -// transform3d_derrivative_t derrivative; -// -// for (int corner_index = 0; corner_index < 4; corner_index++) { -// camera_to_tag.CalculateMatrix(); -// camera_to_tag.RegisterInputs(tape); -// const auto camera_to_tag_eigen = camera_to_tag.ToEigen(); -// auto image_point = (Eigen::Vector3d() << 1, -// detection.corners[corner_index].x / NORMALIZATION, -// detection.corners[corner_index].y / NORMALIZATION) -// .finished(); -// -// auto camera_to_tag_d = -// CalculateDerivative(camera_to_tag_eigen, normalized_camera_matrix_, -// image_point, corner_index); -// camera_to_tag.RegisterOutputs(tape); -// derrivative = -// derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); -// -// loss += CalculateLoss(camera_to_tag_eigen, normalized_camera_matrix_, -// image_point, corner_index); -// tape.newRecording(); -// } -// camera_to_tag.Apply(derrivative); -// } -// ASSERT_LT(solve_timer.Stop(), 1.0 / 250); -// ASSERT_LT(loss, 0.01); -// } + +TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); + DifferntiableTransform3d transform_from_eigen( + square_solver_solution.pose.ToMatrix()); + + const double tolerance = 1e-7; + ASSERT_NEAR(transform_from_pose.t_x.value(), transform_from_eigen.t_x.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.t_y.value(), transform_from_eigen.t_y.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), + tolerance); + + ASSERT_NEAR(transform_from_pose.r_x.value(), transform_from_eigen.r_x.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.r_y.value(), transform_from_eigen.r_y.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), + tolerance); +} + +TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + DifferntiableTransform3d transform(square_solver_solution.pose); + transform.CalculateMatrix(); + + EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( + transform.ToEigen(), 1e-9)); + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + EXPECT_NEAR(square_solver_solution.pose.ToMatrix()(i, j), + transform.matrix[i][j].value(), 1e-9); + } + } +} + +TEST_F(ForwardTest, TestBackpropagation) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector_->GetTagDetections(timestamped_frame); + LOG(INFO) << detections.size(); + auto detection = detections[0]; + + auto square_solver_solution = + square_solver_->EstimatePosition({detection})[0]; + + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + auto feild_to_robot = square_solver_solution.pose.ToMatrix(); + + DifferntiableTransform3d camera_to_tag( + camera_to_robot_ * feild_to_robot.inverse() * feild_to_tag * rotate_yaw); + + // Noise + camera_to_tag.t_x += 0.1; + camera_to_tag.t_y += 0.1; + camera_to_tag.t_z += 0.1; + + camera_to_tag.r_x += 0.1; + camera_to_tag.r_y += 0.1; + camera_to_tag.r_z += 0.1; + + tape_type tape; + + double loss = 0; + utils::Timer solve_timer("solve", false); + for (int epoch = 0; epoch < EPOCHS; epoch++) { + loss = 0; + transform3d_derrivative_t derrivative; + + for (int corner_index = 0; corner_index < 4; corner_index++) { + camera_to_tag.CalculateMatrix(); + camera_to_tag.RegisterInputs(tape); + const auto camera_to_tag_eigen = camera_to_tag.ToEigen(); + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + auto camera_to_tag_d = + CalculateDerivative(camera_to_tag_eigen, normalized_camera_matrix_, + image_point, corner_index); + camera_to_tag.RegisterOutputs(tape); + derrivative = + derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); + + loss += CalculateLoss(camera_to_tag_eigen, normalized_camera_matrix_, + image_point, corner_index); + tape.newRecording(); + } + camera_to_tag.Apply(derrivative); + } + ASSERT_LT(solve_timer.Stop(), 1.0 / 250); + ASSERT_LT(loss, 0.01); +} From 5f793b42beccf751cc5836194be80e6925c991a9 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 26 Apr 2026 01:10:31 +0000 Subject: [PATCH 18/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 109 ++++++++++++++++------------- 1 file changed, 62 insertions(+), 47 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 9056d56f..fef7c5cc 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -14,7 +14,7 @@ #define LOG_PATH "/bos-logs/log181/right" #define LR 0.05 #define EPOCHS 1000 -#define NORMALIZATION 100 +#define NORMALIZATION 1000 namespace fs = std::filesystem; @@ -274,12 +274,16 @@ auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, return normalized_point; } -auto CalculateLoss(const Eigen::Matrix4d& camera_to_tag, +auto CalculateLoss(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, const Eigen::Matrix3d& camera_matrix, const Eigen::Vector3d& image_point, int corner_index) -> double { + Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_tag * + camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * + rotate_yaw * localization::kapriltag_corners_eigen_homogenized[corner_index]; auto normalized_point = projected_point / projected_point[0]; @@ -288,13 +292,15 @@ auto CalculateLoss(const Eigen::Matrix4d& camera_to_tag, return normalized_points_d.array().square().sum(); } -auto CalculateDerivative(const Eigen::Matrix4d& camera_to_tag, +auto CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, const Eigen::Matrix3d& camera_matrix, const Eigen::Vector3d& image_point, int corner_index) -> Eigen::Matrix4d { - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_tag * + camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * + rotate_yaw * localization::kapriltag_corners_eigen_homogenized[corner_index]; auto normalized_point = projected_point / projected_point[0]; @@ -313,10 +319,16 @@ auto CalculateDerivative(const Eigen::Matrix4d& camera_to_tag, auto PI_xd = PI.transpose() * camera_matrix_xd; - auto camera_to_tag_d = - PI_xd * localization::kapriltag_corners_eigen_homogenized[corner_index] - .transpose(); - return camera_to_tag_d; + auto camera_to_robot_xd = camera_to_robot.transpose() * PI_xd; + + auto robot_to_feild_d = + camera_to_robot_xd * + + (feild_to_tag * rotate_yaw * + localization::kapriltag_corners_eigen_homogenized[corner_index]) + .transpose(); + + return robot_to_feild_d; } // TODO: Tolerance is quite high. Find out what causes the loss in precision @@ -389,20 +401,18 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT auto feild_to_tag = kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); - auto feild_to_robot = square_solver_solution.pose.ToMatrix(); - - Eigen::Matrix4d camera_to_tag = - camera_to_robot_ * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; + Eigen::Matrix4d robot_to_feild = + square_solver_solution.pose.ToMatrix().inverse(); // Noise - camera_to_tag(0, 0) += 0.1; - camera_to_tag(0, 3) += 0.1; - camera_to_tag(0, 2) += 0.1; - camera_to_tag(1, 3) += 0.1; + robot_to_feild(0, 0) += 0.1; + robot_to_feild(0, 3) += 0.1; + robot_to_feild(0, 2) += 0.1; + robot_to_feild(1, 3) += 0.1; double loss = 0; for (int epoch = 0; epoch < EPOCHS; epoch++) { - Eigen::Matrix4d camera_to_tag_d = Eigen::Matrix4d::Zero(); + Eigen::Matrix4d robot_to_feild_d = Eigen::Matrix4d::Zero(); loss = 0; for (int corner_index = 0; corner_index < 4; corner_index++) { auto image_point = (Eigen::Vector3d() << 1, @@ -410,13 +420,16 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT detection.corners[corner_index].y / NORMALIZATION) .finished(); - camera_to_tag_d += CalculateDerivative( - camera_to_tag, normalized_camera_matrix_, image_point, corner_index); + robot_to_feild_d += CalculateDerivative( + robot_to_feild, feild_to_tag, camera_to_robot_, + normalized_camera_matrix_, image_point, corner_index); - loss += CalculateLoss(camera_to_tag, normalized_camera_matrix_, - image_point, corner_index); + loss += + CalculateLoss(robot_to_feild, feild_to_tag, camera_to_robot_, + normalized_camera_matrix_, image_point, corner_index); } - camera_to_tag -= camera_to_tag_d * LR; + robot_to_feild -= robot_to_feild_d * LR; + LOG(INFO) << loss; } ASSERT_LT(loss, 0.001); } @@ -477,31 +490,27 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT TEST_F(ForwardTest, TestBackpropagation) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); + // LOG(INFO) << detections.size(); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; auto detections = detector_->GetTagDetections(timestamped_frame); - LOG(INFO) << detections.size(); auto detection = detections[0]; auto square_solver_solution = square_solver_->EstimatePosition({detection})[0]; - auto feild_to_tag = - kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); - auto feild_to_robot = square_solver_solution.pose.ToMatrix(); - DifferntiableTransform3d camera_to_tag( - camera_to_robot_ * feild_to_robot.inverse() * feild_to_tag * rotate_yaw); + DifferntiableTransform3d robot_to_feild(feild_to_robot.inverse()); // Noise - camera_to_tag.t_x += 0.1; - camera_to_tag.t_y += 0.1; - camera_to_tag.t_z += 0.1; + robot_to_feild.t_x += 0.1; + robot_to_feild.t_y += 0.1; + robot_to_feild.t_z += 0.1; - camera_to_tag.r_x += 0.1; - camera_to_tag.r_y += 0.1; - camera_to_tag.r_z += 0.1; + robot_to_feild.r_x += 0.1; + robot_to_feild.r_y += 0.1; + robot_to_feild.r_z += 0.1; tape_type tape; @@ -511,27 +520,33 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT loss = 0; transform3d_derrivative_t derrivative; + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + for (int corner_index = 0; corner_index < 4; corner_index++) { - camera_to_tag.CalculateMatrix(); - camera_to_tag.RegisterInputs(tape); - const auto camera_to_tag_eigen = camera_to_tag.ToEigen(); + robot_to_feild.CalculateMatrix(); + robot_to_feild.RegisterInputs(tape); + const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); auto image_point = (Eigen::Vector3d() << 1, detection.corners[corner_index].x / NORMALIZATION, detection.corners[corner_index].y / NORMALIZATION) .finished(); - auto camera_to_tag_d = - CalculateDerivative(camera_to_tag_eigen, normalized_camera_matrix_, - image_point, corner_index); - camera_to_tag.RegisterOutputs(tape); + auto robot_to_feild_d = CalculateDerivative( + robot_to_feild_eigen, feild_to_tag, camera_to_robot_, + normalized_camera_matrix_, image_point, corner_index); + + robot_to_feild.RegisterOutputs(tape); derrivative = - derrivative + camera_to_tag.BackPropagate(camera_to_tag_d, tape); + derrivative + robot_to_feild.BackPropagate(robot_to_feild_d, tape); + + loss += + CalculateLoss(robot_to_feild_eigen, feild_to_tag, camera_to_robot_, + normalized_camera_matrix_, image_point, corner_index); - loss += CalculateLoss(camera_to_tag_eigen, normalized_camera_matrix_, - image_point, corner_index); tape.newRecording(); } - camera_to_tag.Apply(derrivative); + robot_to_feild.Apply(derrivative); } ASSERT_LT(solve_timer.Stop(), 1.0 / 250); ASSERT_LT(loss, 0.01); From a162e069d831ab492256d6abd9367abb90365709 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 26 Apr 2026 02:26:45 +0000 Subject: [PATCH 19/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 250 +++++++++++++++++++++-------- 1 file changed, 183 insertions(+), 67 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index fef7c5cc..a49697e9 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -230,32 +230,55 @@ const Eigen::Matrix rotate_yaw = class ForwardTest : public ::testing::Test { protected: void SetUp() override { - camera_constant_ = GetCameraConstants().at("second_bot_right"); - - square_solver_ = std::make_unique(camera_constant_); - detector_ = std::make_unique( - camera_constant_.frame_height.value(), - camera_constant_.frame_height.value(), - ReadIntrinsics(camera_constant_.intrinsics_path.value())); - camera_matrix_ = CameraMatrixFromJson( - ReadIntrinsics(camera_constant_.intrinsics_path.value())); - normalized_camera_matrix_ = camera_matrix_ / NORMALIZATION; - normalized_camera_matrix_(0, 0) = 1; - - camera_to_robot_ = + camera_constant_right_ = GetCameraConstants().at("second_bot_right"); + square_solver_right_ = + std::make_unique(camera_constant_right_); + detector_right_ = std::make_unique( + camera_constant_right_.frame_width.value(), + camera_constant_right_.frame_height.value(), + ReadIntrinsics(camera_constant_right_.intrinsics_path.value())); + camera_matrix_right_ = CameraMatrixFromJson( + ReadIntrinsics(camera_constant_right_.intrinsics_path.value())); + normalized_camera_matrix_right_ = camera_matrix_right_ / NORMALIZATION; + normalized_camera_matrix_right_(0, 0) = 1; + camera_to_robot_right_ = utils::ExtrinsicsJsonToCameraToRobot( - utils::ReadExtrinsics(camera_constant_.extrinsics_path.value())) + utils::ReadExtrinsics( + camera_constant_right_.extrinsics_path.value())) + .ToMatrix(); + + camera_constant_left_ = GetCameraConstants().at("second_bot_left"); + square_solver_left_ = std::make_unique(camera_constant_left_); + detector_left_ = std::make_unique( + camera_constant_left_.frame_width.value(), + camera_constant_left_.frame_height.value(), + ReadIntrinsics(camera_constant_left_.intrinsics_path.value())); + camera_matrix_left_ = CameraMatrixFromJson( + ReadIntrinsics(camera_constant_left_.intrinsics_path.value())); + normalized_camera_matrix_left_ = camera_matrix_left_ / NORMALIZATION; + normalized_camera_matrix_left_(0, 0) = 1; + camera_to_robot_left_ = + utils::ExtrinsicsJsonToCameraToRobot( + utils::ReadExtrinsics( + camera_constant_left_.extrinsics_path.value())) .ToMatrix(); } void TearDown() override {} - camera_constant_t camera_constant_; - std::unique_ptr square_solver_; - std::unique_ptr detector_; - Eigen::Matrix3d camera_matrix_; - Eigen::Matrix3d normalized_camera_matrix_; - Eigen::Matrix4d camera_to_robot_; + std::unique_ptr detector_right_; + camera_constant_t camera_constant_right_; + std::unique_ptr square_solver_right_; + Eigen::Matrix3d camera_matrix_right_; + Eigen::Matrix3d normalized_camera_matrix_right_; + Eigen::Matrix4d camera_to_robot_right_; + + std::unique_ptr detector_left_; + camera_constant_t camera_constant_left_; + std::unique_ptr square_solver_left_; + Eigen::Matrix3d camera_matrix_left_; + Eigen::Matrix3d normalized_camera_matrix_left_; + Eigen::Matrix4d camera_to_robot_left_; }; auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, @@ -338,7 +361,7 @@ void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, EXPECT_NEAR(image_point.y, projected_points[2], tolerance); } -TEST_F(ForwardTest, ProjectTest) { // NOLINT +TEST_F(ForwardTest, TestForward) { // NOLINT // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners // // feild_to_camera = feild_to_tag * 180_yaw * camera_to_tag.inv @@ -361,25 +384,25 @@ TEST_F(ForwardTest, ProjectTest) { // NOLINT cv::Mat image = cv::imread(image_paths[i]); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); + auto detections = detector_right_->GetTagDetections(timestamped_frame); if (detections.empty()) { continue; } auto detection = detections[0]; auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; + square_solver_right_->EstimatePosition({detection})[0]; for (int j = 0; j < 4; j++) { auto projected_points = ProjectPoints(square_solver_solution.pose, kapriltag_layout.GetTagPose(detection.tag_id).value(), - camera_matrix_, camera_to_robot_, j); + camera_matrix_right_, camera_to_robot_right_, j); - auto projected_points_normalized = - ProjectPoints(square_solver_solution.pose, - kapriltag_layout.GetTagPose(detection.tag_id).value(), - normalized_camera_matrix_, camera_to_robot_, j); + auto projected_points_normalized = ProjectPoints( + square_solver_solution.pose, + kapriltag_layout.GetTagPose(detection.tag_id).value(), + normalized_camera_matrix_right_, camera_to_robot_right_, j); CheckIsEqual(detection.corners[j], projected_points, 1); CheckIsEqual(detection.corners[j] / NORMALIZATION, @@ -392,11 +415,11 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); + auto detections = detector_right_->GetTagDetections(timestamped_frame); auto detection = detections[0]; auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; + square_solver_right_->EstimatePosition({detection})[0]; auto feild_to_tag = kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); @@ -421,15 +444,14 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT .finished(); robot_to_feild_d += CalculateDerivative( - robot_to_feild, feild_to_tag, camera_to_robot_, - normalized_camera_matrix_, image_point, corner_index); + robot_to_feild, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); - loss += - CalculateLoss(robot_to_feild, feild_to_tag, camera_to_robot_, - normalized_camera_matrix_, image_point, corner_index); + loss += CalculateLoss( + robot_to_feild, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); } robot_to_feild -= robot_to_feild_d * LR; - LOG(INFO) << loss; } ASSERT_LT(loss, 0.001); } @@ -438,11 +460,11 @@ TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); + auto detections = detector_right_->GetTagDetections(timestamped_frame); auto detection = detections[0]; auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; + square_solver_right_->EstimatePosition({detection})[0]; DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); DifferntiableTransform3d transform_from_eigen( @@ -468,11 +490,11 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); + auto detections = detector_right_->GetTagDetections(timestamped_frame); auto detection = detections[0]; auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; + square_solver_right_->EstimatePosition({detection})[0]; DifferntiableTransform3d transform(square_solver_solution.pose); transform.CalculateMatrix(); @@ -488,16 +510,18 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT } } -TEST_F(ForwardTest, TestBackpropagation) { // NOLINT +TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); // LOG(INFO) << detections.size(); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector_->GetTagDetections(timestamped_frame); - auto detection = detections[0]; + auto detections = detector_right_->GetTagDetections(timestamped_frame); + + // PCHECK(detections.size() > 1) + // << "Need at least two detections to test differnt tag positions"; auto square_solver_solution = - square_solver_->EstimatePosition({detection})[0]; + square_solver_right_->EstimatePosition(detections)[0]; auto feild_to_robot = square_solver_solution.pose.ToMatrix(); @@ -519,35 +543,127 @@ TEST_F(ForwardTest, TestBackpropagation) { // NOLINT for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; transform3d_derrivative_t derrivative; + for (const auto& detection : detections) { + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + for (int corner_index = 0; corner_index < 4; corner_index++) { + robot_to_feild.CalculateMatrix(); + robot_to_feild.RegisterInputs(tape); + const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + auto robot_to_feild_d = CalculateDerivative( + robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); + + robot_to_feild.RegisterOutputs(tape); + derrivative = + derrivative + robot_to_feild.BackPropagate(robot_to_feild_d, tape); + + loss += CalculateLoss( + robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, + normalized_camera_matrix_right_, image_point, corner_index); + + tape.newRecording(); + } + } + robot_to_feild.Apply(derrivative); + } + ASSERT_LT(solve_timer.Stop(), 0.1); + ASSERT_LT(loss, 0.01); +} +TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT + cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.411762.jpg"); - auto feild_to_tag = - kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); + std::map> detections; + { + timestamped_frame_t timestamped_frame{ + .frame = std::move(image_right), .timestamp = 0, .invalid = false}; + detections[camera_constant_right_.name] = + detector_right_->GetTagDetections(timestamped_frame); + CHECK(!detections[camera_constant_right_.name].empty()); + } + { + timestamped_frame_t timestamped_frame{ + .frame = std::move(image_left), .timestamp = 0, .invalid = false}; + detections[camera_constant_left_.name] = + detector_left_->GetTagDetections(timestamped_frame); + CHECK(!detections[camera_constant_left_.name].empty()); + } - for (int corner_index = 0; corner_index < 4; corner_index++) { - robot_to_feild.CalculateMatrix(); - robot_to_feild.RegisterInputs(tape); - const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) - .finished(); + auto square_solver_solution = square_solver_right_->EstimatePosition( + detections[camera_constant_right_.name])[0]; - auto robot_to_feild_d = CalculateDerivative( - robot_to_feild_eigen, feild_to_tag, camera_to_robot_, - normalized_camera_matrix_, image_point, corner_index); + auto feild_to_robot = square_solver_solution.pose.ToMatrix(); + DifferntiableTransform3d robot_to_feild(feild_to_robot.inverse()); + + // Noise + robot_to_feild.t_x += 0.1; + robot_to_feild.t_y += 0.1; + robot_to_feild.t_z += 0.1; - robot_to_feild.RegisterOutputs(tape); - derrivative = - derrivative + robot_to_feild.BackPropagate(robot_to_feild_d, tape); + robot_to_feild.r_x += 0.1; + robot_to_feild.r_y += 0.1; + robot_to_feild.r_z += 0.1; - loss += - CalculateLoss(robot_to_feild_eigen, feild_to_tag, camera_to_robot_, - normalized_camera_matrix_, image_point, corner_index); + tape_type tape; - tape.newRecording(); + double loss = 0; + std::vector camera_constants{camera_constant_left_, + camera_constant_right_}; + utils::Timer solve_timer("solve", false); + for (int epoch = 0; epoch < EPOCHS; epoch++) { + loss = 0; + transform3d_derrivative_t derrivative; + for (const auto& camera_constant : camera_constants) { + + auto camera_to_robot = + utils::ExtrinsicsJsonToCameraToRobot( + utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) + .ToMatrix(); + + auto camera_matrix = CameraMatrixFromJson( + ReadIntrinsics(camera_constant.intrinsics_path.value())); + Eigen::Matrix3d normalized_camera_matrix = camera_matrix / NORMALIZATION; + normalized_camera_matrix(0, 0) = 1; + + for (const auto& detection : detections[camera_constant.name]) { + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + for (int corner_index = 0; corner_index < 4; corner_index++) { + robot_to_feild.CalculateMatrix(); + robot_to_feild.RegisterInputs(tape); + const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + auto robot_to_feild_d = CalculateDerivative( + robot_to_feild_eigen, feild_to_tag, camera_to_robot, + normalized_camera_matrix, image_point, corner_index); + + robot_to_feild.RegisterOutputs(tape); + derrivative = derrivative + + robot_to_feild.BackPropagate(robot_to_feild_d, tape); + + loss += CalculateLoss(robot_to_feild_eigen, feild_to_tag, + camera_to_robot, normalized_camera_matrix, + image_point, corner_index); + + tape.newRecording(); + } + } } robot_to_feild.Apply(derrivative); - } - ASSERT_LT(solve_timer.Stop(), 1.0 / 250); - ASSERT_LT(loss, 0.01); + // LOG(INFO) << loss; + } // The test is very unoptimized + ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized + ASSERT_LT(loss, 50); } From 48732792f5df2f6716f0e9430b3f7956f062e57b Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 26 Apr 2026 02:46:15 +0000 Subject: [PATCH 20/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/forward_test.cc | 33 ++++++++++++++++++++++++------ 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index a49697e9..acce56f7 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -13,8 +13,9 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" #define LR 0.05 -#define EPOCHS 1000 +#define EPOCHS 100 #define NORMALIZATION 1000 +#define BETA 0.3 namespace fs = std::filesystem; @@ -49,6 +50,24 @@ using transform3d_derrivative_t = struct Transfrom3dDerrivative { .r_y = r_y + other.r_y, .r_z = r_z + other.r_z}; } + + auto operator-(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x - other.t_x, + .t_y = t_y - other.t_y, + .t_z = t_z - other.t_z, + .r_x = r_x - other.r_x, + .r_y = r_y - other.r_y, + .r_z = r_z - other.r_z}; + } + + auto operator*(const double other) -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x * other, + .t_y = t_y * other, + .t_z = t_z * other, + .r_x = r_x * other, + .r_y = r_y * other, + .r_z = r_z * other}; + } }; auto operator<<(std::ostream& os, const Transfrom3dDerrivative& v) @@ -616,7 +635,8 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT double loss = 0; std::vector camera_constants{camera_constant_left_, camera_constant_right_}; - utils::Timer solve_timer("solve", false); + utils::Timer solve_timer("solve", true); + transform3d_derrivative_t velocity; for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; transform3d_derrivative_t derrivative; @@ -661,9 +681,10 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT } } } - robot_to_feild.Apply(derrivative); - // LOG(INFO) << loss; - } // The test is very unoptimized + velocity = (velocity * BETA) + derrivative; + robot_to_feild.Apply(velocity); + LOG(INFO) << loss; + } ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized - ASSERT_LT(loss, 50); + ASSERT_LT(loss, 0.01); } From 7d44f8cc95745eea649aaabf5b3b87021d1376a6 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 26 Apr 2026 02:54:14 +0000 Subject: [PATCH 21/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 191 +---------------------------- src/test/unit_test/forward_test.cc | 3 +- 2 files changed, 2 insertions(+), 192 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index af746478..ef15b32a 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -7,16 +7,6 @@ namespace localization { -using data_point_t = struct DataPoint { - Eigen::Vector2d undistorted_point; - std::string name; - Eigen::Vector4d field_to_tag_corner_homogenous; -}; - -constexpr auto sq(double num) -> double { - return num * num; -} - using frc::AprilTagFieldLayout; JointSolver::JointSolver( @@ -24,192 +14,13 @@ JointSolver::JointSolver( const AprilTagFieldLayout& layout) : robot_to_field_(Eigen::Matrix4d::Identity()) { // clang-format off - const Eigen::Matrix4d rotate_yaw_cv = (Eigen::Matrix4d() << - -1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, -1, 0, - 0, 0, 0, 1).finished(); - // clang-format on - for (const frc::AprilTag& tag : layout.GetTags()) { - Eigen::Matrix4d field_to_tag = tag.pose.ToMatrix(); - utils::ChangeBasis(field_to_tag, utils::WPI_TO_CV); - std::array field_relative_corners; - for (size_t i = 0; i < kapriltag_corners_eigen.size(); i++) { - field_relative_corners[i] = field_to_tag * rotate_yaw_cv * - utils::Homogenize(kapriltag_corners_eigen[i]); - } - tag_corners_[tag.ID] = field_relative_corners; - } - Eigen::Matrix pi = Eigen::Matrix::Zero(); - pi.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - for (const camera::camera_constant_t& camera_constant : camera_constants_) { - const nlohmann::json intrinsics_json = - utils::ReadIntrinsics(camera_constant.intrinsics_path.value()); - const auto camera_matrix = utils::CameraMatrixFromJson( - intrinsics_json); // TOOD CameraMatrixFromJson now returns inwpilib coords. Joint solver is broken anyway so this is fine for now. - const Eigen::Matrix image_to_camera = camera_matrix * pi; - const Eigen::Matrix4d camera_to_robot = - utils::ExtrinsicsJsonToCameraToRobot( - utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) - .ToMatrix(); - const Eigen::Matrix image_to_robot = - image_to_camera * camera_to_robot; - camera_matrices_.insert( - {camera_constant.name, - {.image_to_robot = image_to_robot, - .distortion_coefficients = - utils::DistortionCoefficientsFromJson(intrinsics_json), - .camera_matrix = utils::EigenToCvMat(camera_matrix)}}); - } } auto JointSolver::EstimatePosition( const std::map>& all_cam_detections, const frc::Pose3d& starting_pose) -> position_estimate_t { - if (all_cam_detections.empty()) { - return {}; - } - robot_to_field_ = starting_pose.ToMatrix().inverse(); - utils::ChangeBasis(robot_to_field_, utils::WPI_TO_CV); - std::vector data_points; - for (const auto& pair : all_cam_detections) { - const CameraMatrices& camera_mats = camera_matrices_.at(pair.first); - for (const tag_detection_t& detection : pair.second) { - std::vector undistorted_corners; - cv::undistortImagePoints(detection.corners, undistorted_corners, - camera_mats.camera_matrix, - camera_mats.distortion_coefficients); - for (size_t i = 0; i < undistorted_corners.size(); i++) { - Eigen::Vector2d undistorted_image_point; - undistorted_image_point << undistorted_corners[i].x, - undistorted_corners[i].y; - const data_point_t datapoint = { - .undistorted_point = undistorted_image_point, - .name = pair.first, - .field_to_tag_corner_homogenous = - tag_corners_[detection.tag_id].value()[i]}; - data_points.push_back(datapoint); - } - } - } - - double loss = std::numeric_limits::infinity(); - int counter = 0; - - const double step = 1e-7; - - utils::TransformValues translation_and_rotation = - utils::ExtractTranslationAndRotation(robot_to_field_); - utils::TransformDecomposition decomposed_robot_to_field; - - while (loss > kacceptable_reprojection_error && counter < 1000000) { - bool printed = false; - for (const data_point_t& data_point : data_points) { - decomposed_robot_to_field = utils::SeparateTranslationAndRotationMatrices( - translation_and_rotation); - const Eigen::Matrix& image_to_robot = - camera_matrices_.at(data_point.name).image_to_robot; - - const Eigen::Vector4d Rx_activation = - decomposed_robot_to_field.Rx * - data_point.field_to_tag_corner_homogenous; - const Eigen::Vector4d Ry_activation = - decomposed_robot_to_field.Ry * Rx_activation; - const Eigen::Vector4d Rz_activation = - decomposed_robot_to_field.Rz * Ry_activation; - - Eigen::Vector3d projection = image_to_robot * - decomposed_robot_to_field.translation * - Rz_activation; - const double lambda = projection(2); - projection /= lambda; - - const Eigen::Vector2d projection_error( - projection.x() - data_point.undistorted_point.x(), - projection.y() - data_point.undistorted_point.y()); - loss = 0.5 * projection_error.squaredNorm(); - - if (counter % 10000 == 0 && !printed) { - printed = true; - // std::cout << "loss: " << loss << std::endl; - } - - const Eigen::Vector3d d_projection( - projection_error.x() / lambda, projection_error.y() / lambda, - -(projection_error.x() * projection.x() + - projection_error.y() * projection.y()) / - lambda); - - Eigen::Vector4d accumulated_gradient = - image_to_robot.transpose() * d_projection; - - const Eigen::Matrix4d d_translation = - accumulated_gradient * Rz_activation.transpose(); - - accumulated_gradient = decomposed_robot_to_field.translation.transpose() * - accumulated_gradient; - const Eigen::Matrix4d d_Rz = - accumulated_gradient * Ry_activation.transpose(); - - accumulated_gradient = - decomposed_robot_to_field.Rz.transpose() * accumulated_gradient; - const Eigen::Matrix4d d_Ry = - accumulated_gradient * Rx_activation.transpose(); - - accumulated_gradient = - decomposed_robot_to_field.Ry.transpose() * accumulated_gradient; - const Eigen::Matrix4d d_Rx = - accumulated_gradient * - data_point.field_to_tag_corner_homogenous.transpose(); - - // clang-format off - const Eigen::Matrix4d d_Rz_d_rz = - (Eigen::Matrix4d() << - -sin(translation_and_rotation.rz), -cos(translation_and_rotation.rz), 0, 0, - cos(translation_and_rotation.rz), -sin(translation_and_rotation.rz), 0, 0, - 0, 0, 0, 0, - 0, 0, 0, 0).finished(); - const Eigen::Matrix4d d_Ry_d_ry = - (Eigen::Matrix4d() << - -sin(translation_and_rotation.ry), 0, cos(translation_and_rotation.ry), 0, - 0, 0, 0, 0, - -cos(translation_and_rotation.ry), 0, -sin(translation_and_rotation.ry), 0, - 0, 0, 0, 0).finished(); - const Eigen::Matrix4d d_Rx_d_rx = - (Eigen::Matrix4d() << - 0, 0, 0, 0, - 0, -sin(translation_and_rotation.rx), -cos(translation_and_rotation.rx), 0, - 0, cos(translation_and_rotation.rx), -sin(translation_and_rotation.rx), 0, - 0, 0, 0, 0).finished(); - // clang-format on - - translation_and_rotation.rx -= - step * (d_Rx_d_rx.transpose() * d_Rx).trace(); - translation_and_rotation.ry -= - step * (d_Ry_d_ry.transpose() * d_Ry).trace(); - translation_and_rotation.rz -= - step * (d_Rz_d_rz.transpose() * d_Rz).trace(); - - translation_and_rotation.x -= step * d_translation(0, 3); - translation_and_rotation.y -= step * d_translation(1, 3); - translation_and_rotation.z -= step * d_translation(2, 3); - } - counter++; - } - - Eigen::Matrix4d field_to_robot = - (decomposed_robot_to_field.translation * decomposed_robot_to_field.Rz * - decomposed_robot_to_field.Ry * decomposed_robot_to_field.Rx) - .inverse(); - - utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - "Field to robot cv"); - utils::ChangeBasis(field_to_robot, utils::CV_TO_WPI); - utils::PrintTransformationMatrix(utils::EigenToCvMat(field_to_robot), - "Field to robot wpi"); - - return {.pose = frc::Pose3d(field_to_robot), .variance = 0, .timestamp = 0}; + return {}; } } // namespace localization diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index acce56f7..36325352 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -635,7 +635,7 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT double loss = 0; std::vector camera_constants{camera_constant_left_, camera_constant_right_}; - utils::Timer solve_timer("solve", true); + utils::Timer solve_timer("solve", false); transform3d_derrivative_t velocity; for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; @@ -683,7 +683,6 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT } velocity = (velocity * BETA) + derrivative; robot_to_feild.Apply(velocity); - LOG(INFO) << loss; } ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized ASSERT_LT(loss, 0.01); From ead925cf59a37f4db17fa9d6e8b467859c0ba0e4 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 26 Apr 2026 04:21:33 +0000 Subject: [PATCH 22/47] wip Signed-off-by: Charlie Huang --- src/localization/CMakeLists.txt | 2 +- src/localization/joint_solver.cc | 45 +++++- src/localization/joint_solver.h | 201 ++++++++++++++++++++++++-- src/test/unit_test/forward_test.cc | 222 ++--------------------------- 4 files changed, 244 insertions(+), 226 deletions(-) diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index 5a3ae30b..cba41a32 100644 --- a/src/localization/CMakeLists.txt +++ b/src/localization/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.10) add_library(localization 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 unambiguous_estimator.cc simulation_sender.cc networktable_sender.cc) -target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi absl::status) +target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi absl::status XAD::xad) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index ef15b32a..fda09a0a 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -8,17 +8,52 @@ namespace localization { using frc::AprilTagFieldLayout; +using utils::CameraMatrixFromJson; +using utils::ExtrinsicsJsonToCameraToRobot; +using utils::ReadExtrinsics; +using utils::ReadIntrinsics; + +// clang-format off +const Eigen::Matrix PI = + (Eigen::Matrix() << + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0).finished(); + +const Eigen::Matrix rotate_yaw = + (Eigen::Matrix() << + -1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1).finished(); +// clang-format on + +void NormalizeCameraMatrix(Eigen::Matrix3d& camera_matrix) { + camera_matrix = camera_matrix / 1000; + camera_matrix(0, 0) = 1; +} JointSolver::JointSolver( - const std::vector& camera_constants_, - const AprilTagFieldLayout& layout) - : robot_to_field_(Eigen::Matrix4d::Identity()) { - // clang-format off + const std::vector& camera_constants, + const AprilTagFieldLayout& layout) { + for (int i = 0; i < camera_constants.size(); i++) { + camera_name_to_index[camera_constants[i].name] = i; + + normalized_camera_matrix_.emplace_back( + CameraMatrixFromJson( + ReadIntrinsics(camera_constants[i].intrinsics_path.value()))); + NormalizeCameraMatrix(normalized_camera_matrix_[i]); + + camera_to_robot_.emplace_back( + ExtrinsicsJsonToCameraToRobot( + ReadExtrinsics(camera_constants[i].extrinsics_path.value())) + .ToMatrix()); + } } auto JointSolver::EstimatePosition( const std::map>& - all_cam_detections, + camera_detections, const frc::Pose3d& starting_pose) -> position_estimate_t { return {}; } diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 7dfb91f1..a885fb60 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -1,5 +1,7 @@ #pragma once #include +#include +#include #include "nlohmann/json.hpp" #include "src/camera/camera_constants.h" #include "src/localization/position_solver.h" @@ -8,26 +10,199 @@ using json = nlohmann::json; namespace localization { -struct CameraMatrices { - Eigen::Matrix image_to_robot; - cv::Mat distortion_coefficients; - cv::Mat camera_matrix; -}; -static constexpr int kmax_tags = 50; + class JointSolver { public: - JointSolver(const std::vector& camera_constants_, + using mode = xad::adj; + using tape_type = mode::tape_type; + using AD = mode::active_type; + + using transform3d_derrivative_t = struct Transfrom3dDerrivative { + double t_x = 0; + double t_y = 0; + double t_z = 0; + double r_x = 0; + double r_y = 0; + double r_z = 0; + + auto operator+(const Transfrom3dDerrivative other) + -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x + other.t_x, + .t_y = t_y + other.t_y, + .t_z = t_z + other.t_z, + .r_x = r_x + other.r_x, + .r_y = r_y + other.r_y, + .r_z = r_z + other.r_z}; + } + + auto operator-(const Transfrom3dDerrivative other) + -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x - other.t_x, + .t_y = t_y - other.t_y, + .t_z = t_z - other.t_z, + .r_x = r_x - other.r_x, + .r_y = r_y - other.r_y, + .r_z = r_z - other.r_z}; + } + + auto operator*(const double other) -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x * other, + .t_y = t_y * other, + .t_z = t_z * other, + .r_x = r_x * other, + .r_y = r_y * other, + .r_z = r_z * other}; + } + }; + + struct DifferntiableTransform3d { + + // Translation in meters, rotation in radians + AD t_x; + AD t_y; + AD t_z; + AD r_x; + AD r_y; + AD r_z; + std::array, 4> matrix; + + DifferntiableTransform3d(frc::Pose3d pose) + : t_x(pose.Translation().X().value()), + t_y(pose.Translation().Y().value()), + t_z(pose.Translation().Z().value()), + r_x(pose.Rotation().X().value()), + r_y(pose.Rotation().Y().value()), + r_z(pose.Rotation().Z().value()) {} + + DifferntiableTransform3d(frc::Transform3d pose) + : t_x(pose.Translation().X().value()), + t_y(pose.Translation().Y().value()), + t_z(pose.Translation().Z().value()), + r_x(pose.Rotation().X().value()), + r_y(pose.Rotation().Y().value()), + r_z(pose.Rotation().Z().value()) {} + + DifferntiableTransform3d(Eigen::Matrix4d matrix) + : t_x(matrix(0, 3)), t_y(matrix(1, 3)), t_z(matrix(2, 3)) { + Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); + Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); + r_x = euler(2); + r_y = euler(1); + r_z = euler(0); + } + + void Update(transform3d_derrivative_t derrivative, double lr) { + t_x -= derrivative.t_x * lr; + t_y -= derrivative.t_y * lr; + t_z -= derrivative.t_z * lr; + + r_x -= derrivative.r_x * lr; + r_y -= derrivative.r_y * lr; + r_z -= derrivative.r_z * lr; + } + + void RegisterInputs(tape_type& tape) { + tape.registerInput(r_x); + tape.registerInput(r_y); + tape.registerInput(r_z); + + tape.registerInput(t_x); + tape.registerInput(t_y); + tape.registerInput(t_z); + } + + void RegisterOutputs(tape_type& tape) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + tape.registerOutput(matrix[i][j]); + } + } + } + + auto ToEigen() -> Eigen::Matrix4d const { + // clang-format off + return (Eigen::Matrix() << + matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), + matrix[1][0].value(), matrix[1][1].value(), matrix[1][2].value(), matrix[1][3].value(), + matrix[2][0].value(), matrix[2][1].value(), matrix[2][2].value(), matrix[2][3].value(), + matrix[3][0].value(), matrix[3][1].value(), matrix[3][2].value(), matrix[3][3].value()) + .finished(); + // clang-format on + } + void CalculateMatrix() { + AD cos_x = xad::cos(r_x); + AD cos_y = xad::cos(r_y); + AD cos_z = xad::cos(r_z); + + AD sin_x = xad::sin(r_x); + AD sin_y = xad::sin(r_y); + AD sin_z = xad::sin(r_z); + + // Rotation + matrix[0][0] = cos_z * cos_y; + matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; + matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; + + matrix[1][0] = sin_z * cos_y; + matrix[1][1] = sin_z * sin_y * sin_x + cos_z * cos_x; + matrix[1][2] = sin_z * sin_y * cos_x - cos_z * sin_x; + + matrix[2][0] = -sin_y; + matrix[2][1] = cos_y * sin_x; + matrix[2][2] = cos_y * cos_x; + + // Translation + matrix[0][3] = t_x; + matrix[1][3] = t_y; + matrix[2][3] = t_z; + + matrix[3][0] = 0; + matrix[3][1] = 0; + matrix[3][2] = 0; + matrix[3][3] = 1; + } + + auto BackPropagate(const Eigen::Matrix4d& next_derrivative, tape_type& tape) + -> transform3d_derrivative_t { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + tape.registerOutput(matrix[i][j]); + derivative(matrix[i][j]) = next_derrivative(i, j); + } + } + tape.computeAdjoints(); + transform3d_derrivative_t derrivative{ + .t_x = xad::derivative(t_x), + .t_y = xad::derivative(t_y), + .t_z = xad::derivative(t_z), + .r_x = xad::derivative(r_x), + .r_y = xad::derivative(r_y), + .r_z = xad::derivative(r_z), + }; + return derrivative; + } + void Apply(const transform3d_derrivative_t& derrivative, double lr) { + t_x -= derrivative.t_x * lr; + t_y -= derrivative.t_y * lr; + t_z -= derrivative.t_z * lr; + + r_x -= derrivative.r_x * lr; + r_y -= derrivative.r_y * lr; + r_z -= derrivative.r_z * lr; + } + }; + + public: + JointSolver(const std::vector& camera_constants, const frc::AprilTagFieldLayout& layout = kapriltag_layout); auto EstimatePosition( const std::map>& - all_cam_detections, + camera_detections, const frc::Pose3d& starting_pose) -> position_estimate_t; - Eigen::Matrix4d robot_to_field_; private: - static constexpr double kacceptable_reprojection_error = 0.005; - std::map camera_matrices_; - std::array>, kmax_tags> - tag_corners_; + std::unordered_map camera_name_to_index; + std::vector normalized_camera_matrix_; + std::vector camera_to_robot_; }; } // namespace localization diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 36325352..1f887f12 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -3,6 +3,7 @@ #include #include #include "src/localization/joint_solver.h" +#include "src/localization/joint_solver.h" // NO LINT #include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" #include "src/utils/camera_utils.h" @@ -22,6 +23,7 @@ namespace fs = std::filesystem; using camera::camera_constant_t; using camera::GetCameraConstants; using camera::timestamped_frame_t; +using localization::JointSolver; using localization::kapriltag_layout; using localization::OpenCVAprilTagDetector; using localization::position_estimate_t; @@ -34,203 +36,6 @@ using mode = xad::adj; using tape_type = mode::tape_type; using AD = mode::active_type; -using transform3d_derrivative_t = struct Transfrom3dDerrivative { - double t_x = 0; - double t_y = 0; - double t_z = 0; - double r_x = 0; - double r_y = 0; - double r_z = 0; - - auto operator+(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x + other.t_x, - .t_y = t_y + other.t_y, - .t_z = t_z + other.t_z, - .r_x = r_x + other.r_x, - .r_y = r_y + other.r_y, - .r_z = r_z + other.r_z}; - } - - auto operator-(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x - other.t_x, - .t_y = t_y - other.t_y, - .t_z = t_z - other.t_z, - .r_x = r_x - other.r_x, - .r_y = r_y - other.r_y, - .r_z = r_z - other.r_z}; - } - - auto operator*(const double other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x * other, - .t_y = t_y * other, - .t_z = t_z * other, - .r_x = r_x * other, - .r_y = r_y * other, - .r_z = r_z * other}; - } -}; - -auto operator<<(std::ostream& os, const Transfrom3dDerrivative& v) - -> std::ostream& { - os << "tx: " << v.t_x << "\n"; - os << "ty: " << v.t_y << "\n"; - os << "tz: " << v.t_z << "\n"; - - os << "rx: " << v.r_x << "\n"; - os << "ry: " << v.r_y << "\n"; - os << "rz: " << v.r_z << "\n"; - return os; -} - -struct DifferntiableTransform3d { - - // Translation in meters, rotation in radians - AD t_x; - AD t_y; - AD t_z; - AD r_x; - AD r_y; - AD r_z; - std::array, 4> matrix; - - DifferntiableTransform3d(frc::Pose3d pose) - : t_x(pose.Translation().X().value()), - t_y(pose.Translation().Y().value()), - t_z(pose.Translation().Z().value()), - r_x(pose.Rotation().X().value()), - r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} - - DifferntiableTransform3d(frc::Transform3d pose) - : t_x(pose.Translation().X().value()), - t_y(pose.Translation().Y().value()), - t_z(pose.Translation().Z().value()), - r_x(pose.Rotation().X().value()), - r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} - - DifferntiableTransform3d(Eigen::Matrix4d matrix) - : t_x(matrix(0, 3)), t_y(matrix(1, 3)), t_z(matrix(2, 3)) { - Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); - Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); - r_x = euler(2); - r_y = euler(1); - r_z = euler(0); - } - - void Update(transform3d_derrivative_t derrivative) { - t_x -= derrivative.t_x * LR; - t_y -= derrivative.t_y * LR; - t_z -= derrivative.t_z * LR; - - r_x -= derrivative.r_x * LR; - r_y -= derrivative.r_y * LR; - r_z -= derrivative.r_z * LR; - } - - void RegisterInputs(tape_type& tape) { - tape.registerInput(r_x); - tape.registerInput(r_y); - tape.registerInput(r_z); - - tape.registerInput(t_x); - tape.registerInput(t_y); - tape.registerInput(t_z); - } - - void RegisterOutputs(tape_type& tape) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); - } - } - } - - auto ToEigen() -> Eigen::Matrix4d const { - // clang-format off - return (Eigen::Matrix() << - matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), - matrix[1][0].value(), matrix[1][1].value(), matrix[1][2].value(), matrix[1][3].value(), - matrix[2][0].value(), matrix[2][1].value(), matrix[2][2].value(), matrix[2][3].value(), - matrix[3][0].value(), matrix[3][1].value(), matrix[3][2].value(), matrix[3][3].value()) - .finished(); - // clang-format on - } - void CalculateMatrix() { - AD cos_x = xad::cos(r_x); - AD cos_y = xad::cos(r_y); - AD cos_z = xad::cos(r_z); - - AD sin_x = xad::sin(r_x); - AD sin_y = xad::sin(r_y); - AD sin_z = xad::sin(r_z); - - // Rotation - matrix[0][0] = cos_z * cos_y; - matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; - matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; - - matrix[1][0] = sin_z * cos_y; - matrix[1][1] = sin_z * sin_y * sin_x + cos_z * cos_x; - matrix[1][2] = sin_z * sin_y * cos_x - cos_z * sin_x; - - matrix[2][0] = -sin_y; - matrix[2][1] = cos_y * sin_x; - matrix[2][2] = cos_y * cos_x; - - // Translation - matrix[0][3] = t_x; - matrix[1][3] = t_y; - matrix[2][3] = t_z; - - matrix[3][0] = 0; - matrix[3][1] = 0; - matrix[3][2] = 0; - matrix[3][3] = 1; - } - - auto BackPropagate(const Eigen::Matrix4d& next_derrivative, tape_type& tape) - -> transform3d_derrivative_t { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); - derivative(matrix[i][j]) = next_derrivative(i, j); - } - } - tape.computeAdjoints(); - transform3d_derrivative_t derrivative{ - .t_x = xad::derivative(t_x), - .t_y = xad::derivative(t_y), - .t_z = xad::derivative(t_z), - .r_x = xad::derivative(r_x), - .r_y = xad::derivative(r_y), - .r_z = xad::derivative(r_z), - }; - return derrivative; - } - void Apply(const transform3d_derrivative_t& derrivative) { - t_x -= derrivative.t_x * LR; - t_y -= derrivative.t_y * LR; - t_z -= derrivative.t_z * LR; - - r_x -= derrivative.r_x * LR; - r_y -= derrivative.r_y * LR; - r_z -= derrivative.r_z * LR; - } -}; - -auto operator<<(std::ostream& os, const DifferntiableTransform3d& v) - -> std::ostream& { - os << "tx: " << v.t_x << "\n"; - os << "ty: " << v.t_y << "\n"; - os << "tz: " << v.t_z << "\n"; - - os << "rx: " << v.r_x << "\n"; - os << "ry: " << v.r_y << "\n"; - os << "rz: " << v.r_z << "\n"; - return os; -} - // clang-format off const Eigen::Matrix PI = (Eigen::Matrix() << @@ -485,8 +290,9 @@ TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT auto square_solver_solution = square_solver_right_->EstimatePosition({detection})[0]; - DifferntiableTransform3d transform_from_pose(square_solver_solution.pose); - DifferntiableTransform3d transform_from_eigen( + JointSolver::DifferntiableTransform3d transform_from_pose( + square_solver_solution.pose); + JointSolver::DifferntiableTransform3d transform_from_eigen( square_solver_solution.pose.ToMatrix()); const double tolerance = 1e-7; @@ -515,7 +321,7 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT auto square_solver_solution = square_solver_right_->EstimatePosition({detection})[0]; - DifferntiableTransform3d transform(square_solver_solution.pose); + JointSolver::DifferntiableTransform3d transform(square_solver_solution.pose); transform.CalculateMatrix(); EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( @@ -544,7 +350,8 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT auto feild_to_robot = square_solver_solution.pose.ToMatrix(); - DifferntiableTransform3d robot_to_feild(feild_to_robot.inverse()); + JointSolver::DifferntiableTransform3d robot_to_feild( + feild_to_robot.inverse()); // Noise robot_to_feild.t_x += 0.1; @@ -561,7 +368,7 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT utils::Timer solve_timer("solve", false); for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; - transform3d_derrivative_t derrivative; + JointSolver::transform3d_derrivative_t derrivative; for (const auto& detection : detections) { auto feild_to_tag = kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); @@ -590,7 +397,7 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT tape.newRecording(); } } - robot_to_feild.Apply(derrivative); + robot_to_feild.Apply(derrivative, LR); } ASSERT_LT(solve_timer.Stop(), 0.1); ASSERT_LT(loss, 0.01); @@ -619,7 +426,8 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT detections[camera_constant_right_.name])[0]; auto feild_to_robot = square_solver_solution.pose.ToMatrix(); - DifferntiableTransform3d robot_to_feild(feild_to_robot.inverse()); + JointSolver::DifferntiableTransform3d robot_to_feild( + feild_to_robot.inverse()); // Noise robot_to_feild.t_x += 0.1; @@ -636,10 +444,10 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT std::vector camera_constants{camera_constant_left_, camera_constant_right_}; utils::Timer solve_timer("solve", false); - transform3d_derrivative_t velocity; + JointSolver::transform3d_derrivative_t velocity; for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; - transform3d_derrivative_t derrivative; + JointSolver::transform3d_derrivative_t derrivative; for (const auto& camera_constant : camera_constants) { auto camera_to_robot = @@ -682,7 +490,7 @@ TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT } } velocity = (velocity * BETA) + derrivative; - robot_to_feild.Apply(velocity); + robot_to_feild.Apply(velocity, LR); } ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized ASSERT_LT(loss, 0.01); From e9c1261615c582d0eacf3c7c7575159898bc2175 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 26 Apr 2026 17:15:15 +0000 Subject: [PATCH 23/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 112 +++++++++++++++++++++++++- src/localization/joint_solver.h | 3 + src/localization/position_receiver.cc | 12 +-- src/localization/position_receiver.h | 6 +- 4 files changed, 122 insertions(+), 11 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index fda09a0a..7b194566 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -7,6 +7,13 @@ namespace localization { +#define IMAGE_STRIDE 4 +#define LOG_PATH "/bos-logs/log181/right" +#define LR 0.05 +#define EPOCHS 100 +#define NORMALIZATION 1000 +#define BETA 0.3 + using frc::AprilTagFieldLayout; using utils::CameraMatrixFromJson; using utils::ExtrinsicsJsonToCameraToRobot; @@ -33,10 +40,67 @@ void NormalizeCameraMatrix(Eigen::Matrix3d& camera_matrix) { camera_matrix(0, 0) = 1; } +auto CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, int corner_index) + -> Eigen::Matrix4d { + Eigen::Vector3d projected_point = + camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * + rotate_yaw * + localization::kapriltag_corners_eigen_homogenized[corner_index]; + auto normalized_point = projected_point / projected_point[0]; + + auto normalized_points_d = normalized_point - image_point; + + // clang-format off + auto projected_point_d = (Eigen::Vector3d() << + (-normalized_points_d[1] * projected_point[1]) / (projected_point[0] * projected_point[0]) + + (-normalized_points_d[2] * projected_point[1]) / (projected_point[0] * projected_point[0]), + normalized_points_d[1] / projected_point[0], + normalized_points_d[2] / projected_point[0] + ).finished(); + // clang-format on + + auto camera_matrix_xd = camera_matrix.transpose() * projected_point_d; + + auto PI_xd = PI.transpose() * camera_matrix_xd; + + auto camera_to_robot_xd = camera_to_robot.transpose() * PI_xd; + + auto robot_to_feild_d = + camera_to_robot_xd * + + (feild_to_tag * rotate_yaw * + localization::kapriltag_corners_eigen_homogenized[corner_index]) + .transpose(); + + return robot_to_feild_d; +} + +auto CalculateLoss(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, int corner_index) + -> double { + + Eigen::Vector3d projected_point = + camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * + rotate_yaw * + localization::kapriltag_corners_eigen_homogenized[corner_index]; + auto normalized_point = projected_point / projected_point[0]; + + auto normalized_points_d = normalized_point - image_point; + + return normalized_points_d.array().square().sum(); +} + JointSolver::JointSolver( const std::vector& camera_constants, const AprilTagFieldLayout& layout) { - for (int i = 0; i < camera_constants.size(); i++) { + for (size_t i = 0; i < camera_constants.size(); i++) { camera_name_to_index[camera_constants[i].name] = i; normalized_camera_matrix_.emplace_back( @@ -55,6 +119,52 @@ auto JointSolver::EstimatePosition( const std::map>& camera_detections, const frc::Pose3d& starting_pose) -> position_estimate_t { + tape_.clearAll(); + DifferntiableTransform3d robot_to_feild( + frc::Pose3d(position_receiver_.Get()).ToMatrix().inverse()); + + JointSolver::transform3d_derrivative_t velocity; + double loss = 0; + for (int epoch = 0; epoch < EPOCHS; epoch++) { + loss = 0; + JointSolver::transform3d_derrivative_t derrivative; + for (auto const& [camera_name, tag_detections] : camera_detections) { + int camera_index = camera_name_to_index[camera_name]; + const auto& camera_to_robot = camera_to_robot_[camera_index]; + const auto& camera_matrix = normalized_camera_matrix_[camera_index]; + + for (const auto& detection : tag_detections) { + auto feild_to_tag = + kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); + + for (int corner_index = 0; corner_index < 4; corner_index++) { + robot_to_feild.CalculateMatrix(); + robot_to_feild.RegisterInputs(tape_); + const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); + auto image_point = (Eigen::Vector3d() << 1, + detection.corners[corner_index].x / NORMALIZATION, + detection.corners[corner_index].y / NORMALIZATION) + .finished(); + + auto robot_to_feild_d = CalculateDerivative( + robot_to_feild_eigen, feild_to_tag, camera_to_robot, + camera_matrix, image_point, corner_index); + + robot_to_feild.RegisterOutputs(tape_); + derrivative = derrivative + + robot_to_feild.BackPropagate(robot_to_feild_d, tape_); + + loss += + CalculateLoss(robot_to_feild_eigen, feild_to_tag, camera_to_robot, + camera_matrix, image_point, corner_index); + + tape_.newRecording(); + } + } + } + velocity = (velocity * BETA) + derrivative; + robot_to_feild.Apply(velocity, LR); + } return {}; } diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index a885fb60..584601f3 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -4,6 +4,7 @@ #include #include "nlohmann/json.hpp" #include "src/camera/camera_constants.h" +#include "src/localization/position_receiver.h" #include "src/localization/position_solver.h" #include "src/localization/square_solver.h" @@ -204,5 +205,7 @@ class JointSolver { std::unordered_map camera_name_to_index; std::vector normalized_camera_matrix_; std::vector camera_to_robot_; + PositionReceiver position_receiver_; + tape_type tape_; }; } // namespace localization diff --git a/src/localization/position_receiver.cc b/src/localization/position_receiver.cc index 063c417c..75b0f1bf 100644 --- a/src/localization/position_receiver.cc +++ b/src/localization/position_receiver.cc @@ -4,14 +4,14 @@ namespace localization { PositionReceiver::PositionReceiver() { auto instance = nt::NetworkTableInstance::GetDefault(); - std::shared_ptr table = instance.GetTable(""); - nt::StructTopic pose3d_topic = - table->GetStructTopic("Pose3d"); - pose3d_subscriber_ = pose3d_topic.Subscribe({}); + std::shared_ptr table = instance.GetTable("DriveState"); + nt::StructTopic pose_topic = + table->GetStructTopic("Pose"); + pose_subscriber_ = pose_topic.Subscribe({}); } -auto PositionReceiver::Get() -> frc::Pose3d { - return pose3d_subscriber_.Get(); +auto PositionReceiver::Get() -> frc::Pose2d { + return pose_subscriber_.Get(); } } // namespace localization diff --git a/src/localization/position_receiver.h b/src/localization/position_receiver.h index ad4922df..904d9c76 100644 --- a/src/localization/position_receiver.h +++ b/src/localization/position_receiver.h @@ -9,11 +9,9 @@ namespace localization { class PositionReceiver { public: PositionReceiver(); - auto Get() -> frc::Pose3d; + auto Get() -> frc::Pose2d; private: - nt::StructSubscriber pose3d_subscriber_; - - std::mutex mutex_; + nt::StructSubscriber pose_subscriber_; }; } // namespace localization From 692549d7330beaa97f5db643050d0801265a6bd9 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 27 Apr 2026 05:14:48 +0000 Subject: [PATCH 24/47] wip Signed-off-by: Charlie Huang --- constants/second_bot/left_intrinsics.json | 10 +- src/localization/joint_solver.cc | 70 +++-- src/localization/joint_solver.h | 44 ++- src/localization/position_receiver.cc | 6 +- src/localization/position_receiver.h | 4 +- src/test/unit_test/CMakeLists.txt | 3 - src/test/unit_test/forward_test.cc | 322 +++++++++++----------- src/test/unit_test/joint_solve_test.cc | 54 ---- 8 files changed, 254 insertions(+), 259 deletions(-) delete mode 100644 src/test/unit_test/joint_solve_test.cc diff --git a/constants/second_bot/left_intrinsics.json b/constants/second_bot/left_intrinsics.json index 0414268f..db6395b4 100644 --- a/constants/second_bot/left_intrinsics.json +++ b/constants/second_bot/left_intrinsics.json @@ -3,9 +3,9 @@ "cy": 399.3051603994019, "fx": 905.4343172084684, "fy": 905.0217994058659, - "k1": 0.04175770867385965, - "k2": -0.07919120051340409, - "k3": 0.014128797448067021, - "p1": -0.0023943526206403067, - "p2": -0.003217715458550516 + "k1": 0.0, + "k2": 0.0, + "k3": 0.0, + "p1": 0.0, + "p2": 0.0 }% diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 7b194566..f482b26e 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -11,7 +11,7 @@ namespace localization { #define LOG_PATH "/bos-logs/log181/right" #define LR 0.05 #define EPOCHS 100 -#define NORMALIZATION 1000 +#define NORMALIZATION 500 #define BETA 0.3 using frc::AprilTagFieldLayout; @@ -40,12 +40,12 @@ void NormalizeCameraMatrix(Eigen::Matrix3d& camera_matrix) { camera_matrix(0, 0) = 1; } -auto CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, int corner_index) - -> Eigen::Matrix4d { +auto JointSolver::CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, + int corner_index) -> Eigen::Matrix4d { Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * rotate_yaw * @@ -79,12 +79,12 @@ auto CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, return robot_to_feild_d; } -auto CalculateLoss(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, int corner_index) - -> double { +auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, + int corner_index) -> double { Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * @@ -94,6 +94,9 @@ auto CalculateLoss(const Eigen::Matrix4d& robot_to_feild, auto normalized_points_d = normalized_point - image_point; + // LOG(INFO) << "\n" << normalized_point << "\n" << image_point; + // LOG(INFO) << normalized_points_d.array().square().sum(); + return normalized_points_d.array().square().sum(); } @@ -118,10 +121,24 @@ JointSolver::JointSolver( auto JointSolver::EstimatePosition( const std::map>& camera_detections, - const frc::Pose3d& starting_pose) -> position_estimate_t { + std::optional initial_pose) -> position_estimate_t { + std::vector tag_ids; + double average_timestamp = 0; + int total_detections = 0; + for (const auto& [_, tag_detections] : camera_detections) { + for (const auto& tag_detection : tag_detections) { + tag_ids.push_back(tag_detection.tag_id); + average_timestamp += tag_detection.timestamp; + total_detections++; + } + } + average_timestamp /= total_detections; + tape_.clearAll(); DifferntiableTransform3d robot_to_feild( - frc::Pose3d(position_receiver_.Get()).ToMatrix().inverse()); + frc::Pose3d(initial_pose.value_or(position_receiver_.Get())) + .ToMatrix() + .inverse()); JointSolver::transform3d_derrivative_t velocity; double loss = 0; @@ -163,9 +180,28 @@ auto JointSolver::EstimatePosition( } } velocity = (velocity * BETA) + derrivative; - robot_to_feild.Apply(velocity, LR); + robot_to_feild.Update(velocity, LR); } - return {}; + robot_to_feild.CalculateMatrix(); + + Eigen::Matrix4d feild_to_robot = robot_to_feild.ToEigen().inverse(); + feild_to_robot(3, 0) = 0; + feild_to_robot(3, 1) = 0; + feild_to_robot(3, 2) = 0; + feild_to_robot(3, 3) = 1; + + return position_estimate_t{ + .tag_ids = tag_ids, + .rejected_tag_ids = {}, + .pose = frc::Pose3d(feild_to_robot), + .variance = 1, + .timestamp = average_timestamp, + .num_tags = total_detections, + .avg_tag_dist = -1, + .latency = -1, + .invalid = false, + .loss = 0, + }; } } // namespace localization diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 584601f3..ba96e908 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -97,8 +97,8 @@ class JointSolver { t_y -= derrivative.t_y * lr; t_z -= derrivative.t_z * lr; - r_x -= derrivative.r_x * lr; - r_y -= derrivative.r_y * lr; + // r_x -= derrivative.r_x * lr; + // r_y -= derrivative.r_y * lr; r_z -= derrivative.r_z * lr; } @@ -182,24 +182,31 @@ class JointSolver { }; return derrivative; } - void Apply(const transform3d_derrivative_t& derrivative, double lr) { - t_x -= derrivative.t_x * lr; - t_y -= derrivative.t_y * lr; - t_z -= derrivative.t_z * lr; - - r_x -= derrivative.r_x * lr; - r_y -= derrivative.r_y * lr; - r_z -= derrivative.r_z * lr; - } }; + public: + auto static CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, + int corner_index) -> Eigen::Matrix4d; + + auto static CalculateLoss(const Eigen::Matrix4d& robot_to_feild, + const Eigen::Matrix4d& feild_to_tag, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Vector3d& image_point, + int corner_index) -> double; + public: JointSolver(const std::vector& camera_constants, const frc::AprilTagFieldLayout& layout = kapriltag_layout); auto EstimatePosition( const std::map>& camera_detections, - const frc::Pose3d& starting_pose) -> position_estimate_t; + std::optional intial_pose = std::nullopt) + -> position_estimate_t; private: std::unordered_map camera_name_to_index; @@ -208,4 +215,17 @@ class JointSolver { PositionReceiver position_receiver_; tape_type tape_; }; + +inline auto operator<<(std::ostream& os, + const JointSolver::Transfrom3dDerrivative& d) + -> std::ostream& { + os << "t_x=" << d.t_x << "\n" + << "t_y=" << d.t_y << "\n" + << "t_z=" << d.t_z << "\n" + << "r_x=" << d.r_x << "\n" + << "r_y=" << d.r_y << "\n" + << "r_z=" << d.r_z << ""; + return os; +} } // namespace localization +// diff --git a/src/localization/position_receiver.cc b/src/localization/position_receiver.cc index 75b0f1bf..8a8c807f 100644 --- a/src/localization/position_receiver.cc +++ b/src/localization/position_receiver.cc @@ -5,12 +5,12 @@ namespace localization { PositionReceiver::PositionReceiver() { auto instance = nt::NetworkTableInstance::GetDefault(); std::shared_ptr table = instance.GetTable("DriveState"); - nt::StructTopic pose_topic = - table->GetStructTopic("Pose"); + nt::StructTopic pose_topic = + table->GetStructTopic("Pose"); pose_subscriber_ = pose_topic.Subscribe({}); } -auto PositionReceiver::Get() -> frc::Pose2d { +auto PositionReceiver::Get() -> frc::Pose3d { return pose_subscriber_.Get(); } diff --git a/src/localization/position_receiver.h b/src/localization/position_receiver.h index 904d9c76..61238825 100644 --- a/src/localization/position_receiver.h +++ b/src/localization/position_receiver.h @@ -9,9 +9,9 @@ namespace localization { class PositionReceiver { public: PositionReceiver(); - auto Get() -> frc::Pose2d; + auto Get() -> frc::Pose3d; private: - nt::StructSubscriber pose_subscriber_; + nt::StructSubscriber pose_subscriber_; }; } // namespace localization diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index f0bee837..5dead05d 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -1,8 +1,5 @@ enable_testing() -add_executable(joint_solve_test joint_solve_test.cc) -target_link_libraries(joint_solve_test PRIVATE localization utils GTest::gtest GTest::gtest_main) - add_executable(multi_tag_test multi_tag_test.cc) target_link_libraries(multi_tag_test PRIVATE localization utils GTest::gtest_main) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 1f887f12..7fdf1453 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -13,10 +13,10 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" -#define LR 0.05 -#define EPOCHS 100 -#define NORMALIZATION 1000 -#define BETA 0.3 +#define LR 0.01 +#define EPOCHS 10000 +#define NORMALIZATION 500 +#define BETA 0.99 namespace fs = std::filesystem; @@ -121,63 +121,6 @@ auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, return normalized_point; } -auto CalculateLoss(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, int corner_index) - -> double { - - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * - rotate_yaw * - localization::kapriltag_corners_eigen_homogenized[corner_index]; - auto normalized_point = projected_point / projected_point[0]; - - auto normalized_points_d = normalized_point - image_point; - - return normalized_points_d.array().square().sum(); -} - -auto CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, int corner_index) - -> Eigen::Matrix4d { - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * - rotate_yaw * - localization::kapriltag_corners_eigen_homogenized[corner_index]; - auto normalized_point = projected_point / projected_point[0]; - - auto normalized_points_d = normalized_point - image_point; - - // clang-format off - auto projected_point_d = (Eigen::Vector3d() << - (-normalized_points_d[1] * projected_point[1]) / (projected_point[0] * projected_point[0]) + - (-normalized_points_d[2] * projected_point[1]) / (projected_point[0] * projected_point[0]), - normalized_points_d[1] / projected_point[0], - normalized_points_d[2] / projected_point[0] - ).finished(); - // clang-format on - - auto camera_matrix_xd = camera_matrix.transpose() * projected_point_d; - - auto PI_xd = PI.transpose() * camera_matrix_xd; - - auto camera_to_robot_xd = camera_to_robot.transpose() * PI_xd; - - auto robot_to_feild_d = - camera_to_robot_xd * - - (feild_to_tag * rotate_yaw * - localization::kapriltag_corners_eigen_homogenized[corner_index]) - .transpose(); - - return robot_to_feild_d; -} - // TODO: Tolerance is quite high. Find out what causes the loss in precision void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, double tolerance = 1) { @@ -267,15 +210,15 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT detection.corners[corner_index].y / NORMALIZATION) .finished(); - robot_to_feild_d += CalculateDerivative( + robot_to_feild_d += JointSolver::CalculateDerivative( robot_to_feild, feild_to_tag, camera_to_robot_right_, normalized_camera_matrix_right_, image_point, corner_index); - loss += CalculateLoss( + loss += JointSolver::CalculateLoss( robot_to_feild, feild_to_tag, camera_to_robot_right_, normalized_camera_matrix_right_, image_point, corner_index); } - robot_to_feild -= robot_to_feild_d * LR; + robot_to_feild -= robot_to_feild_d * 0.01; } ASSERT_LT(loss, 0.001); } @@ -336,12 +279,12 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT } TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/20.411762.jpg"); - // LOG(INFO) << detections.size(); + cv::Mat image = cv::imread("/bos-logs/log181/right/6.767740.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; auto detections = detector_right_->GetTagDetections(timestamped_frame); + LOG(INFO) << detections.size(); // PCHECK(detections.size() > 1) // << "Need at least two detections to test differnt tag positions"; @@ -353,19 +296,23 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT JointSolver::DifferntiableTransform3d robot_to_feild( feild_to_robot.inverse()); + robot_to_feild.CalculateMatrix(); + LOG(INFO) << "robot_to_feild_before\n" << robot_to_feild.ToEigen(); + // Noise robot_to_feild.t_x += 0.1; robot_to_feild.t_y += 0.1; robot_to_feild.t_z += 0.1; - robot_to_feild.r_x += 0.1; - robot_to_feild.r_y += 0.1; - robot_to_feild.r_z += 0.1; + // robot_to_feild.r_x += 0.05; + // robot_to_feild.r_y += 0.05; + robot_to_feild.r_z += 0.05; tape_type tape; double loss = 0; utils::Timer solve_timer("solve", false); + JointSolver::transform3d_derrivative_t velocity; for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; JointSolver::transform3d_derrivative_t derrivative; @@ -382,7 +329,7 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT detection.corners[corner_index].y / NORMALIZATION) .finished(); - auto robot_to_feild_d = CalculateDerivative( + auto robot_to_feild_d = JointSolver::CalculateDerivative( robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, normalized_camera_matrix_right_, image_point, corner_index); @@ -390,108 +337,157 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT derrivative = derrivative + robot_to_feild.BackPropagate(robot_to_feild_d, tape); - loss += CalculateLoss( + loss += JointSolver::CalculateLoss( robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, normalized_camera_matrix_right_, image_point, corner_index); tape.newRecording(); } } - robot_to_feild.Apply(derrivative, LR); - } - ASSERT_LT(solve_timer.Stop(), 0.1); - ASSERT_LT(loss, 0.01); -} -TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT - cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.411762.jpg"); - cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); - std::map> detections; - { - timestamped_frame_t timestamped_frame{ - .frame = std::move(image_right), .timestamp = 0, .invalid = false}; - detections[camera_constant_right_.name] = - detector_right_->GetTagDetections(timestamped_frame); - CHECK(!detections[camera_constant_right_.name].empty()); - } - { - timestamped_frame_t timestamped_frame{ - .frame = std::move(image_left), .timestamp = 0, .invalid = false}; - detections[camera_constant_left_.name] = - detector_left_->GetTagDetections(timestamped_frame); - CHECK(!detections[camera_constant_left_.name].empty()); + velocity = (velocity * BETA) + derrivative; + robot_to_feild.Update(velocity, LR); + if (epoch == 0) { + LOG(INFO) << loss; + } } - auto square_solver_solution = square_solver_right_->EstimatePosition( - detections[camera_constant_right_.name])[0]; - - auto feild_to_robot = square_solver_solution.pose.ToMatrix(); - JointSolver::DifferntiableTransform3d robot_to_feild( - feild_to_robot.inverse()); - - // Noise - robot_to_feild.t_x += 0.1; - robot_to_feild.t_y += 0.1; - robot_to_feild.t_z += 0.1; - - robot_to_feild.r_x += 0.1; - robot_to_feild.r_y += 0.1; - robot_to_feild.r_z += 0.1; + robot_to_feild.CalculateMatrix(); + LOG(INFO) << "robot_to_feild_after\n" << robot_to_feild.ToEigen(); + LOG(INFO) << loss; - tape_type tape; - - double loss = 0; - std::vector camera_constants{camera_constant_left_, - camera_constant_right_}; - utils::Timer solve_timer("solve", false); - JointSolver::transform3d_derrivative_t velocity; - for (int epoch = 0; epoch < EPOCHS; epoch++) { - loss = 0; - JointSolver::transform3d_derrivative_t derrivative; - for (const auto& camera_constant : camera_constants) { - - auto camera_to_robot = - utils::ExtrinsicsJsonToCameraToRobot( - utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) - .ToMatrix(); - - auto camera_matrix = CameraMatrixFromJson( - ReadIntrinsics(camera_constant.intrinsics_path.value())); - Eigen::Matrix3d normalized_camera_matrix = camera_matrix / NORMALIZATION; - normalized_camera_matrix(0, 0) = 1; - - for (const auto& detection : detections[camera_constant.name]) { - auto feild_to_tag = - kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); - - for (int corner_index = 0; corner_index < 4; corner_index++) { - robot_to_feild.CalculateMatrix(); - robot_to_feild.RegisterInputs(tape); - const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) - .finished(); - - auto robot_to_feild_d = CalculateDerivative( - robot_to_feild_eigen, feild_to_tag, camera_to_robot, - normalized_camera_matrix, image_point, corner_index); - - robot_to_feild.RegisterOutputs(tape); - derrivative = derrivative + - robot_to_feild.BackPropagate(robot_to_feild_d, tape); - - loss += CalculateLoss(robot_to_feild_eigen, feild_to_tag, - camera_to_robot, normalized_camera_matrix, - image_point, corner_index); - - tape.newRecording(); - } - } - } - velocity = (velocity * BETA) + derrivative; - robot_to_feild.Apply(velocity, LR); - } - ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized + ASSERT_LT(solve_timer.Stop(), 0.1); ASSERT_LT(loss, 0.01); } + +// TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT +// cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.411762.jpg"); +// +// cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); +// std::map> detections; +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_right), .timestamp = 0, .invalid = false}; +// detections[camera_constant_right_.name] = +// detector_right_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_right_.name].empty()); +// } +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_left), .timestamp = 0, .invalid = false}; +// detections[camera_constant_left_.name] = +// detector_left_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_left_.name].empty()); +// } +// +// auto square_solver_solution = square_solver_right_->EstimatePosition( +// detections[camera_constant_right_.name])[0]; +// +// auto feild_to_robot = square_solver_solution.pose.ToMatrix(); +// JointSolver::DifferntiableTransform3d robot_to_feild( +// feild_to_robot.inverse()); +// +// // Noise +// robot_to_feild.t_x += 0.1; +// robot_to_feild.t_y += 0.1; +// robot_to_feild.t_z += 0.1; +// +// robot_to_feild.r_x += 0.1; +// robot_to_feild.r_y += 0.1; +// robot_to_feild.r_z += 0.1; +// +// tape_type tape; +// +// double loss = 0; +// std::vector camera_constants{camera_constant_left_, +// camera_constant_right_}; +// utils::Timer solve_timer("solve", false); +// JointSolver::transform3d_derrivative_t velocity; +// for (int epoch = 0; epoch < EPOCHS; epoch++) { +// loss = 0; +// JointSolver::transform3d_derrivative_t derrivative; +// for (const auto& camera_constant : camera_constants) { +// +// auto camera_to_robot = +// utils::ExtrinsicsJsonToCameraToRobot( +// utils::ReadExtrinsics(camera_constant.extrinsics_path.value())) +// .ToMatrix(); +// +// auto camera_matrix = CameraMatrixFromJson( +// ReadIntrinsics(camera_constant.intrinsics_path.value())); +// Eigen::Matrix3d normalized_camera_matrix = camera_matrix / NORMALIZATION; +// normalized_camera_matrix(0, 0) = 1; +// +// for (const auto& detection : detections[camera_constant.name]) { +// auto feild_to_tag = +// kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); +// +// for (int corner_index = 0; corner_index < 4; corner_index++) { +// robot_to_feild.CalculateMatrix(); +// robot_to_feild.RegisterInputs(tape); +// const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); +// auto image_point = (Eigen::Vector3d() << 1, +// detection.corners[corner_index].x / NORMALIZATION, +// detection.corners[corner_index].y / NORMALIZATION) +// .finished(); +// +// auto robot_to_feild_d = JointSolver::CalculateDerivative( +// robot_to_feild_eigen, feild_to_tag, camera_to_robot, +// normalized_camera_matrix, image_point, corner_index); +// +// robot_to_feild.RegisterOutputs(tape); +// derrivative = derrivative + +// robot_to_feild.BackPropagate(robot_to_feild_d, tape); +// +// loss += JointSolver::CalculateLoss( +// robot_to_feild_eigen, feild_to_tag, camera_to_robot, +// normalized_camera_matrix, image_point, corner_index); +// +// tape.newRecording(); +// } +// } +// } +// velocity = (velocity * BETA) + derrivative; +// robot_to_feild.Apply(velocity, LR); +// } +// ASSERT_LT(solve_timer.Stop(), 0.1); // The test is very unoptimized +// ASSERT_LT(loss, 0.01); +// } + +// TEST_F(ForwardTest, JointSolverTest) { // NOLINT +// cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.411762.jpg"); +// +// cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); +// std::map> detections; +// +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_right), .timestamp = 0, .invalid = false}; +// detections[camera_constant_right_.name] = +// detector_right_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_right_.name].empty()); +// } +// { +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image_left), .timestamp = 0, .invalid = false}; +// detections[camera_constant_left_.name] = +// detector_left_->GetTagDetections(timestamped_frame); +// CHECK(!detections[camera_constant_left_.name].empty()); +// } +// +// auto square_solver_solution = square_solver_left_->EstimatePosition( +// detections[camera_constant_left_.name])[0]; +// +// std::vector camera_constants{camera_constant_left_, +// camera_constant_right_}; +// +// JointSolver joint_solver(camera_constants); +// position_estimate_t joint_solver_solution = +// joint_solver.EstimatePosition(detections, square_solver_solution.pose); +// +// // LOG(INFO) << "square_solver_solution_pose\n" +// // << square_solver_solution.pose.ToMatrix(); +// // +// // LOG(INFO) << "joint_solver_solution_pose\n" +// // << joint_solver_solution.pose.ToMatrix(); +// } diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc deleted file mode 100644 index f9be2e63..00000000 --- a/src/test/unit_test/joint_solve_test.cc +++ /dev/null @@ -1,54 +0,0 @@ -#include -#include "src/localization/joint_solver.h" -#include "src/localization/opencv_apriltag_detector.h" -#include "src/localization/square_solver.h" -#include "src/utils/camera_utils.h" -#include "src/utils/constants_from_json.h" -#include "src/utils/transform.h" - -std::vector detections( - {{.tag_id = 31, - .corners = {cv::Point2d(100.0, 200.0), cv::Point2d(200.0, 200.0), - cv::Point2d(200.0, 100.0), cv::Point2d(100.0, 100.0)}, - .timestamp = 0.0, - .confidence = 0.0}}); - -TEST(JointSolveTest, EstimatePosition) { // NOLINT - camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("second_bot_left"); - std::string image_path = "/bos-logs/log181/left/9.627390.jpg"; - - localization::JointSolver joint_solver({camera_constant}); - localization::SquareSolver square_solver(camera_constant); - auto detector = std::make_unique( - camera_constant.frame_width.value(), camera_constant.frame_height.value(), - utils::ReadIntrinsics(camera_constant.intrinsics_path.value())); - - cv::Mat image = cv::imread(image_path); - camera::timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - - auto detections = detector->GetTagDetections(timestamped_frame); - - auto square_solver_solution = square_solver.EstimatePosition({detections})[0]; - - utils::PrintTransformationMatrix( - utils::EigenToCvMat(square_solver_solution.pose.ToMatrix())); - std::cout << square_solver_solution.pose.ToMatrix() << std::endl; - - frc::Transform3d noise( - frc::Translation3d(units::meter_t{0.067}, units::meter_t{0.1}, - units::meter_t{-0.1}), - {}); - - square_solver_solution.pose = square_solver_solution.pose.TransformBy(noise); - - std::map> - associated_detections; - associated_detections.insert({camera_constant.name, detections}); - auto joint_solver_solution = joint_solver.EstimatePosition( - associated_detections, square_solver_solution.pose); - std::cout << "Joint solver solution: " << joint_solver_solution << std::endl; - std::cout << "Square solver solution: " << square_solver_solution - << std::endl; -} From 918c34d37393c42a2d2f1b41f5111abea1766c45 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 27 Apr 2026 06:01:40 +0000 Subject: [PATCH 25/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 51 ++++++++++++++++++++---- src/localization/joint_solver.h | 14 +++++++ src/test/unit_test/forward_test.cc | 63 ++++++++++++------------------ 3 files changed, 83 insertions(+), 45 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index f482b26e..cc40c18b 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -35,11 +35,6 @@ const Eigen::Matrix rotate_yaw = 0, 0, 0, 1).finished(); // clang-format on -void NormalizeCameraMatrix(Eigen::Matrix3d& camera_matrix) { - camera_matrix = camera_matrix / 1000; - camera_matrix(0, 0) = 1; -} - auto JointSolver::CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, const Eigen::Matrix4d& feild_to_tag, const Eigen::Matrix4d& camera_to_robot, @@ -100,16 +95,56 @@ auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, return normalized_points_d.array().square().sum(); } +auto JointSolver::NormalizePoint( + const cv::Point2d& image_point, + const camera::camera_constant_t& camera_constant) -> Eigen::Vector3d { + // clang-format off + return (Eigen::Vector3d() << + 1, + image_point.x / camera_constant.frame_width.value(), + image_point.y / camera_constant.frame_height.value()) + .finished(); + // clang-format on +} + +auto JointSolver::ProjectPoints(const frc::Pose3d& camera_pose, + const frc::Pose3d& tag_pose, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Matrix4d& camera_to_robot, + int corner_index) -> Eigen::Vector3d { + auto feild_to_robot = camera_pose.ToMatrix(); + auto feild_to_tag = tag_pose.ToMatrix(); + auto camera_to_tag = + camera_to_robot * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; + + Eigen::Vector3d projected_point = + camera_matrix * PI * camera_to_tag * + localization::kapriltag_corners_eigen_homogenized[corner_index]; + auto normalized_point = projected_point / projected_point[0]; + return normalized_point; +} + +auto JointSolver::NormalizeCameraMatrix( + Eigen::Matrix3d camera_matrix, + const camera::camera_constant_t& camera_constant) -> Eigen::Matrix3d { + camera_matrix(1, 0) /= camera_constant.frame_width.value(); + camera_matrix(1, 1) /= camera_constant.frame_width.value(); + + camera_matrix(2, 0) /= camera_constant.frame_height.value(); + camera_matrix(2, 2) /= camera_constant.frame_height.value(); + return camera_matrix; +} + JointSolver::JointSolver( const std::vector& camera_constants, const AprilTagFieldLayout& layout) { for (size_t i = 0; i < camera_constants.size(); i++) { camera_name_to_index[camera_constants[i].name] = i; - normalized_camera_matrix_.emplace_back( + normalized_camera_matrix_.emplace_back(NormalizeCameraMatrix( CameraMatrixFromJson( - ReadIntrinsics(camera_constants[i].intrinsics_path.value()))); - NormalizeCameraMatrix(normalized_camera_matrix_[i]); + ReadIntrinsics(camera_constants[i].intrinsics_path.value())), + camera_constants[i])); camera_to_robot_.emplace_back( ExtrinsicsJsonToCameraToRobot( diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index ba96e908..c5fbd2d1 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -199,6 +199,20 @@ class JointSolver { const Eigen::Vector3d& image_point, int corner_index) -> double; + auto static NormalizePoint(const cv::Point2d& image_point, + const camera::camera_constant_t& camera_constant) + -> Eigen::Vector3d; + + auto static ProjectPoints(const frc::Pose3d& camera_pose, + const frc::Pose3d& tag_pose, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Matrix4d& camera_to_robot, + int corner_index) -> Eigen::Vector3d; + + auto static NormalizeCameraMatrix( + Eigen::Matrix3d camera_matrix, + const camera::camera_constant_t& camera_constant) -> Eigen::Matrix3d; + public: JointSolver(const std::vector& camera_constants, const frc::AprilTagFieldLayout& layout = kapriltag_layout); diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 7fdf1453..ea1203a6 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -13,7 +13,7 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" -#define LR 0.01 +#define LR 0.1 #define EPOCHS 10000 #define NORMALIZATION 500 #define BETA 0.99 @@ -63,8 +63,8 @@ class ForwardTest : public ::testing::Test { ReadIntrinsics(camera_constant_right_.intrinsics_path.value())); camera_matrix_right_ = CameraMatrixFromJson( ReadIntrinsics(camera_constant_right_.intrinsics_path.value())); - normalized_camera_matrix_right_ = camera_matrix_right_ / NORMALIZATION; - normalized_camera_matrix_right_(0, 0) = 1; + normalized_camera_matrix_right_ = JointSolver::NormalizeCameraMatrix( + camera_matrix_right_, camera_constant_right_); camera_to_robot_right_ = utils::ExtrinsicsJsonToCameraToRobot( utils::ReadExtrinsics( @@ -79,8 +79,8 @@ class ForwardTest : public ::testing::Test { ReadIntrinsics(camera_constant_left_.intrinsics_path.value())); camera_matrix_left_ = CameraMatrixFromJson( ReadIntrinsics(camera_constant_left_.intrinsics_path.value())); - normalized_camera_matrix_left_ = camera_matrix_left_ / NORMALIZATION; - normalized_camera_matrix_left_(0, 0) = 1; + normalized_camera_matrix_left_ = JointSolver::NormalizeCameraMatrix( + camera_matrix_left_, camera_constant_left_); camera_to_robot_left_ = utils::ExtrinsicsJsonToCameraToRobot( utils::ReadExtrinsics( @@ -105,29 +105,19 @@ class ForwardTest : public ::testing::Test { Eigen::Matrix4d camera_to_robot_left_; }; -auto ProjectPoints(const frc::Pose3d& camera_pose, const frc::Pose3d& tag_pose, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Matrix4d& camera_to_robot, int corner_index) - -> Eigen::Vector3d { - auto feild_to_robot = camera_pose.ToMatrix(); - auto feild_to_tag = tag_pose.ToMatrix(); - auto camera_to_tag = - camera_to_robot * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; - - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_tag * - localization::kapriltag_corners_eigen_homogenized[corner_index]; - auto normalized_point = projected_point / projected_point[0]; - return normalized_point; -} - // TODO: Tolerance is quite high. Find out what causes the loss in precision void CheckIsEqual(cv::Point2d image_point, Eigen::Vector3d projected_points, - double tolerance = 1) { + double tolerance) { EXPECT_NEAR(image_point.x, projected_points[1], tolerance); EXPECT_NEAR(image_point.y, projected_points[2], tolerance); } +void CheckIsEqual(Eigen::Vector3d image_point, Eigen::Vector3d projected_points, + double tolerance) { + EXPECT_NEAR(image_point[1], projected_points[1], tolerance); + EXPECT_NEAR(image_point[2], projected_points[2], tolerance); +} + TEST_F(ForwardTest, TestForward) { // NOLINT // [u, v] = camera_matrix * PI * camera_to_tag * tag_corners // @@ -161,19 +151,20 @@ TEST_F(ForwardTest, TestForward) { // NOLINT square_solver_right_->EstimatePosition({detection})[0]; for (int j = 0; j < 4; j++) { - auto projected_points = - ProjectPoints(square_solver_solution.pose, - kapriltag_layout.GetTagPose(detection.tag_id).value(), - camera_matrix_right_, camera_to_robot_right_, j); + auto projected_points = JointSolver::ProjectPoints( + square_solver_solution.pose, + kapriltag_layout.GetTagPose(detection.tag_id).value(), + camera_matrix_right_, camera_to_robot_right_, j); - auto projected_points_normalized = ProjectPoints( + auto projected_points_normalized = JointSolver::ProjectPoints( square_solver_solution.pose, kapriltag_layout.GetTagPose(detection.tag_id).value(), normalized_camera_matrix_right_, camera_to_robot_right_, j); CheckIsEqual(detection.corners[j], projected_points, 1); - CheckIsEqual(detection.corners[j] / NORMALIZATION, - projected_points_normalized, 1.0 / NORMALIZATION); + CheckIsEqual(JointSolver::NormalizePoint(detection.corners[j], + camera_constant_right_), + projected_points_normalized, 0.001); } } } @@ -205,10 +196,9 @@ TEST_F(ForwardTest, TestDerrivative) { // NOLINT Eigen::Matrix4d robot_to_feild_d = Eigen::Matrix4d::Zero(); loss = 0; for (int corner_index = 0; corner_index < 4; corner_index++) { - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) - .finished(); + + auto image_point = JointSolver::NormalizePoint( + detection.corners[corner_index], camera_constant_right_); robot_to_feild_d += JointSolver::CalculateDerivative( robot_to_feild, feild_to_tag, camera_to_robot_right_, @@ -324,10 +314,9 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT robot_to_feild.CalculateMatrix(); robot_to_feild.RegisterInputs(tape); const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) - .finished(); + + auto image_point = JointSolver::NormalizePoint( + detection.corners[corner_index], camera_constant_right_); auto robot_to_feild_d = JointSolver::CalculateDerivative( robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, From da9614f0c858dbe34594ad6a563901d90b69cbe9 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 27 Apr 2026 06:10:44 +0000 Subject: [PATCH 26/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 11 +++++++---- src/localization/joint_solver.h | 1 + src/test/unit_test/forward_test.cc | 1 - 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index cc40c18b..5ba5130d 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -11,7 +11,6 @@ namespace localization { #define LOG_PATH "/bos-logs/log181/right" #define LR 0.05 #define EPOCHS 100 -#define NORMALIZATION 500 #define BETA 0.3 using frc::AprilTagFieldLayout; @@ -137,7 +136,8 @@ auto JointSolver::NormalizeCameraMatrix( JointSolver::JointSolver( const std::vector& camera_constants, - const AprilTagFieldLayout& layout) { + const AprilTagFieldLayout& layout) + : camera_constant_(camera_constants) { for (size_t i = 0; i < camera_constants.size(); i++) { camera_name_to_index[camera_constants[i].name] = i; @@ -193,10 +193,13 @@ auto JointSolver::EstimatePosition( robot_to_feild.CalculateMatrix(); robot_to_feild.RegisterInputs(tape_); const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); + + // clang-format off auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / NORMALIZATION, - detection.corners[corner_index].y / NORMALIZATION) + detection.corners[corner_index].x / camera_constant_[camera_index].frame_width.value(), + detection.corners[corner_index].y / camera_constant_[camera_index].frame_height.value()) .finished(); + // clang-format on auto robot_to_feild_d = CalculateDerivative( robot_to_feild_eigen, feild_to_tag, camera_to_robot, diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index c5fbd2d1..44356aac 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -226,6 +226,7 @@ class JointSolver { std::unordered_map camera_name_to_index; std::vector normalized_camera_matrix_; std::vector camera_to_robot_; + std::vector camera_constant_; PositionReceiver position_receiver_; tape_type tape_; }; diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index ea1203a6..f3efbe55 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -15,7 +15,6 @@ #define LOG_PATH "/bos-logs/log181/right" #define LR 0.1 #define EPOCHS 10000 -#define NORMALIZATION 500 #define BETA 0.99 namespace fs = std::filesystem; From 9a109f0a4ff93f5d32bd7196abd62e80dec550b0 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Tue, 28 Apr 2026 03:20:38 +0000 Subject: [PATCH 27/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 2 +- src/localization/joint_solver.h | 49 +++++++++++++++++++++++++++--- src/test/unit_test/forward_test.cc | 30 +++++++++++++----- 3 files changed, 67 insertions(+), 14 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 5ba5130d..6e2484bf 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -218,7 +218,7 @@ auto JointSolver::EstimatePosition( } } velocity = (velocity * BETA) + derrivative; - robot_to_feild.Update(velocity, LR); + robot_to_feild.Update(velocity, LR, LR); } robot_to_feild.CalculateMatrix(); diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 44356aac..d25b3d76 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -54,6 +54,44 @@ class JointSolver { .r_y = r_y * other, .r_z = r_z * other}; } + + auto operator+(const double other) -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x + other, + .t_y = t_y + other, + .t_z = t_z + other, + .r_x = r_x + other, + .r_y = r_y + other, + .r_z = r_z + other}; + } + + auto operator*(const Transfrom3dDerrivative other) + -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x * other.t_x, + .t_y = t_y * other.t_y, + .t_z = t_z * other.t_z, + .r_x = r_x * other.r_x, + .r_y = r_y * other.r_y, + .r_z = r_z * other.r_z}; + } + + auto operator/(const Transfrom3dDerrivative other) + -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = t_x / other.t_x, + .t_y = t_y / other.t_y, + .t_z = t_z / other.t_z, + .r_x = r_x / other.r_x, + .r_y = r_y / other.r_y, + .r_z = r_z / other.r_z}; + } + + auto sqrt() -> Transfrom3dDerrivative { + return Transfrom3dDerrivative{.t_x = std::sqrt(t_x), + .t_y = std::sqrt(t_y), + .t_z = std::sqrt(t_z), + .r_x = std::sqrt(r_x), + .r_y = std::sqrt(r_y), + .r_z = std::sqrt(r_z)}; + } }; struct DifferntiableTransform3d { @@ -92,14 +130,15 @@ class JointSolver { r_z = euler(0); } - void Update(transform3d_derrivative_t derrivative, double lr) { - t_x -= derrivative.t_x * lr; - t_y -= derrivative.t_y * lr; - t_z -= derrivative.t_z * lr; + void Update(transform3d_derrivative_t derrivative, double lr_translation, + double lr_rotation) { + t_x -= derrivative.t_x * lr_translation; + t_y -= derrivative.t_y * lr_translation; + t_z -= derrivative.t_z * lr_translation; // r_x -= derrivative.r_x * lr; // r_y -= derrivative.r_y * lr; - r_z -= derrivative.r_z * lr; + // r_z -= derrivative.r_z * lr_rotation; } void RegisterInputs(tape_type& tape) { diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index f3efbe55..35b2ceab 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -13,9 +13,13 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" -#define LR 0.1 -#define EPOCHS 10000 -#define BETA 0.99 +#define TRANSLATION_LR 0.5 +#define ROTATION_LR 0.001 +#define LR 0.01 +#define EPOCHS 500 +#define BETA_1 0.90 +#define BETA_2 0.990 +#define EPSILON 1e-9 namespace fs = std::filesystem; @@ -289,19 +293,20 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT LOG(INFO) << "robot_to_feild_before\n" << robot_to_feild.ToEigen(); // Noise - robot_to_feild.t_x += 0.1; + robot_to_feild.t_x += 0.2; robot_to_feild.t_y += 0.1; robot_to_feild.t_z += 0.1; // robot_to_feild.r_x += 0.05; // robot_to_feild.r_y += 0.05; - robot_to_feild.r_z += 0.05; + // robot_to_feild.r_z += 0.05; tape_type tape; double loss = 0; - utils::Timer solve_timer("solve", false); + utils::Timer solve_timer("solve", true); JointSolver::transform3d_derrivative_t velocity; + JointSolver::transform3d_derrivative_t squared_gradiants; for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; JointSolver::transform3d_derrivative_t derrivative; @@ -333,11 +338,20 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT } } - velocity = (velocity * BETA) + derrivative; - robot_to_feild.Update(velocity, LR); + velocity = (velocity * BETA_1) + (derrivative * (1)); + squared_gradiants = (squared_gradiants * BETA_2) + + (derrivative * derrivative * (1 - BETA_2)); + auto update = velocity / ((squared_gradiants + EPSILON).sqrt()); + robot_to_feild.Update(update, LR * TRANSLATION_LR, LR * ROTATION_LR); if (epoch == 0) { LOG(INFO) << loss; } + LOG(INFO) << epoch << " " << loss; + LOG(INFO) << "tx_d " << derrivative.t_x << " ty_d " << derrivative.t_y; + // LOG(INFO) << "tx_vel " << velocity.t_x << " ty_vel " << velocity.t_y; + LOG(INFO) << "tx_update " << update.t_x << " ty_update" << update.t_y; + LOG(INFO) << "---------------------------------------"; + // LOG(INFO) << "\n" << derrivative; } robot_to_feild.CalculateMatrix(); From fb38f6ed7321998d045de4e98f06a9f5b0d16e34 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 4 May 2026 00:20:53 +0000 Subject: [PATCH 28/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.h | 89 ++++++++++++++++++------------ src/test/unit_test/forward_test.cc | 74 +++++++++---------------- 2 files changed, 80 insertions(+), 83 deletions(-) diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index d25b3d76..cb878668 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -19,8 +19,8 @@ class JointSolver { using AD = mode::active_type; using transform3d_derrivative_t = struct Transfrom3dDerrivative { - double t_x = 0; - double t_y = 0; + double scaler = 0; + double theta = 0; double t_z = 0; double r_x = 0; double r_y = 0; @@ -28,8 +28,8 @@ class JointSolver { auto operator+(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x + other.t_x, - .t_y = t_y + other.t_y, + return Transfrom3dDerrivative{.scaler = scaler + other.scaler, + .theta = theta + other.theta, .t_z = t_z + other.t_z, .r_x = r_x + other.r_x, .r_y = r_y + other.r_y, @@ -38,8 +38,8 @@ class JointSolver { auto operator-(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x - other.t_x, - .t_y = t_y - other.t_y, + return Transfrom3dDerrivative{.scaler = scaler - other.scaler, + .theta = theta - other.theta, .t_z = t_z - other.t_z, .r_x = r_x - other.r_x, .r_y = r_y - other.r_y, @@ -47,8 +47,8 @@ class JointSolver { } auto operator*(const double other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x * other, - .t_y = t_y * other, + return Transfrom3dDerrivative{.scaler = scaler * other, + .theta = theta * other, .t_z = t_z * other, .r_x = r_x * other, .r_y = r_y * other, @@ -56,8 +56,8 @@ class JointSolver { } auto operator+(const double other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x + other, - .t_y = t_y + other, + return Transfrom3dDerrivative{.scaler = scaler + other, + .theta = theta + other, .t_z = t_z + other, .r_x = r_x + other, .r_y = r_y + other, @@ -66,8 +66,8 @@ class JointSolver { auto operator*(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x * other.t_x, - .t_y = t_y * other.t_y, + return Transfrom3dDerrivative{.scaler = scaler * other.scaler, + .theta = theta * other.theta, .t_z = t_z * other.t_z, .r_x = r_x * other.r_x, .r_y = r_y * other.r_y, @@ -76,8 +76,8 @@ class JointSolver { auto operator/(const Transfrom3dDerrivative other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = t_x / other.t_x, - .t_y = t_y / other.t_y, + return Transfrom3dDerrivative{.scaler = scaler / other.scaler, + .theta = theta / other.theta, .t_z = t_z / other.t_z, .r_x = r_x / other.r_x, .r_y = r_y / other.r_y, @@ -85,8 +85,8 @@ class JointSolver { } auto sqrt() -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.t_x = std::sqrt(t_x), - .t_y = std::sqrt(t_y), + return Transfrom3dDerrivative{.scaler = std::sqrt(scaler), + .theta = std::sqrt(theta), .t_z = std::sqrt(t_z), .r_x = std::sqrt(r_x), .r_y = std::sqrt(r_y), @@ -97,8 +97,9 @@ class JointSolver { struct DifferntiableTransform3d { // Translation in meters, rotation in radians - AD t_x; - AD t_y; + AD scaler; + AD theta; + AD t_z; AD r_x; AD r_y; @@ -106,23 +107,29 @@ class JointSolver { std::array, 4> matrix; DifferntiableTransform3d(frc::Pose3d pose) - : t_x(pose.Translation().X().value()), - t_y(pose.Translation().Y().value()), + : scaler(std::hypot(pose.Translation().X().value(), + pose.Translation().Y().value())), + theta(std::atan2(pose.Translation().Y().value(), + pose.Translation().X().value())), t_z(pose.Translation().Z().value()), r_x(pose.Rotation().X().value()), r_y(pose.Rotation().Y().value()), r_z(pose.Rotation().Z().value()) {} DifferntiableTransform3d(frc::Transform3d pose) - : t_x(pose.Translation().X().value()), - t_y(pose.Translation().Y().value()), + : scaler(std::hypot(pose.Translation().X().value(), + pose.Translation().Y().value())), + theta(std::atan2(pose.Translation().Y().value(), + pose.Translation().X().value())), t_z(pose.Translation().Z().value()), r_x(pose.Rotation().X().value()), r_y(pose.Rotation().Y().value()), r_z(pose.Rotation().Z().value()) {} DifferntiableTransform3d(Eigen::Matrix4d matrix) - : t_x(matrix(0, 3)), t_y(matrix(1, 3)), t_z(matrix(2, 3)) { + : scaler(std::hypot(matrix(0, 3), matrix(1, 3))), + theta(std::atan2(matrix(1, 3), matrix(0, 3))), + t_z(matrix(2, 3)) { Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); r_x = euler(2); @@ -132,13 +139,13 @@ class JointSolver { void Update(transform3d_derrivative_t derrivative, double lr_translation, double lr_rotation) { - t_x -= derrivative.t_x * lr_translation; - t_y -= derrivative.t_y * lr_translation; - t_z -= derrivative.t_z * lr_translation; + scaler -= derrivative.scaler * lr_translation; + theta -= derrivative.theta * lr_translation; + // t_z -= derrivative.t_z * lr_translation; // r_x -= derrivative.r_x * lr; // r_y -= derrivative.r_y * lr; - // r_z -= derrivative.r_z * lr_rotation; + r_z -= derrivative.r_z * lr_rotation; } void RegisterInputs(tape_type& tape) { @@ -146,8 +153,8 @@ class JointSolver { tape.registerInput(r_y); tape.registerInput(r_z); - tape.registerInput(t_x); - tape.registerInput(t_y); + tape.registerInput(scaler); + tape.registerInput(theta); tape.registerInput(t_z); } @@ -192,8 +199,8 @@ class JointSolver { matrix[2][2] = cos_y * cos_x; // Translation - matrix[0][3] = t_x; - matrix[1][3] = t_y; + matrix[0][3] = std::cos(theta) * scaler; + matrix[1][3] = std::sin(theta) * scaler; matrix[2][3] = t_z; matrix[3][0] = 0; @@ -212,8 +219,8 @@ class JointSolver { } tape.computeAdjoints(); transform3d_derrivative_t derrivative{ - .t_x = xad::derivative(t_x), - .t_y = xad::derivative(t_y), + .scaler = xad::derivative(scaler), + .theta = xad::derivative(theta), .t_z = xad::derivative(t_z), .r_x = xad::derivative(r_x), .r_y = xad::derivative(r_y), @@ -273,13 +280,25 @@ class JointSolver { inline auto operator<<(std::ostream& os, const JointSolver::Transfrom3dDerrivative& d) -> std::ostream& { - os << "t_x=" << d.t_x << "\n" - << "t_y=" << d.t_y << "\n" + os << "scaler=" << d.scaler << "\n" + << "theta=" << d.theta << "\n" << "t_z=" << d.t_z << "\n" << "r_x=" << d.r_x << "\n" << "r_y=" << d.r_y << "\n" << "r_z=" << d.r_z << ""; return os; } + +inline auto operator<<(std::ostream& os, + const JointSolver::DifferntiableTransform3d& t) + -> std::ostream& { + os << "scaler=" << t.scaler << "\n" + << "theta=" << t.theta << "\n" + << "t_z=" << t.t_z << "\n" + << "r_x=" << t.r_x << "\n" + << "r_y=" << t.r_y << "\n" + << "r_z=" << t.r_z << ""; + return os; +} } // namespace localization // diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 35b2ceab..a0374e82 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -13,11 +13,11 @@ #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" -#define TRANSLATION_LR 0.5 -#define ROTATION_LR 0.001 -#define LR 0.01 -#define EPOCHS 500 -#define BETA_1 0.90 +#define TRANSLATION_LR 1 +#define ROTATION_LR 0.01 +#define LR 0.1 +#define EPOCHS 1000 +#define BETA_1 0.99 #define BETA_2 0.990 #define EPSILON 1e-9 @@ -232,9 +232,10 @@ TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT square_solver_solution.pose.ToMatrix()); const double tolerance = 1e-7; - ASSERT_NEAR(transform_from_pose.t_x.value(), transform_from_eigen.t_x.value(), - tolerance); - ASSERT_NEAR(transform_from_pose.t_y.value(), transform_from_eigen.t_y.value(), + // clang-format off + ASSERT_NEAR(transform_from_pose.scaler.value(), transform_from_eigen.scaler.value(), + tolerance); + ASSERT_NEAR(transform_from_pose.theta.value(), transform_from_eigen.theta.value(), tolerance); ASSERT_NEAR(transform_from_pose.t_z.value(), transform_from_eigen.t_z.value(), tolerance); @@ -245,6 +246,7 @@ TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT tolerance); ASSERT_NEAR(transform_from_pose.r_z.value(), transform_from_eigen.r_z.value(), tolerance); + // clang-format on } TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT @@ -273,6 +275,7 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT cv::Mat image = cv::imread("/bos-logs/log181/right/6.767740.jpg"); + // cv::Mat image = cv::imread("/bos-logs/log181/right/17.047751.jpg"); timestamped_frame_t timestamped_frame{ .frame = std::move(image), .timestamp = 0, .invalid = false}; auto detections = detector_right_->GetTagDetections(timestamped_frame); @@ -286,27 +289,25 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT auto feild_to_robot = square_solver_solution.pose.ToMatrix(); - JointSolver::DifferntiableTransform3d robot_to_feild( - feild_to_robot.inverse()); + const auto robot_to_feild = feild_to_robot.inverse(); + JointSolver::DifferntiableTransform3d T(frc::Pose3d{}); - robot_to_feild.CalculateMatrix(); - LOG(INFO) << "robot_to_feild_before\n" << robot_to_feild.ToEigen(); + T.CalculateMatrix(); + LOG(INFO) << "robot_to_feild_before\n" << robot_to_feild; // Noise - robot_to_feild.t_x += 0.2; - robot_to_feild.t_y += 0.1; - robot_to_feild.t_z += 0.1; + // robot_to_feild.scaler += 0.2; + // robot_to_feild.theta += 0.2; + // robot_to_feild.t_z += 0.1; // robot_to_feild.r_x += 0.05; // robot_to_feild.r_y += 0.05; - // robot_to_feild.r_z += 0.05; + // robot_to_feild.r_z += 0.1; tape_type tape; double loss = 0; utils::Timer solve_timer("solve", true); - JointSolver::transform3d_derrivative_t velocity; - JointSolver::transform3d_derrivative_t squared_gradiants; for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; JointSolver::transform3d_derrivative_t derrivative; @@ -315,51 +316,28 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); for (int corner_index = 0; corner_index < 4; corner_index++) { - robot_to_feild.CalculateMatrix(); - robot_to_feild.RegisterInputs(tape); - const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); + T.CalculateMatrix(); + T.RegisterInputs(tape); auto image_point = JointSolver::NormalizePoint( detection.corners[corner_index], camera_constant_right_); auto robot_to_feild_d = JointSolver::CalculateDerivative( - robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, + robot_to_feild, feild_to_tag, camera_to_robot_right_, normalized_camera_matrix_right_, image_point, corner_index); - robot_to_feild.RegisterOutputs(tape); - derrivative = - derrivative + robot_to_feild.BackPropagate(robot_to_feild_d, tape); + T.RegisterOutputs(tape); + derrivative = derrivative + T.BackPropagate(robot_to_feild_d, tape); loss += JointSolver::CalculateLoss( - robot_to_feild_eigen, feild_to_tag, camera_to_robot_right_, + robot_to_feild, feild_to_tag, camera_to_robot_right_, normalized_camera_matrix_right_, image_point, corner_index); tape.newRecording(); } } - - velocity = (velocity * BETA_1) + (derrivative * (1)); - squared_gradiants = (squared_gradiants * BETA_2) + - (derrivative * derrivative * (1 - BETA_2)); - auto update = velocity / ((squared_gradiants + EPSILON).sqrt()); - robot_to_feild.Update(update, LR * TRANSLATION_LR, LR * ROTATION_LR); - if (epoch == 0) { - LOG(INFO) << loss; - } - LOG(INFO) << epoch << " " << loss; - LOG(INFO) << "tx_d " << derrivative.t_x << " ty_d " << derrivative.t_y; - // LOG(INFO) << "tx_vel " << velocity.t_x << " ty_vel " << velocity.t_y; - LOG(INFO) << "tx_update " << update.t_x << " ty_update" << update.t_y; - LOG(INFO) << "---------------------------------------"; - // LOG(INFO) << "\n" << derrivative; + LOG(INFO) << "loss " << loss; } - - robot_to_feild.CalculateMatrix(); - LOG(INFO) << "robot_to_feild_after\n" << robot_to_feild.ToEigen(); - LOG(INFO) << loss; - - ASSERT_LT(solve_timer.Stop(), 0.1); - ASSERT_LT(loss, 0.01); } // TEST_F(ForwardTest, TestMultiCameraBackpropagation) { // NOLINT From 7b8fc00ff0ef2cc780a75fc30b826a1f378dc4e7 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 4 May 2026 00:43:50 +0000 Subject: [PATCH 29/47] wip Signed-off-by: Charlie Huang --- src/localization/CMakeLists.txt | 2 +- src/localization/joint_solver.cc | 2 +- src/localization/joint_solver.h | 116 ++++++++++++------------ src/localization/levenberg_marquardt.cc | 48 ++++++++++ src/localization/levenberg_marquardt.h | 18 ++++ src/test/unit_test/CMakeLists.txt | 3 + src/test/unit_test/forward_test.cc | 8 +- src/test/unit_test/joint_solve_test.cc | 43 +++++++++ 8 files changed, 174 insertions(+), 66 deletions(-) create mode 100644 src/localization/levenberg_marquardt.cc create mode 100644 src/localization/levenberg_marquardt.h create mode 100644 src/test/unit_test/joint_solve_test.cc diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index cba41a32..2ee11092 100644 --- a/src/localization/CMakeLists.txt +++ b/src/localization/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.10) -add_library(localization 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 unambiguous_estimator.cc simulation_sender.cc networktable_sender.cc) +add_library(localization 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 unambiguous_estimator.cc simulation_sender.cc networktable_sender.cc levenberg_marquardt.cc) target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi absl::status XAD::xad) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 6e2484bf..f5f5b851 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -170,7 +170,7 @@ auto JointSolver::EstimatePosition( average_timestamp /= total_detections; tape_.clearAll(); - DifferntiableTransform3d robot_to_feild( + DifferentiableTransform3d robot_to_feild( frc::Pose3d(initial_pose.value_or(position_receiver_.Get())) .ToMatrix() .inverse()); diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index cb878668..104f0ab0 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -18,7 +18,7 @@ class JointSolver { using tape_type = mode::tape_type; using AD = mode::active_type; - using transform3d_derrivative_t = struct Transfrom3dDerrivative { + using transform3d_derrivative_t = struct Transfrom3dDerivative { double scaler = 0; double theta = 0; double t_z = 0; @@ -26,75 +26,71 @@ class JointSolver { double r_y = 0; double r_z = 0; - auto operator+(const Transfrom3dDerrivative other) - -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = scaler + other.scaler, - .theta = theta + other.theta, - .t_z = t_z + other.t_z, - .r_x = r_x + other.r_x, - .r_y = r_y + other.r_y, - .r_z = r_z + other.r_z}; + auto operator+(const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler + other.scaler, + .theta = theta + other.theta, + .t_z = t_z + other.t_z, + .r_x = r_x + other.r_x, + .r_y = r_y + other.r_y, + .r_z = r_z + other.r_z}; } - auto operator-(const Transfrom3dDerrivative other) - -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = scaler - other.scaler, - .theta = theta - other.theta, - .t_z = t_z - other.t_z, - .r_x = r_x - other.r_x, - .r_y = r_y - other.r_y, - .r_z = r_z - other.r_z}; + auto operator-(const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler - other.scaler, + .theta = theta - other.theta, + .t_z = t_z - other.t_z, + .r_x = r_x - other.r_x, + .r_y = r_y - other.r_y, + .r_z = r_z - other.r_z}; } - auto operator*(const double other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = scaler * other, - .theta = theta * other, - .t_z = t_z * other, - .r_x = r_x * other, - .r_y = r_y * other, - .r_z = r_z * other}; + auto operator*(const double other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler * other, + .theta = theta * other, + .t_z = t_z * other, + .r_x = r_x * other, + .r_y = r_y * other, + .r_z = r_z * other}; } - auto operator+(const double other) -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = scaler + other, - .theta = theta + other, - .t_z = t_z + other, - .r_x = r_x + other, - .r_y = r_y + other, - .r_z = r_z + other}; + auto operator+(const double other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler + other, + .theta = theta + other, + .t_z = t_z + other, + .r_x = r_x + other, + .r_y = r_y + other, + .r_z = r_z + other}; } - auto operator*(const Transfrom3dDerrivative other) - -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = scaler * other.scaler, - .theta = theta * other.theta, - .t_z = t_z * other.t_z, - .r_x = r_x * other.r_x, - .r_y = r_y * other.r_y, - .r_z = r_z * other.r_z}; + auto operator*(const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler * other.scaler, + .theta = theta * other.theta, + .t_z = t_z * other.t_z, + .r_x = r_x * other.r_x, + .r_y = r_y * other.r_y, + .r_z = r_z * other.r_z}; } - auto operator/(const Transfrom3dDerrivative other) - -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = scaler / other.scaler, - .theta = theta / other.theta, - .t_z = t_z / other.t_z, - .r_x = r_x / other.r_x, - .r_y = r_y / other.r_y, - .r_z = r_z / other.r_z}; + auto operator/(const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler / other.scaler, + .theta = theta / other.theta, + .t_z = t_z / other.t_z, + .r_x = r_x / other.r_x, + .r_y = r_y / other.r_y, + .r_z = r_z / other.r_z}; } - auto sqrt() -> Transfrom3dDerrivative { - return Transfrom3dDerrivative{.scaler = std::sqrt(scaler), - .theta = std::sqrt(theta), - .t_z = std::sqrt(t_z), - .r_x = std::sqrt(r_x), - .r_y = std::sqrt(r_y), - .r_z = std::sqrt(r_z)}; + auto sqrt() -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = std::sqrt(scaler), + .theta = std::sqrt(theta), + .t_z = std::sqrt(t_z), + .r_x = std::sqrt(r_x), + .r_y = std::sqrt(r_y), + .r_z = std::sqrt(r_z)}; } }; - struct DifferntiableTransform3d { + struct DifferentiableTransform3d { // Translation in meters, rotation in radians AD scaler; @@ -106,7 +102,7 @@ class JointSolver { AD r_z; std::array, 4> matrix; - DifferntiableTransform3d(frc::Pose3d pose) + DifferentiableTransform3d(frc::Pose3d pose) : scaler(std::hypot(pose.Translation().X().value(), pose.Translation().Y().value())), theta(std::atan2(pose.Translation().Y().value(), @@ -116,7 +112,7 @@ class JointSolver { r_y(pose.Rotation().Y().value()), r_z(pose.Rotation().Z().value()) {} - DifferntiableTransform3d(frc::Transform3d pose) + DifferentiableTransform3d(frc::Transform3d pose) : scaler(std::hypot(pose.Translation().X().value(), pose.Translation().Y().value())), theta(std::atan2(pose.Translation().Y().value(), @@ -126,7 +122,7 @@ class JointSolver { r_y(pose.Rotation().Y().value()), r_z(pose.Rotation().Z().value()) {} - DifferntiableTransform3d(Eigen::Matrix4d matrix) + DifferentiableTransform3d(Eigen::Matrix4d matrix) : scaler(std::hypot(matrix(0, 3), matrix(1, 3))), theta(std::atan2(matrix(1, 3), matrix(0, 3))), t_z(matrix(2, 3)) { @@ -278,7 +274,7 @@ class JointSolver { }; inline auto operator<<(std::ostream& os, - const JointSolver::Transfrom3dDerrivative& d) + const JointSolver::Transfrom3dDerivative& d) -> std::ostream& { os << "scaler=" << d.scaler << "\n" << "theta=" << d.theta << "\n" @@ -290,7 +286,7 @@ inline auto operator<<(std::ostream& os, } inline auto operator<<(std::ostream& os, - const JointSolver::DifferntiableTransform3d& t) + const JointSolver::DifferentiableTransform3d& t) -> std::ostream& { os << "scaler=" << t.scaler << "\n" << "theta=" << t.theta << "\n" diff --git a/src/localization/levenberg_marquardt.cc b/src/localization/levenberg_marquardt.cc new file mode 100644 index 00000000..17edd11d --- /dev/null +++ b/src/localization/levenberg_marquardt.cc @@ -0,0 +1,48 @@ +#include "levenberg_marquardt.h" + +// AI +auto levenberg_marquardt( + Eigen::VectorXd& params, + const std::function& + compute_residual, + const std::function& + compute_jacobian, + double& lambda, double tolerance) -> bool { + const Eigen::VectorXd residual = compute_residual(params); + if (!residual.allFinite()) { + return false; + } + + const double old_loss = residual.squaredNorm(); + + const Eigen::MatrixXd jacobian = compute_jacobian(params); + if (!jacobian.allFinite()) {} + + const Eigen::MatrixXd JTJ = jacobian.transpose() * jacobian; + return false; + Eigen::MatrixXd A = JTJ; + A.diagonal().array() += lambda; + + const Eigen::VectorXd g = jacobian.transpose() * residual; + + Eigen::VectorXd delta = A.ldlt().solve(g); + if (!delta.allFinite()) { + return false; + } + + const Eigen::VectorXd trial_params = params + delta; + const Eigen::VectorXd trial_residual = compute_residual(trial_params); + if (!trial_residual.allFinite()) { + return false; + } + const double new_loss = trial_residual.squaredNorm(); + + if (new_loss <= old_loss) { + params = trial_params; + lambda *= 0.5; + return delta.norm() < tolerance; + } + + lambda *= 2.0; + return false; +} diff --git a/src/localization/levenberg_marquardt.h b/src/localization/levenberg_marquardt.h new file mode 100644 index 00000000..75a83ca7 --- /dev/null +++ b/src/localization/levenberg_marquardt.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +// Performs a single damped Gauss-Newton step using user-provided callbacks to +// compute the residual vector and Jacobian at arbitrary parameter values. The +// trial step is accepted (and lambda halved) when the recomputed residual norm +// does not increase; otherwise lambda is doubled and the parameters remain +// unchanged. Returns true when an accepted step falls below the provided +// tolerance. +auto levenberg_marquardt( + Eigen::VectorXd& params, + const std::function& + compute_residual, + const std::function& + compute_jacobian, + double& lambda, double tolerance = 1e-8) -> bool; diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index 5dead05d..2f583bac 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -15,6 +15,9 @@ target_link_libraries(matrix PRIVATE localization utils GTest::gtest_main) add_executable(forward_test forward_test.cc) target_link_libraries(forward_test PRIVATE localization utils GTest::gtest_main XAD::xad) +add_executable(joint_solve_test joint_solve_test.cc) +target_link_libraries(joint_solve_test PRIVATE localization utils GTest::gtest_main XAD::xad) + include(GoogleTest) gtest_discover_tests(multi_tag_test) gtest_discover_tests(square_solve_test) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index a0374e82..9f014009 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -226,9 +226,9 @@ TEST_F(ForwardTest, TestTransfrom3dConstructor) { // NOLINT auto square_solver_solution = square_solver_right_->EstimatePosition({detection})[0]; - JointSolver::DifferntiableTransform3d transform_from_pose( + JointSolver::DifferentiableTransform3d transform_from_pose( square_solver_solution.pose); - JointSolver::DifferntiableTransform3d transform_from_eigen( + JointSolver::DifferentiableTransform3d transform_from_eigen( square_solver_solution.pose.ToMatrix()); const double tolerance = 1e-7; @@ -259,7 +259,7 @@ TEST_F(ForwardTest, TestTransfrom3d) { // NOLINT auto square_solver_solution = square_solver_right_->EstimatePosition({detection})[0]; - JointSolver::DifferntiableTransform3d transform(square_solver_solution.pose); + JointSolver::DifferentiableTransform3d transform(square_solver_solution.pose); transform.CalculateMatrix(); EXPECT_TRUE(square_solver_solution.pose.ToMatrix().isApprox( @@ -290,7 +290,7 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT auto feild_to_robot = square_solver_solution.pose.ToMatrix(); const auto robot_to_feild = feild_to_robot.inverse(); - JointSolver::DifferntiableTransform3d T(frc::Pose3d{}); + JointSolver::DifferentiableTransform3d T(frc::Pose3d{}); T.CalculateMatrix(); LOG(INFO) << "robot_to_feild_before\n" << robot_to_feild; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc new file mode 100644 index 00000000..c8544b2a --- /dev/null +++ b/src/test/unit_test/joint_solve_test.cc @@ -0,0 +1,43 @@ +#include +#include +#include +#include +#include "src/localization/joint_solver.h" +#include "src/localization/opencv_apriltag_detector.h" +#include "src/localization/square_solver.h" +#include "src/utils/camera_utils.h" +#include "src/utils/constants_from_json.h" +#include "src/utils/timer.h" +#include "src/utils/transform.h" + +namespace fs = std::filesystem; + +using camera::camera_constant_t; +using camera::GetCameraConstants; +using camera::timestamped_frame_t; +using localization::JointSolver; +using localization::kapriltag_layout; +using localization::OpenCVAprilTagDetector; +using localization::position_estimate_t; +using localization::SquareSolver; +using localization::tag_detection_t; +using utils::CameraMatrixFromJson; +using utils::ReadIntrinsics; + +TEST(JointSolveTest, TestJointSolve) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + auto camera_constant = GetCameraConstants().at("second_bot_right"); + auto square_solver = std::make_unique(camera_constant); + auto joint_solver = + std::make_unique(std::vector{camera_constant}); + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + ReadIntrinsics(camera_constant.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; +} From 39030e1fbe3a1b87b9b79339f4022e5b279cd629 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 4 May 2026 03:51:39 +0000 Subject: [PATCH 30/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 357 ++++++++++++++++++++----- src/localization/joint_solver.h | 257 ++++-------------- src/test/unit_test/CMakeLists.txt | 4 +- src/test/unit_test/forward_test.cc | 36 +-- src/test/unit_test/joint_solve_test.cc | 7 + 5 files changed, 357 insertions(+), 304 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index f5f5b851..da4d8ad2 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -1,6 +1,7 @@ #include "src/localization/joint_solver.h" #include #include +#include "src/localization/position_solver.h" #include "src/utils/camera_utils.h" #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" @@ -34,6 +35,214 @@ const Eigen::Matrix rotate_yaw = 0, 0, 0, 1).finished(); // clang-format on +auto JointSolver::Transfrom3dDerivative::operator+( + const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler + other.scaler, + .theta = theta + other.theta, + .t_z = t_z + other.t_z, + .r_x = r_x + other.r_x, + .r_y = r_y + other.r_y, + .r_z = r_z + other.r_z}; +} + +auto JointSolver::Transfrom3dDerivative::operator-( + const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler - other.scaler, + .theta = theta - other.theta, + .t_z = t_z - other.t_z, + .r_x = r_x - other.r_x, + .r_y = r_y - other.r_y, + .r_z = r_z - other.r_z}; +} + +auto JointSolver::Transfrom3dDerivative::operator*(const double other) + -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler * other, + .theta = theta * other, + .t_z = t_z * other, + .r_x = r_x * other, + .r_y = r_y * other, + .r_z = r_z * other}; +} + +auto JointSolver::Transfrom3dDerivative::operator+(const double other) + -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler + other, + .theta = theta + other, + .t_z = t_z + other, + .r_x = r_x + other, + .r_y = r_y + other, + .r_z = r_z + other}; +} + +auto JointSolver::Transfrom3dDerivative::operator*( + const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler * other.scaler, + .theta = theta * other.theta, + .t_z = t_z * other.t_z, + .r_x = r_x * other.r_x, + .r_y = r_y * other.r_y, + .r_z = r_z * other.r_z}; +} + +auto JointSolver::Transfrom3dDerivative::operator/( + const Transfrom3dDerivative other) -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = scaler / other.scaler, + .theta = theta / other.theta, + .t_z = t_z / other.t_z, + .r_x = r_x / other.r_x, + .r_y = r_y / other.r_y, + .r_z = r_z / other.r_z}; +} + +auto JointSolver::Transfrom3dDerivative::sqrt() -> Transfrom3dDerivative { + return Transfrom3dDerivative{.scaler = std::sqrt(scaler), + .theta = std::sqrt(theta), + .t_z = std::sqrt(t_z), + .r_x = std::sqrt(r_x), + .r_y = std::sqrt(r_y), + .r_z = std::sqrt(r_z)}; +} + +JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( + const Eigen::VectorXd& params) + : scaler(params(0)), + theta(params(1)), + t_z(params(2)), + r_x(params(3)), + r_y(params(4)), + r_z(params(5)) {} + +JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( + frc::Pose3d pose) + : scaler(std::hypot(pose.Translation().X().value(), + pose.Translation().Y().value())), + theta(std::atan2(pose.Translation().Y().value(), + pose.Translation().X().value())), + t_z(pose.Translation().Z().value()), + r_x(pose.Rotation().X().value()), + r_y(pose.Rotation().Y().value()), + r_z(pose.Rotation().Z().value()) {} + +JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( + frc::Transform3d pose) + : scaler(std::hypot(pose.Translation().X().value(), + pose.Translation().Y().value())), + theta(std::atan2(pose.Translation().Y().value(), + pose.Translation().X().value())), + t_z(pose.Translation().Z().value()), + r_x(pose.Rotation().X().value()), + r_y(pose.Rotation().Y().value()), + r_z(pose.Rotation().Z().value()) {} + +JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( + Eigen::Matrix4d matrix) + : scaler(std::hypot(matrix(0, 3), matrix(1, 3))), + theta(std::atan2(matrix(1, 3), matrix(0, 3))), + t_z(matrix(2, 3)) { + Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); + Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); + r_x = euler(2); + r_z = euler(0); + r_y = euler(1); +} + +void JointSolver::DifferentiableTransform3d::Update( + transform3d_derivative_t derrivative, double lr_translation, + double lr_rotation) { + scaler -= derrivative.scaler * lr_translation; + theta -= derrivative.theta * lr_translation; + // t_z -= derrivative.t_z * lr_translation; + + // r_x -= derrivative.r_x * lr; + // r_y -= derrivative.r_y * lr; + r_z -= derrivative.r_z * lr_rotation; +} + +void JointSolver::DifferentiableTransform3d::RegisterInputs(tape_type& tape) { + tape.registerInput(r_x); + tape.registerInput(r_y); + tape.registerInput(r_z); + + tape.registerInput(scaler); + tape.registerInput(theta); + tape.registerInput(t_z); +} + +void JointSolver::DifferentiableTransform3d::RegisterOutputs(tape_type& tape) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + tape.registerOutput(matrix[i][j]); + } + } +} + +auto JointSolver::DifferentiableTransform3d::ToEigen() + -> Eigen::Matrix4d const { + // clang-format off + return (Eigen::Matrix() << + matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), + matrix[1][0].value(), matrix[1][1].value(), matrix[1][2].value(), matrix[1][3].value(), + matrix[2][0].value(), matrix[2][1].value(), matrix[2][2].value(), matrix[2][3].value(), + matrix[3][0].value(), matrix[3][1].value(), matrix[3][2].value(), matrix[3][3].value()) + .finished(); + // clang-format on +} + +void JointSolver::DifferentiableTransform3d::CalculateMatrix() { + AD cos_x = xad::cos(r_x); + AD cos_y = xad::cos(r_y); + AD cos_z = xad::cos(r_z); + + AD sin_x = xad::sin(r_x); + AD sin_y = xad::sin(r_y); + AD sin_z = xad::sin(r_z); + + // Rotation + matrix[0][0] = cos_z * cos_y; + matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; + matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; + + matrix[1][0] = sin_z * cos_y; + matrix[1][1] = sin_z * sin_y * sin_x + cos_z * cos_x; + matrix[1][2] = sin_z * sin_y * cos_x - cos_z * sin_x; + + matrix[2][0] = -sin_y; + matrix[2][1] = cos_y * sin_x; + matrix[2][2] = cos_y * cos_x; + + // Translation + matrix[0][3] = std::cos(theta) * scaler; + matrix[1][3] = std::sin(theta) * scaler; + matrix[2][3] = t_z; + + matrix[3][0] = 0; + matrix[3][1] = 0; + matrix[3][2] = 0; + matrix[3][3] = 1; +} + +auto JointSolver::DifferentiableTransform3d::BackPropagate( + const Eigen::Matrix4d& next_derrivative, tape_type& tape) + -> transform3d_derivative_t { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + tape.registerOutput(matrix[i][j]); + derivative(matrix[i][j]) = next_derrivative(i, j); + } + } + tape.computeAdjoints(); + transform3d_derivative_t derrivative{ + .scaler = xad::derivative(scaler), + .theta = xad::derivative(theta), + .t_z = xad::derivative(t_z), + .r_x = xad::derivative(r_x), + .r_y = xad::derivative(r_y), + .r_z = xad::derivative(r_z), + }; + return derrivative; +} + auto JointSolver::CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, const Eigen::Matrix4d& feild_to_tag, const Eigen::Matrix4d& camera_to_robot, @@ -106,19 +315,22 @@ auto JointSolver::NormalizePoint( // clang-format on } -auto JointSolver::ProjectPoints(const frc::Pose3d& camera_pose, - const frc::Pose3d& tag_pose, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Matrix4d& camera_to_robot, - int corner_index) -> Eigen::Vector3d { - auto feild_to_robot = camera_pose.ToMatrix(); - auto feild_to_tag = tag_pose.ToMatrix(); +auto JointSolver::ProjectPoints( + const Eigen::Matrix4d feild_to_robot, const Eigen::Matrix4d feild_to_tag, + const Eigen::Matrix3d& camera_matrix, + const Eigen::Matrix4d& camera_to_robot, + const Eigen::Vector4d& homogenized_apriltag_corner) -> Eigen::Vector3d { auto camera_to_tag = camera_to_robot * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; + // LOG(INFO) << "feild_to_tag\n" << feild_to_tag; + // LOG(INFO) << "robot_to_feild\n" << feild_to_robot.inverse(); + LOG(INFO) << "camera_to_robot\n" << camera_to_robot; + LOG(INFO) << "test\n" << feild_to_robot.inverse() * feild_to_tag * rotate_yaw; + LOG(INFO) << "camera_to_tag\n" << camera_to_tag; + Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_tag * - localization::kapriltag_corners_eigen_homogenized[corner_index]; + camera_matrix * PI * camera_to_tag * homogenized_apriltag_corner; auto normalized_point = projected_point / projected_point[0]; return normalized_point; } @@ -127,10 +339,10 @@ auto JointSolver::NormalizeCameraMatrix( Eigen::Matrix3d camera_matrix, const camera::camera_constant_t& camera_constant) -> Eigen::Matrix3d { camera_matrix(1, 0) /= camera_constant.frame_width.value(); - camera_matrix(1, 1) /= camera_constant.frame_width.value(); camera_matrix(2, 0) /= camera_constant.frame_height.value(); camera_matrix(2, 2) /= camera_constant.frame_height.value(); + camera_matrix(1, 1) /= camera_constant.frame_width.value(); return camera_matrix; } @@ -146,11 +358,26 @@ JointSolver::JointSolver( ReadIntrinsics(camera_constants[i].intrinsics_path.value())), camera_constants[i])); - camera_to_robot_.emplace_back( + robot_to_camera_.emplace_back( ExtrinsicsJsonToCameraToRobot( ReadExtrinsics(camera_constants[i].extrinsics_path.value())) - .ToMatrix()); + .ToMatrix() + .inverse()); + } +} + +auto JointSolver::CalculateResidual(const Eigen::VectorXd& params) + -> Eigen::VectorXd { + for (const auto data_point : data_points_) { + // auto camera_pose = field_to_robot_ * data_point.robot_to_camera; + auto projected_point = ProjectPoints( + field_to_robot_, data_point.feild_to_tag, + data_point.normalized_camera_matrix, data_point.camera_to_robot, + data_point.homogenized_apriltag_corner); + LOG(INFO) << "projected_point\n" << projected_point; + LOG(INFO) << "image_point\n" << data_point.normalized_image_point; } + return {}; } auto JointSolver::EstimatePosition( @@ -160,78 +387,41 @@ auto JointSolver::EstimatePosition( std::vector tag_ids; double average_timestamp = 0; int total_detections = 0; - for (const auto& [_, tag_detections] : camera_detections) { + + data_points_.clear(); + + for (const auto& [camera_name, tag_detections] : camera_detections) { + int index = camera_name_to_index[camera_name]; for (const auto& tag_detection : tag_detections) { tag_ids.push_back(tag_detection.tag_id); average_timestamp += tag_detection.timestamp; total_detections++; - } - } - average_timestamp /= total_detections; - - tape_.clearAll(); - DifferentiableTransform3d robot_to_feild( - frc::Pose3d(initial_pose.value_or(position_receiver_.Get())) - .ToMatrix() - .inverse()); - - JointSolver::transform3d_derrivative_t velocity; - double loss = 0; - for (int epoch = 0; epoch < EPOCHS; epoch++) { - loss = 0; - JointSolver::transform3d_derrivative_t derrivative; - for (auto const& [camera_name, tag_detections] : camera_detections) { - int camera_index = camera_name_to_index[camera_name]; - const auto& camera_to_robot = camera_to_robot_[camera_index]; - const auto& camera_matrix = normalized_camera_matrix_[camera_index]; - - for (const auto& detection : tag_detections) { - auto feild_to_tag = - kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); - - for (int corner_index = 0; corner_index < 4; corner_index++) { - robot_to_feild.CalculateMatrix(); - robot_to_feild.RegisterInputs(tape_); - const auto robot_to_feild_eigen = robot_to_feild.ToEigen(); - - // clang-format off - auto image_point = (Eigen::Vector3d() << 1, - detection.corners[corner_index].x / camera_constant_[camera_index].frame_width.value(), - detection.corners[corner_index].y / camera_constant_[camera_index].frame_height.value()) - .finished(); - // clang-format on - - auto robot_to_feild_d = CalculateDerivative( - robot_to_feild_eigen, feild_to_tag, camera_to_robot, - camera_matrix, image_point, corner_index); - - robot_to_feild.RegisterOutputs(tape_); - derrivative = derrivative + - robot_to_feild.BackPropagate(robot_to_feild_d, tape_); - - loss += - CalculateLoss(robot_to_feild_eigen, feild_to_tag, camera_to_robot, - camera_matrix, image_point, corner_index); - - tape_.newRecording(); - } + int corner_index = 0; + for (const auto& corner : tag_detection.corners) { + data_points_.push_back(datapoint_t{ + .normalized_image_point = + NormalizePoint(corner, camera_constant_[index]), + .normalized_camera_matrix = normalized_camera_matrix_[index], + .camera_to_robot = robot_to_camera_[index].inverse(), + .feild_to_tag = + kapriltag_layout.GetTagPose(tag_detection.tag_id)->ToMatrix(), + .homogenized_apriltag_corner = localization:: + kapriltag_corners_eigen_homogenized[corner_index]}); + corner_index++; } } - velocity = (velocity * BETA) + derrivative; - robot_to_feild.Update(velocity, LR, LR); } - robot_to_feild.CalculateMatrix(); - Eigen::Matrix4d feild_to_robot = robot_to_feild.ToEigen().inverse(); - feild_to_robot(3, 0) = 0; - feild_to_robot(3, 1) = 0; - feild_to_robot(3, 2) = 0; - feild_to_robot(3, 3) = 1; + // frc::Pose3d(initial_pose.value_or(position_receiver_.Get())) + field_to_robot_ = initial_pose.value_or(position_receiver_.Get()).ToMatrix(); + for (size_t i = 0; i < data_points_.size(); i++) { + CalculateResidual({}); + } return position_estimate_t{ .tag_ids = tag_ids, .rejected_tag_ids = {}, - .pose = frc::Pose3d(feild_to_robot), + .pose = {}, .variance = 1, .timestamp = average_timestamp, .num_tags = total_detections, @@ -242,4 +432,27 @@ auto JointSolver::EstimatePosition( }; } +auto operator<<(std::ostream& os, const JointSolver::Transfrom3dDerivative& d) + -> std::ostream& { + os << "scaler=" << d.scaler << "\n" + << "theta=" << d.theta << "\n" + << "t_z=" << d.t_z << "\n" + << "r_x=" << d.r_x << "\n" + << "r_y=" << d.r_y << "\n" + << "r_z=" << d.r_z << ""; + return os; +} + +auto operator<<(std::ostream& os, + const JointSolver::DifferentiableTransform3d& t) + -> std::ostream& { + os << "scaler=" << t.scaler << "\n" + << "theta=" << t.theta << "\n" + << "t_z=" << t.t_z << "\n" + << "r_x=" << t.r_x << "\n" + << "r_y=" << t.r_y << "\n" + << "r_z=" << t.r_z << ""; + return os; +} + } // namespace localization diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 104f0ab0..731a9ca9 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -18,7 +18,15 @@ class JointSolver { using tape_type = mode::tape_type; using AD = mode::active_type; - using transform3d_derrivative_t = struct Transfrom3dDerivative { + using datapoint_t = struct DataPoint { + const Eigen::Vector3d& normalized_image_point; + const Eigen::Matrix3d& normalized_camera_matrix; + const Eigen::Matrix4d& camera_to_robot; + const Eigen::Matrix4d& feild_to_tag; + const Eigen::Vector4d& homogenized_apriltag_corner; + }; + + using transform3d_derivative_t = struct Transfrom3dDerivative { double scaler = 0; double theta = 0; double t_z = 0; @@ -26,73 +34,16 @@ class JointSolver { double r_y = 0; double r_z = 0; - auto operator+(const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler + other.scaler, - .theta = theta + other.theta, - .t_z = t_z + other.t_z, - .r_x = r_x + other.r_x, - .r_y = r_y + other.r_y, - .r_z = r_z + other.r_z}; - } - - auto operator-(const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler - other.scaler, - .theta = theta - other.theta, - .t_z = t_z - other.t_z, - .r_x = r_x - other.r_x, - .r_y = r_y - other.r_y, - .r_z = r_z - other.r_z}; - } - - auto operator*(const double other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler * other, - .theta = theta * other, - .t_z = t_z * other, - .r_x = r_x * other, - .r_y = r_y * other, - .r_z = r_z * other}; - } - - auto operator+(const double other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler + other, - .theta = theta + other, - .t_z = t_z + other, - .r_x = r_x + other, - .r_y = r_y + other, - .r_z = r_z + other}; - } - - auto operator*(const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler * other.scaler, - .theta = theta * other.theta, - .t_z = t_z * other.t_z, - .r_x = r_x * other.r_x, - .r_y = r_y * other.r_y, - .r_z = r_z * other.r_z}; - } - - auto operator/(const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler / other.scaler, - .theta = theta / other.theta, - .t_z = t_z / other.t_z, - .r_x = r_x / other.r_x, - .r_y = r_y / other.r_y, - .r_z = r_z / other.r_z}; - } - - auto sqrt() -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = std::sqrt(scaler), - .theta = std::sqrt(theta), - .t_z = std::sqrt(t_z), - .r_x = std::sqrt(r_x), - .r_y = std::sqrt(r_y), - .r_z = std::sqrt(r_z)}; - } + auto operator+(const Transfrom3dDerivative other) -> Transfrom3dDerivative; + auto operator-(const Transfrom3dDerivative other) -> Transfrom3dDerivative; + auto operator*(const double other) -> Transfrom3dDerivative; + auto operator+(const double other) -> Transfrom3dDerivative; + auto operator*(const Transfrom3dDerivative other) -> Transfrom3dDerivative; + auto operator/(const Transfrom3dDerivative other) -> Transfrom3dDerivative; + auto sqrt() -> Transfrom3dDerivative; }; struct DifferentiableTransform3d { - - // Translation in meters, rotation in radians AD scaler; AD theta; @@ -102,128 +53,19 @@ class JointSolver { AD r_z; std::array, 4> matrix; - DifferentiableTransform3d(frc::Pose3d pose) - : scaler(std::hypot(pose.Translation().X().value(), - pose.Translation().Y().value())), - theta(std::atan2(pose.Translation().Y().value(), - pose.Translation().X().value())), - t_z(pose.Translation().Z().value()), - r_x(pose.Rotation().X().value()), - r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} - - DifferentiableTransform3d(frc::Transform3d pose) - : scaler(std::hypot(pose.Translation().X().value(), - pose.Translation().Y().value())), - theta(std::atan2(pose.Translation().Y().value(), - pose.Translation().X().value())), - t_z(pose.Translation().Z().value()), - r_x(pose.Rotation().X().value()), - r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} - - DifferentiableTransform3d(Eigen::Matrix4d matrix) - : scaler(std::hypot(matrix(0, 3), matrix(1, 3))), - theta(std::atan2(matrix(1, 3), matrix(0, 3))), - t_z(matrix(2, 3)) { - Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); - Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); - r_x = euler(2); - r_y = euler(1); - r_z = euler(0); - } - - void Update(transform3d_derrivative_t derrivative, double lr_translation, - double lr_rotation) { - scaler -= derrivative.scaler * lr_translation; - theta -= derrivative.theta * lr_translation; - // t_z -= derrivative.t_z * lr_translation; - - // r_x -= derrivative.r_x * lr; - // r_y -= derrivative.r_y * lr; - r_z -= derrivative.r_z * lr_rotation; - } - - void RegisterInputs(tape_type& tape) { - tape.registerInput(r_x); - tape.registerInput(r_y); - tape.registerInput(r_z); - - tape.registerInput(scaler); - tape.registerInput(theta); - tape.registerInput(t_z); - } - - void RegisterOutputs(tape_type& tape) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); - } - } - } - - auto ToEigen() -> Eigen::Matrix4d const { - // clang-format off - return (Eigen::Matrix() << - matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), - matrix[1][0].value(), matrix[1][1].value(), matrix[1][2].value(), matrix[1][3].value(), - matrix[2][0].value(), matrix[2][1].value(), matrix[2][2].value(), matrix[2][3].value(), - matrix[3][0].value(), matrix[3][1].value(), matrix[3][2].value(), matrix[3][3].value()) - .finished(); - // clang-format on - } - void CalculateMatrix() { - AD cos_x = xad::cos(r_x); - AD cos_y = xad::cos(r_y); - AD cos_z = xad::cos(r_z); - - AD sin_x = xad::sin(r_x); - AD sin_y = xad::sin(r_y); - AD sin_z = xad::sin(r_z); - - // Rotation - matrix[0][0] = cos_z * cos_y; - matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; - matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; - - matrix[1][0] = sin_z * cos_y; - matrix[1][1] = sin_z * sin_y * sin_x + cos_z * cos_x; - matrix[1][2] = sin_z * sin_y * cos_x - cos_z * sin_x; - - matrix[2][0] = -sin_y; - matrix[2][1] = cos_y * sin_x; - matrix[2][2] = cos_y * cos_x; - - // Translation - matrix[0][3] = std::cos(theta) * scaler; - matrix[1][3] = std::sin(theta) * scaler; - matrix[2][3] = t_z; - - matrix[3][0] = 0; - matrix[3][1] = 0; - matrix[3][2] = 0; - matrix[3][3] = 1; - } - + DifferentiableTransform3d(frc::Pose3d pose); + DifferentiableTransform3d(frc::Transform3d pose); + DifferentiableTransform3d(Eigen::Matrix4d matrix); + DifferentiableTransform3d(const Eigen::VectorXd& params); + + void Update(transform3d_derivative_t derrivative, double lr_translation, + double lr_rotation); + void RegisterInputs(tape_type& tape); + void RegisterOutputs(tape_type& tape); + auto ToEigen() -> Eigen::Matrix4d const; + void CalculateMatrix(); auto BackPropagate(const Eigen::Matrix4d& next_derrivative, tape_type& tape) - -> transform3d_derrivative_t { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); - derivative(matrix[i][j]) = next_derrivative(i, j); - } - } - tape.computeAdjoints(); - transform3d_derrivative_t derrivative{ - .scaler = xad::derivative(scaler), - .theta = xad::derivative(theta), - .t_z = xad::derivative(t_z), - .r_x = xad::derivative(r_x), - .r_y = xad::derivative(r_y), - .r_z = xad::derivative(r_z), - }; - return derrivative; - } + -> transform3d_derivative_t; }; public: @@ -245,11 +87,12 @@ class JointSolver { const camera::camera_constant_t& camera_constant) -> Eigen::Vector3d; - auto static ProjectPoints(const frc::Pose3d& camera_pose, - const frc::Pose3d& tag_pose, + auto static ProjectPoints(const Eigen::Matrix4d feild_to_robot, + const Eigen::Matrix4d feild_to_tag, const Eigen::Matrix3d& camera_matrix, const Eigen::Matrix4d& camera_to_robot, - int corner_index) -> Eigen::Vector3d; + const Eigen::Vector4d& homogenized_apriltag_corner) + -> Eigen::Vector3d; auto static NormalizeCameraMatrix( Eigen::Matrix3d camera_matrix, @@ -264,37 +107,27 @@ class JointSolver { std::optional intial_pose = std::nullopt) -> position_estimate_t; + private: + auto CalculateResidual(const Eigen::VectorXd& candidate) -> Eigen::VectorXd; + private: std::unordered_map camera_name_to_index; std::vector normalized_camera_matrix_; - std::vector camera_to_robot_; + std::vector robot_to_camera_; std::vector camera_constant_; + + std::vector data_points_; + Eigen::Matrix4d field_to_robot_; + PositionReceiver position_receiver_; tape_type tape_; }; -inline auto operator<<(std::ostream& os, - const JointSolver::Transfrom3dDerivative& d) - -> std::ostream& { - os << "scaler=" << d.scaler << "\n" - << "theta=" << d.theta << "\n" - << "t_z=" << d.t_z << "\n" - << "r_x=" << d.r_x << "\n" - << "r_y=" << d.r_y << "\n" - << "r_z=" << d.r_z << ""; - return os; -} +auto operator<<(std::ostream& os, const JointSolver::Transfrom3dDerivative& d) + -> std::ostream&; -inline auto operator<<(std::ostream& os, - const JointSolver::DifferentiableTransform3d& t) - -> std::ostream& { - os << "scaler=" << t.scaler << "\n" - << "theta=" << t.theta << "\n" - << "t_z=" << t.t_z << "\n" - << "r_x=" << t.r_x << "\n" - << "r_y=" << t.r_y << "\n" - << "r_z=" << t.r_z << ""; - return os; -} +auto operator<<(std::ostream& os, + const JointSolver::DifferentiableTransform3d& t) + -> std::ostream&; } // namespace localization // diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index 2f583bac..3b88b4a1 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -12,8 +12,8 @@ target_link_libraries(general_solver_test PRIVATE localization utils GTest::gtes add_executable(matrix matrix.cc) target_link_libraries(matrix PRIVATE localization utils GTest::gtest_main) -add_executable(forward_test forward_test.cc) -target_link_libraries(forward_test PRIVATE localization utils GTest::gtest_main XAD::xad) +# add_executable(forward_test forward_test.cc) +# target_link_libraries(forward_test PRIVATE localization utils GTest::gtest_main XAD::xad) add_executable(joint_solve_test joint_solve_test.cc) target_link_libraries(joint_solve_test PRIVATE localization utils GTest::gtest_main XAD::xad) diff --git a/src/test/unit_test/forward_test.cc b/src/test/unit_test/forward_test.cc index 9f014009..b2569841 100644 --- a/src/test/unit_test/forward_test.cc +++ b/src/test/unit_test/forward_test.cc @@ -133,9 +133,9 @@ TEST_F(ForwardTest, TestForward) { // NOLINT // feild_to_robot * camera_to_robot.inv * camera_to_tag = feild_to_tag * 180_yaw // camera_to_robot.inv * camera_to_tag = feild_to_robot.inv * feild_to_tag * 180_yaw // camera_to_tag = camera_to_robot * feild_to_robot.inv * feild_to_tag * 180_yaw - std::vector image_paths; for (const auto& file : fs::directory_iterator(LOG_PATH)) { + image_paths.push_back(file.path()); } std::sort(image_paths.begin(), image_paths.end()); @@ -153,22 +153,22 @@ TEST_F(ForwardTest, TestForward) { // NOLINT auto square_solver_solution = square_solver_right_->EstimatePosition({detection})[0]; - for (int j = 0; j < 4; j++) { - auto projected_points = JointSolver::ProjectPoints( - square_solver_solution.pose, - kapriltag_layout.GetTagPose(detection.tag_id).value(), - camera_matrix_right_, camera_to_robot_right_, j); - - auto projected_points_normalized = JointSolver::ProjectPoints( - square_solver_solution.pose, - kapriltag_layout.GetTagPose(detection.tag_id).value(), - normalized_camera_matrix_right_, camera_to_robot_right_, j); - - CheckIsEqual(detection.corners[j], projected_points, 1); - CheckIsEqual(JointSolver::NormalizePoint(detection.corners[j], - camera_constant_right_), - projected_points_normalized, 0.001); - } + // for (int j = 0; j < 4; j++) { + // auto projected_points = JointSolver::ProjectPoints( + // square_solver_solution.pose, + // kapriltag_layout.GetTagPose(detection.tag_id).value(), + // camera_matrix_right_, camera_to_robot_right_, j); + // + // auto projected_points_normalized = JointSolver::ProjectPoints( + // square_solver_solution.pose, + // kapriltag_layout.GetTagPose(detection.tag_id).value(), + // normalized_camera_matrix_right_, camera_to_robot_right_, j); + // + // CheckIsEqual(detection.corners[j], projected_points, 1); + // CheckIsEqual(JointSolver::NormalizePoint(detection.corners[j], + // camera_constant_right_), + // projected_points_normalized, 0.001); + // } } } @@ -310,7 +310,7 @@ TEST_F(ForwardTest, TestMultiTagBackpropagation) { // NOLINT utils::Timer solve_timer("solve", true); for (int epoch = 0; epoch < EPOCHS; epoch++) { loss = 0; - JointSolver::transform3d_derrivative_t derrivative; + JointSolver::transform3d_derivative_t derrivative; for (const auto& detection : detections) { auto feild_to_tag = kapriltag_layout.GetTagPose(detection.tag_id).value().ToMatrix(); diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index c8544b2a..1ee02078 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -40,4 +40,11 @@ TEST(JointSolveTest, TestJointSolve) { // NOLINT auto detection = detections[0]; auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; + + const std::map> camera_detections{ + {camera_constant.name, {detection}}, + }; + + auto joint_solve_solution = joint_solver->EstimatePosition( + camera_detections, square_solver_solution.pose); } From 93b5a7ac330febdb3f9fcec455fcf5c71ce43627 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sun, 10 May 2026 03:01:41 +0000 Subject: [PATCH 31/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 154 ++++++++++++++++++------ src/localization/joint_solver.h | 29 ++--- src/localization/levenberg_marquardt.cc | 52 ++++++++ src/localization/levenberg_marquardt.h | 8 ++ src/test/unit_test/CMakeLists.txt | 3 + src/test/unit_test/xad_test.cc | 81 +++++++++++++ 6 files changed, 274 insertions(+), 53 deletions(-) create mode 100644 src/test/unit_test/xad_test.cc diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index da4d8ad2..22fafec7 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -35,6 +35,10 @@ const Eigen::Matrix rotate_yaw = 0, 0, 0, 1).finished(); // clang-format on +constexpr auto square(double x) -> double { + return x * x; +} + auto JointSolver::Transfrom3dDerivative::operator+( const Transfrom3dDerivative other) -> Transfrom3dDerivative { return Transfrom3dDerivative{.scaler = scaler + other.scaler, @@ -218,8 +222,8 @@ void JointSolver::DifferentiableTransform3d::CalculateMatrix() { matrix[3][0] = 0; matrix[3][1] = 0; - matrix[3][2] = 0; matrix[3][3] = 1; + matrix[3][2] = 0; } auto JointSolver::DifferentiableTransform3d::BackPropagate( @@ -297,9 +301,6 @@ auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, auto normalized_points_d = normalized_point - image_point; - // LOG(INFO) << "\n" << normalized_point << "\n" << image_point; - // LOG(INFO) << normalized_points_d.array().square().sum(); - return normalized_points_d.array().square().sum(); } @@ -315,22 +316,11 @@ auto JointSolver::NormalizePoint( // clang-format on } -auto JointSolver::ProjectPoints( - const Eigen::Matrix4d feild_to_robot, const Eigen::Matrix4d feild_to_tag, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Vector4d& homogenized_apriltag_corner) -> Eigen::Vector3d { - auto camera_to_tag = - camera_to_robot * feild_to_robot.inverse() * feild_to_tag * rotate_yaw; - - // LOG(INFO) << "feild_to_tag\n" << feild_to_tag; - // LOG(INFO) << "robot_to_feild\n" << feild_to_robot.inverse(); - LOG(INFO) << "camera_to_robot\n" << camera_to_robot; - LOG(INFO) << "test\n" << feild_to_robot.inverse() * feild_to_tag * rotate_yaw; - LOG(INFO) << "camera_to_tag\n" << camera_to_tag; +auto JointSolver::ProjectPoints(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& correction, + const Eigen::Vector4d& x) -> Eigen::Vector3d { - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_tag * homogenized_apriltag_corner; + Eigen::Vector3d projected_point = A * correction * x; auto normalized_point = projected_point / projected_point[0]; return normalized_point; } @@ -346,6 +336,34 @@ auto JointSolver::NormalizeCameraMatrix( return camera_matrix; } +auto JointSolver::CreateTransformationMatrix(const Eigen::VectorXd& params) + -> Eigen::Matrix4d { + double tx = params[0]; + double ty = params[1]; + double tz = params[2]; + + double rx = params[3]; + double ry = params[4]; + double rz = params[5]; + + const double cos_x = std::cos(rx); + const double cos_y = std::cos(ry); + const double cos_z = std::cos(rz); + + const double sin_x = std::sin(rx); + const double sin_y = std::sin(ry); + const double sin_z = std::sin(rz); + + // clang-format off + return (Eigen::Matrix() << + cos_z * cos_y, cos_z * sin_y * sin_x - sin_z * cos_x, cos_z * sin_y * cos_x + sin_z * sin_x, tx, + sin_z * cos_y, sin_z * sin_y * sin_x + cos_z * cos_x, sin_z * sin_y * cos_x - cos_z * sin_x, ty, + -sin_y, cos_y * sin_x, cos_y * cos_x, tz, + 0, 0, 0, 1) + .finished(); + // clang-format on +} + JointSolver::JointSolver( const std::vector& camera_constants, const AprilTagFieldLayout& layout) @@ -367,15 +385,66 @@ JointSolver::JointSolver( } auto JointSolver::CalculateResidual(const Eigen::VectorXd& params) - -> Eigen::VectorXd { - for (const auto data_point : data_points_) { - // auto camera_pose = field_to_robot_ * data_point.robot_to_camera; - auto projected_point = ProjectPoints( - field_to_robot_, data_point.feild_to_tag, - data_point.normalized_camera_matrix, data_point.camera_to_robot, - data_point.homogenized_apriltag_corner); - LOG(INFO) << "projected_point\n" << projected_point; - LOG(INFO) << "image_point\n" << data_point.normalized_image_point; + -> Eigen::MatrixXd { + const Eigen::MatrixXd correction = CreateTransformationMatrix(params); + Eigen::MatrixXd residual(data_points_.size(), 3); + for (size_t i = 0; i < data_points_.size(); i++) { + const auto& data_point = data_points_[i]; + auto projected_point = + ProjectPoints(data_point.A, correction, data_point.x); + const auto point_residual = + projected_point - data_point.normalized_image_point; + residual(i, 0) = point_residual[0]; + residual(i, 1) = point_residual[1]; + residual(i, 2) = point_residual[2]; + } + LOG(INFO) << "residual\n" << residual; + return residual; +} + +auto JointSolver::CalculateJacobian(const Eigen::VectorXd& params) + -> Eigen::MatrixXd { + + const Eigen::Matrix4d correction = CreateTransformationMatrix(params); + + for (size_t i = 0; i < data_points_.size(); i++) { + const auto& data_point = data_points_[i]; + + Eigen::Vector3d projected_point = + data_points_[i].A * correction * data_points_[i].x; + auto normalized_point = projected_point / projected_point[0]; + + // [z u v] + const double z = projected_point[0]; + const double u = projected_point[1]; + const double v = projected_point[2]; + + // clang-format off + const Eigen::Matrix normalized_point_d = + (Eigen::Matrix() << + 0, 0, 0, + -u / square(z), 1/z, 0, + -v / square(z), 0, 1/z).finished(); + // clang-format on + + const Eigen::MatrixXd A_d = normalized_point_d * data_points_[i].A; + + Eigen::Matrix J; + J(0, 0) = 0; // dz/t_x + J(0, 1) = 0; // dz/t_y + J(0, 2) = 0; // dz/t_z + J(0, 3) = 0; // dz/r_x + J(0, 4) = 0; // dz/r_y + J(0, 5) = 0; // dz/r_z + + J(1, 0) = A_d(1, 0) * data_points_[i].x(3); // du/t_x + J(0, 1) = A_d(1, 1) * data_points_[i].x(3); // du/t_y + J(0, 2) = A_d(1, 2) * data_points_[i].x(3); // du/t_z + J(0, 3) = 0; // du/r_x + J(0, 4) = 0; // du/r_y + J(0, 5) = 0; // du/r_z + + LOG(INFO) << "A_d\n" << A_d; } return {}; } @@ -387,6 +456,8 @@ auto JointSolver::EstimatePosition( std::vector tag_ids; double average_timestamp = 0; int total_detections = 0; + field_to_robot_ = initial_pose.value_or(position_receiver_.Get()).ToMatrix(); + const auto robot_to_feild = field_to_robot_.inverse(); data_points_.clear(); @@ -398,26 +469,31 @@ auto JointSolver::EstimatePosition( total_detections++; int corner_index = 0; for (const auto& corner : tag_detection.corners) { + const auto field_to_tag = + kapriltag_layout.GetTagPose(tag_detection.tag_id)->ToMatrix(); + const auto homogenized_apriltag_corner = + localization::kapriltag_corners_eigen_homogenized[corner_index]; + const auto normalized_camera_matrix = normalized_camera_matrix_[index]; + const auto camera_to_robot = robot_to_camera_[index].inverse(); data_points_.push_back(datapoint_t{ .normalized_image_point = NormalizePoint(corner, camera_constant_[index]), - .normalized_camera_matrix = normalized_camera_matrix_[index], - .camera_to_robot = robot_to_camera_[index].inverse(), - .feild_to_tag = - kapriltag_layout.GetTagPose(tag_detection.tag_id)->ToMatrix(), - .homogenized_apriltag_corner = localization:: - kapriltag_corners_eigen_homogenized[corner_index]}); + .normalized_camera_matrix = normalized_camera_matrix, + .camera_to_robot = camera_to_robot, + .feild_to_tag = field_to_tag, + .homogenized_apriltag_corner = homogenized_apriltag_corner, + .x = robot_to_feild * field_to_tag * rotate_yaw * + homogenized_apriltag_corner, + .A = normalized_camera_matrix * PI * camera_to_robot}); corner_index++; } } } - // frc::Pose3d(initial_pose.value_or(position_receiver_.Get())) - field_to_robot_ = initial_pose.value_or(position_receiver_.Get()).ToMatrix(); + const Eigen::VectorXd kZeroTransformParams = Eigen::VectorXd::Zero(6); + CalculateResidual(kZeroTransformParams); + CalculateJacobian(kZeroTransformParams); - for (size_t i = 0; i < data_points_.size(); i++) { - CalculateResidual({}); - } return position_estimate_t{ .tag_ids = tag_ids, .rejected_tag_ids = {}, diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 731a9ca9..21863e0e 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -19,11 +19,13 @@ class JointSolver { using AD = mode::active_type; using datapoint_t = struct DataPoint { - const Eigen::Vector3d& normalized_image_point; - const Eigen::Matrix3d& normalized_camera_matrix; - const Eigen::Matrix4d& camera_to_robot; - const Eigen::Matrix4d& feild_to_tag; - const Eigen::Vector4d& homogenized_apriltag_corner; + Eigen::Vector3d normalized_image_point; + Eigen::Matrix3d normalized_camera_matrix; + Eigen::Matrix4d camera_to_robot; + Eigen::Matrix4d feild_to_tag; + Eigen::Vector4d homogenized_apriltag_corner; + Eigen::Vector4d x; + Eigen::MatrixXd A; }; using transform3d_derivative_t = struct Transfrom3dDerivative { @@ -87,17 +89,17 @@ class JointSolver { const camera::camera_constant_t& camera_constant) -> Eigen::Vector3d; - auto static ProjectPoints(const Eigen::Matrix4d feild_to_robot, - const Eigen::Matrix4d feild_to_tag, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Vector4d& homogenized_apriltag_corner) - -> Eigen::Vector3d; + auto static ProjectPoints(const Eigen::MatrixXd& A, + const Eigen::MatrixXd& correction, + const Eigen::Vector4d& x) -> Eigen::Vector3d; auto static NormalizeCameraMatrix( Eigen::Matrix3d camera_matrix, const camera::camera_constant_t& camera_constant) -> Eigen::Matrix3d; + auto static CreateTransformationMatrix(const Eigen::VectorXd& params) + -> Eigen::Matrix4d; + public: JointSolver(const std::vector& camera_constants, const frc::AprilTagFieldLayout& layout = kapriltag_layout); @@ -106,9 +108,8 @@ class JointSolver { camera_detections, std::optional intial_pose = std::nullopt) -> position_estimate_t; - - private: - auto CalculateResidual(const Eigen::VectorXd& candidate) -> Eigen::VectorXd; + auto CalculateResidual(const Eigen::VectorXd& candidate) -> Eigen::MatrixXd; + auto CalculateJacobian(const Eigen::VectorXd& candidate) -> Eigen::MatrixXd; private: std::unordered_map camera_name_to_index; diff --git a/src/localization/levenberg_marquardt.cc b/src/localization/levenberg_marquardt.cc index 17edd11d..56ed86a0 100644 --- a/src/localization/levenberg_marquardt.cc +++ b/src/localization/levenberg_marquardt.cc @@ -46,3 +46,55 @@ auto levenberg_marquardt( lambda *= 2.0; return false; } + +auto levenberg_marquardt( + Eigen::VectorXd& params, + const std::function& + compute_residual, + const std::function& + compute_jacobian, + double& lambda, double tolerance) -> bool { + const Eigen::MatrixXd residual_matrix = compute_residual(params); + if (residual_matrix.size() == 0 || !residual_matrix.allFinite()) { + return false; + } + + const Eigen::Map residual(residual_matrix.data(), + residual_matrix.size()); + const double old_loss = residual.squaredNorm(); + + const Eigen::MatrixXd jacobian = compute_jacobian(params); + if (!jacobian.allFinite() || jacobian.rows() != residual.size()) { + return false; + } + + const Eigen::MatrixXd JTJ = jacobian.transpose() * jacobian; + Eigen::MatrixXd A = JTJ; + A.diagonal().array() += lambda; + + const Eigen::VectorXd g = jacobian.transpose() * residual; + + Eigen::VectorXd delta = A.ldlt().solve(g); + if (!delta.allFinite()) { + return false; + } + + const Eigen::VectorXd trial_params = params + delta; + const Eigen::MatrixXd trial_residual_matrix = compute_residual(trial_params); + if (trial_residual_matrix.size() != residual.size() || + !trial_residual_matrix.allFinite()) { + return false; + } + const Eigen::Map trial_residual( + trial_residual_matrix.data(), trial_residual_matrix.size()); + const double new_loss = trial_residual.squaredNorm(); + + if (new_loss <= old_loss) { + params = trial_params; + lambda *= 0.5; + return delta.norm() < tolerance; + } + + lambda *= 2.0; + return false; +} diff --git a/src/localization/levenberg_marquardt.h b/src/localization/levenberg_marquardt.h index 75a83ca7..f060e45c 100644 --- a/src/localization/levenberg_marquardt.h +++ b/src/localization/levenberg_marquardt.h @@ -16,3 +16,11 @@ auto levenberg_marquardt( const std::function& compute_jacobian, double& lambda, double tolerance = 1e-8) -> bool; + +auto levenberg_marquardt( + Eigen::VectorXd& params, + const std::function& + compute_residual, + const std::function& + compute_jacobian, + double& lambda, double tolerance = 1e-8) -> bool; diff --git a/src/test/unit_test/CMakeLists.txt b/src/test/unit_test/CMakeLists.txt index 3b88b4a1..913c6a5c 100644 --- a/src/test/unit_test/CMakeLists.txt +++ b/src/test/unit_test/CMakeLists.txt @@ -18,6 +18,9 @@ target_link_libraries(matrix PRIVATE localization utils GTest::gtest_main) add_executable(joint_solve_test joint_solve_test.cc) target_link_libraries(joint_solve_test PRIVATE localization utils GTest::gtest_main XAD::xad) +add_executable(xad_test xad_test.cc) +target_link_libraries(xad_test PRIVATE GTest::gtest_main XAD::xad) + include(GoogleTest) gtest_discover_tests(multi_tag_test) gtest_discover_tests(square_solve_test) diff --git a/src/test/unit_test/xad_test.cc b/src/test/unit_test/xad_test.cc new file mode 100644 index 00000000..ae1caf3f --- /dev/null +++ b/src/test/unit_test/xad_test.cc @@ -0,0 +1,81 @@ +#include +#include +#include +#include + +double x0 = 1.0; +double x1 = 1.5; +double x2 = 1.3; +double x3 = 1.2; + +template +auto f2(const T& x0_in, const T& x1_in, const T& x2_in, const T& x3_in) + -> std::pair { + T y1 = x0_in * x1_in + x2_in; + T y2 = x0_in + x2_in * x3_in; + return {y1, y2}; +} +TEST(XadTest, Add) { // NOLINT + + using mode = xad::fwd; + using AD = mode::active_type; + + AD x0_ad = x0; + AD x1_ad = x1; + AD x2_ad = x2; + AD x3_ad = x3; + + AD sum = x0_ad + x1_ad + x2_ad + x3_ad; + EXPECT_DOUBLE_EQ(sum.value(), x0 + x1 + x2 + x3); +} + +TEST(XadTest, Vector) { // NOLINT + // tape and active data type for 1st order adjoint computation with 2-vector mode + using mode = xad::adj; + using tape_type = mode::tape_type; + using AD = mode::active_type; + + // initialize tape + tape_type tape; + + // set independent variables + AD x0_ad = x0; + AD x1_ad = x1; + AD x2_ad = x2; + AD x3_ad = x3; + + // and register them + tape.registerInput(x0_ad); + tape.registerInput(x1_ad); + tape.registerInput(x2_ad); + tape.registerInput(x3_ad); + + // start recording derivatives + tape.newRecording(); + + // run function with 2 outputs + std::pair y = f2(x0_ad, x1_ad, x2_ad, x3_ad); + + // register and seed adjoint of outputs + tape.registerOutput(y.first); + tape.registerOutput(y.second); + y.first.setAdjoint({1.0, 0.0}); + y.second.setAdjoint({0.0, 1.0}); + + // compute all other adjoints + tape.computeAdjoints(); + + // output results + std::cout << "y1 = " << value(y.first) << "\n" + << "y2 = " << value(y.second) << "\n" + << "\nfirst order derivatives of y1:\n" + << "dy1/dx0 = " << x0_ad.getAdjoint()[0] << "\n" + << "dy1/dx1 = " << x1_ad.getAdjoint()[0] << "\n" + << "dy1/dx2 = " << x2_ad.getAdjoint()[0] << "\n" + << "dy1/dx3 = " << x3_ad.getAdjoint()[0] << "\n" + << "\nfirst order derivatives of y2:\n" + << "dy2/dx0 = " << x0_ad.getAdjoint()[1] << "\n" + << "dy2/dx1 = " << x1_ad.getAdjoint()[1] << "\n" + << "dy2/dx2 = " << x2_ad.getAdjoint()[1] << "\n" + << "dy2/dx3 = " << x3_ad.getAdjoint()[1] << "\n"; +} From 5bd016247fb83de3d821135a9374d664c5e330cc Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Mon, 11 May 2026 06:07:54 +0000 Subject: [PATCH 32/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 348 ++++++++++--------------------- src/localization/joint_solver.h | 74 +++---- src/test/unit_test/xad_test.cc | 36 ++++ 3 files changed, 174 insertions(+), 284 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 22fafec7..e66e4794 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -11,7 +11,7 @@ namespace localization { #define IMAGE_STRIDE 4 #define LOG_PATH "/bos-logs/log181/right" #define LR 0.05 -#define EPOCHS 100 +#define EPOCHS 1 #define BETA 0.3 using frc::AprilTagFieldLayout; @@ -39,161 +39,8 @@ constexpr auto square(double x) -> double { return x * x; } -auto JointSolver::Transfrom3dDerivative::operator+( - const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler + other.scaler, - .theta = theta + other.theta, - .t_z = t_z + other.t_z, - .r_x = r_x + other.r_x, - .r_y = r_y + other.r_y, - .r_z = r_z + other.r_z}; -} - -auto JointSolver::Transfrom3dDerivative::operator-( - const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler - other.scaler, - .theta = theta - other.theta, - .t_z = t_z - other.t_z, - .r_x = r_x - other.r_x, - .r_y = r_y - other.r_y, - .r_z = r_z - other.r_z}; -} - -auto JointSolver::Transfrom3dDerivative::operator*(const double other) - -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler * other, - .theta = theta * other, - .t_z = t_z * other, - .r_x = r_x * other, - .r_y = r_y * other, - .r_z = r_z * other}; -} - -auto JointSolver::Transfrom3dDerivative::operator+(const double other) - -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler + other, - .theta = theta + other, - .t_z = t_z + other, - .r_x = r_x + other, - .r_y = r_y + other, - .r_z = r_z + other}; -} - -auto JointSolver::Transfrom3dDerivative::operator*( - const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler * other.scaler, - .theta = theta * other.theta, - .t_z = t_z * other.t_z, - .r_x = r_x * other.r_x, - .r_y = r_y * other.r_y, - .r_z = r_z * other.r_z}; -} - -auto JointSolver::Transfrom3dDerivative::operator/( - const Transfrom3dDerivative other) -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = scaler / other.scaler, - .theta = theta / other.theta, - .t_z = t_z / other.t_z, - .r_x = r_x / other.r_x, - .r_y = r_y / other.r_y, - .r_z = r_z / other.r_z}; -} - -auto JointSolver::Transfrom3dDerivative::sqrt() -> Transfrom3dDerivative { - return Transfrom3dDerivative{.scaler = std::sqrt(scaler), - .theta = std::sqrt(theta), - .t_z = std::sqrt(t_z), - .r_x = std::sqrt(r_x), - .r_y = std::sqrt(r_y), - .r_z = std::sqrt(r_z)}; -} - -JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( - const Eigen::VectorXd& params) - : scaler(params(0)), - theta(params(1)), - t_z(params(2)), - r_x(params(3)), - r_y(params(4)), - r_z(params(5)) {} - -JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( - frc::Pose3d pose) - : scaler(std::hypot(pose.Translation().X().value(), - pose.Translation().Y().value())), - theta(std::atan2(pose.Translation().Y().value(), - pose.Translation().X().value())), - t_z(pose.Translation().Z().value()), - r_x(pose.Rotation().X().value()), - r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} - -JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( - frc::Transform3d pose) - : scaler(std::hypot(pose.Translation().X().value(), - pose.Translation().Y().value())), - theta(std::atan2(pose.Translation().Y().value(), - pose.Translation().X().value())), - t_z(pose.Translation().Z().value()), - r_x(pose.Rotation().X().value()), - r_y(pose.Rotation().Y().value()), - r_z(pose.Rotation().Z().value()) {} - -JointSolver::DifferentiableTransform3d::DifferentiableTransform3d( - Eigen::Matrix4d matrix) - : scaler(std::hypot(matrix(0, 3), matrix(1, 3))), - theta(std::atan2(matrix(1, 3), matrix(0, 3))), - t_z(matrix(2, 3)) { - Eigen::Matrix3d R = matrix.block<3, 3>(0, 0); - Eigen::Vector3d euler = R.canonicalEulerAngles(2, 1, 0); - r_x = euler(2); - r_z = euler(0); - r_y = euler(1); -} - -void JointSolver::DifferentiableTransform3d::Update( - transform3d_derivative_t derrivative, double lr_translation, - double lr_rotation) { - scaler -= derrivative.scaler * lr_translation; - theta -= derrivative.theta * lr_translation; - // t_z -= derrivative.t_z * lr_translation; - - // r_x -= derrivative.r_x * lr; - // r_y -= derrivative.r_y * lr; - r_z -= derrivative.r_z * lr_rotation; -} - -void JointSolver::DifferentiableTransform3d::RegisterInputs(tape_type& tape) { - tape.registerInput(r_x); - tape.registerInput(r_y); - tape.registerInput(r_z); - - tape.registerInput(scaler); - tape.registerInput(theta); - tape.registerInput(t_z); -} - -void JointSolver::DifferentiableTransform3d::RegisterOutputs(tape_type& tape) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); - } - } -} - -auto JointSolver::DifferentiableTransform3d::ToEigen() - -> Eigen::Matrix4d const { - // clang-format off - return (Eigen::Matrix() << - matrix[0][0].value(), matrix[0][1].value(), matrix[0][2].value(), matrix[0][3].value(), - matrix[1][0].value(), matrix[1][1].value(), matrix[1][2].value(), matrix[1][3].value(), - matrix[2][0].value(), matrix[2][1].value(), matrix[2][2].value(), matrix[2][3].value(), - matrix[3][0].value(), matrix[3][1].value(), matrix[3][2].value(), matrix[3][3].value()) - .finished(); - // clang-format on -} - -void JointSolver::DifferentiableTransform3d::CalculateMatrix() { +auto JointSolver::DifferentiableTransform3d::ToMatrix() + -> std::array, 4> { AD cos_x = xad::cos(r_x); AD cos_y = xad::cos(r_y); AD cos_z = xad::cos(r_z); @@ -203,6 +50,7 @@ void JointSolver::DifferentiableTransform3d::CalculateMatrix() { AD sin_z = xad::sin(r_z); // Rotation + std::array, 4> matrix; matrix[0][0] = cos_z * cos_y; matrix[0][1] = cos_z * sin_y * sin_x - sin_z * cos_x; matrix[0][2] = cos_z * sin_y * cos_x + sin_z * sin_x; @@ -216,74 +64,15 @@ void JointSolver::DifferentiableTransform3d::CalculateMatrix() { matrix[2][2] = cos_y * cos_x; // Translation - matrix[0][3] = std::cos(theta) * scaler; - matrix[1][3] = std::sin(theta) * scaler; + matrix[0][3] = t_x; + matrix[1][3] = t_y; matrix[2][3] = t_z; matrix[3][0] = 0; matrix[3][1] = 0; matrix[3][3] = 1; matrix[3][2] = 0; -} - -auto JointSolver::DifferentiableTransform3d::BackPropagate( - const Eigen::Matrix4d& next_derrivative, tape_type& tape) - -> transform3d_derivative_t { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - tape.registerOutput(matrix[i][j]); - derivative(matrix[i][j]) = next_derrivative(i, j); - } - } - tape.computeAdjoints(); - transform3d_derivative_t derrivative{ - .scaler = xad::derivative(scaler), - .theta = xad::derivative(theta), - .t_z = xad::derivative(t_z), - .r_x = xad::derivative(r_x), - .r_y = xad::derivative(r_y), - .r_z = xad::derivative(r_z), - }; - return derrivative; -} - -auto JointSolver::CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, - int corner_index) -> Eigen::Matrix4d { - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * - rotate_yaw * - localization::kapriltag_corners_eigen_homogenized[corner_index]; - auto normalized_point = projected_point / projected_point[0]; - - auto normalized_points_d = normalized_point - image_point; - - // clang-format off - auto projected_point_d = (Eigen::Vector3d() << - (-normalized_points_d[1] * projected_point[1]) / (projected_point[0] * projected_point[0]) + - (-normalized_points_d[2] * projected_point[1]) / (projected_point[0] * projected_point[0]), - normalized_points_d[1] / projected_point[0], - normalized_points_d[2] / projected_point[0] - ).finished(); - // clang-format on - - auto camera_matrix_xd = camera_matrix.transpose() * projected_point_d; - - auto PI_xd = PI.transpose() * camera_matrix_xd; - - auto camera_to_robot_xd = camera_to_robot.transpose() * PI_xd; - - auto robot_to_feild_d = - camera_to_robot_xd * - - (feild_to_tag * rotate_yaw * - localization::kapriltag_corners_eigen_homogenized[corner_index]) - .transpose(); - - return robot_to_feild_d; + return matrix; } auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, @@ -363,6 +152,53 @@ auto JointSolver::CreateTransformationMatrix(const Eigen::VectorXd& params) .finished(); // clang-format on } +auto JointSolver::Multiply(const std::array, 4>& a, + const Eigen::Vector4d& b) -> std::array { + std::array out{}; + for (int i = 0; i < 4; i++) { + out[i] = 0; + for (int j = 0; j < 4; j++) { + out[i] += a[i][j] * b[j]; + } + } + return out; +} +auto JointSolver::Multiply(const Eigen::Matrix& a, + const std::array& b) -> std::array { + std::array out{}; + for (int i = 0; i < 3; i++) { + out[i] = 0; + for (int j = 0; j < 4; j++) { + out[i] += a(i, j) * b[j]; + } + } + return out; +} + +void JointSolver::SaveJacobian(Eigen::MatrixXd& J, + const differentiable_transform3d_t& correction, + int index) { + const int offset = index * 2; + J(0 + offset, 0) = correction.t_x.getAdjoint()[0]; + J(0 + offset, 1) = correction.t_y.getAdjoint()[0]; + J(0 + offset, 2) = correction.t_z.getAdjoint()[0]; + J(0 + offset, 3) = correction.r_x.getAdjoint()[0]; + J(0 + offset, 4) = correction.r_y.getAdjoint()[0]; + J(0 + offset, 5) = correction.r_z.getAdjoint()[0]; + J(1 + offset, 0) = correction.t_x.getAdjoint()[1]; + J(1 + offset, 1) = correction.t_y.getAdjoint()[1]; + J(1 + offset, 2) = correction.t_z.getAdjoint()[1]; + J(1 + offset, 3) = correction.r_x.getAdjoint()[1]; + J(1 + offset, 4) = correction.r_y.getAdjoint()[1]; + J(1 + offset, 5) = correction.r_z.getAdjoint()[1]; +} + +void JointSolver::SaveResidual(Eigen::VectorXd& residual, double u_residual, + double v_residual, int index) { + const int offset = index * 2; + residual[0 + offset] = u_residual; + residual[1 + offset] = v_residual; +} JointSolver::JointSolver( const std::vector& camera_constants, @@ -444,7 +280,7 @@ auto JointSolver::CalculateJacobian(const Eigen::VectorXd& params) J(0, 4) = 0; // du/r_y J(0, 5) = 0; // du/r_z - LOG(INFO) << "A_d\n" << A_d; + // LOG(INFO) << "A_d\n" << A_d; } return {}; } @@ -490,9 +326,60 @@ auto JointSolver::EstimatePosition( } } - const Eigen::VectorXd kZeroTransformParams = Eigen::VectorXd::Zero(6); - CalculateResidual(kZeroTransformParams); - CalculateJacobian(kZeroTransformParams); + differentiable_transform3d_t correction; + value(correction.t_x) = 0.0; + value(correction.t_y) = 0.0; + value(correction.t_z) = 0.0; + value(correction.r_x) = 0.0; + value(correction.r_y) = 0.0; + value(correction.r_z) = 0.0; + + for (int epoch = 0; epoch < EPOCHS; epoch++) { + Eigen::MatrixXd J(data_points_.size() * 2, 6); + Eigen::VectorXd residual(data_points_.size() * 2); + + tape_.registerInput(correction.t_x); + tape_.registerInput(correction.t_y); + tape_.registerInput(correction.t_z); + tape_.registerInput(correction.r_x); + tape_.registerInput(correction.r_y); + tape_.registerInput(correction.r_z); + tape_.newRecording(); + int index = 0; + for (auto const& data_point : data_points_) { + const auto correction_matrix = correction.ToMatrix(); + auto a = Multiply(correction_matrix, data_point.x); + std::array projected_point = Multiply(data_point.A, a); + AD u = projected_point[1] / projected_point[0]; + AD v = projected_point[2] / projected_point[0]; + tape_.registerOutput(u); + tape_.registerOutput(v); + u.setAdjoint({1.0, 0.0}); + v.setAdjoint({0.0, 1.0}); + LOG(INFO) << u.value() << " " << data_point.normalized_image_point[1]; + tape_.computeAdjoints(); + SaveJacobian(J, correction, index); + SaveResidual(residual, u.value() - data_point.normalized_image_point[1], + v.value() - data_point.normalized_image_point[2], index); + + index++; + } + LOG(INFO) << "J\n" << J; + LOG(INFO) << "Residual\n" << residual; + // J(0, 0) = correction.t_x.getAdjoint()[0]; + // J(0, 1) = correction.t_y.getAdjoint()[0]; + // J(0, 2) = correction.t_z.getAdjoint()[0]; + // J(0, 3) = correction.r_x.getAdjoint()[0]; + // J(0, 4) = correction.r_y.getAdjoint()[0]; + // J(0, 5) = correction.r_z.getAdjoint()[0]; + // J(1, 0) = correction.t_x.getAdjoint()[1]; + // J(1, 1) = correction.t_y.getAdjoint()[1]; + // J(1, 2) = correction.t_z.getAdjoint()[1]; + // J(1, 3) = correction.r_x.getAdjoint()[1]; + // J(1, 4) = correction.r_y.getAdjoint()[1]; + // J(1, 5) = correction.r_z.getAdjoint()[1]; + // LOG(INFO) << "J\n" << J; + } return position_estimate_t{ .tag_ids = tag_ids, @@ -508,22 +395,11 @@ auto JointSolver::EstimatePosition( }; } -auto operator<<(std::ostream& os, const JointSolver::Transfrom3dDerivative& d) - -> std::ostream& { - os << "scaler=" << d.scaler << "\n" - << "theta=" << d.theta << "\n" - << "t_z=" << d.t_z << "\n" - << "r_x=" << d.r_x << "\n" - << "r_y=" << d.r_y << "\n" - << "r_z=" << d.r_z << ""; - return os; -} - auto operator<<(std::ostream& os, - const JointSolver::DifferentiableTransform3d& t) + const JointSolver::differentiable_transform3d_t& t) -> std::ostream& { - os << "scaler=" << t.scaler << "\n" - << "theta=" << t.theta << "\n" + os << "t_x=" << t.t_x << "\n" + << "t_y=" << t.t_y << "\n" << "t_z=" << t.t_z << "\n" << "r_x=" << t.r_x << "\n" << "r_y=" << t.r_y << "\n" diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 21863e0e..51303ead 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -14,7 +14,7 @@ namespace localization { class JointSolver { public: - using mode = xad::adj; + using mode = xad::adj; using tape_type = mode::tape_type; using AD = mode::active_type; @@ -28,56 +28,18 @@ class JointSolver { Eigen::MatrixXd A; }; - using transform3d_derivative_t = struct Transfrom3dDerivative { - double scaler = 0; - double theta = 0; - double t_z = 0; - double r_x = 0; - double r_y = 0; - double r_z = 0; - - auto operator+(const Transfrom3dDerivative other) -> Transfrom3dDerivative; - auto operator-(const Transfrom3dDerivative other) -> Transfrom3dDerivative; - auto operator*(const double other) -> Transfrom3dDerivative; - auto operator+(const double other) -> Transfrom3dDerivative; - auto operator*(const Transfrom3dDerivative other) -> Transfrom3dDerivative; - auto operator/(const Transfrom3dDerivative other) -> Transfrom3dDerivative; - auto sqrt() -> Transfrom3dDerivative; - }; + using differentiable_transform3d_t = struct DifferentiableTransform3d { + AD t_x = 0; + AD t_y = 0; + AD t_z = 0; + AD r_x = 0; + AD r_y = 0; + AD r_z = 0; - struct DifferentiableTransform3d { - AD scaler; - AD theta; - - AD t_z; - AD r_x; - AD r_y; - AD r_z; - std::array, 4> matrix; - - DifferentiableTransform3d(frc::Pose3d pose); - DifferentiableTransform3d(frc::Transform3d pose); - DifferentiableTransform3d(Eigen::Matrix4d matrix); - DifferentiableTransform3d(const Eigen::VectorXd& params); - - void Update(transform3d_derivative_t derrivative, double lr_translation, - double lr_rotation); - void RegisterInputs(tape_type& tape); - void RegisterOutputs(tape_type& tape); - auto ToEigen() -> Eigen::Matrix4d const; - void CalculateMatrix(); - auto BackPropagate(const Eigen::Matrix4d& next_derrivative, tape_type& tape) - -> transform3d_derivative_t; + auto ToMatrix() -> std::array, 4>; }; public: - auto static CalculateDerivative(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, - int corner_index) -> Eigen::Matrix4d; - auto static CalculateLoss(const Eigen::Matrix4d& robot_to_feild, const Eigen::Matrix4d& feild_to_tag, const Eigen::Matrix4d& camera_to_robot, @@ -100,6 +62,21 @@ class JointSolver { auto static CreateTransformationMatrix(const Eigen::VectorXd& params) -> Eigen::Matrix4d; + auto static Multiply(const std::array, 4>& a, + const Eigen::Vector4d& b) -> std::array; + + auto static Multiply(const Eigen::Matrix& a, + const std::array& b) -> std::array; + + // Jacobian should be data_points_.size() x 6(number of params in correction) + // index is should be from 1...data_points_.size() + void static SaveJacobian(Eigen::MatrixXd& J, + const differentiable_transform3d_t& correction, + int index); + + void static SaveResidual(Eigen::VectorXd& residual, double u_residual, + double v_residual, int index); + public: JointSolver(const std::vector& camera_constants, const frc::AprilTagFieldLayout& layout = kapriltag_layout); @@ -124,7 +101,8 @@ class JointSolver { tape_type tape_; }; -auto operator<<(std::ostream& os, const JointSolver::Transfrom3dDerivative& d) +auto operator<<(std::ostream& os, + const JointSolver::differentiable_transform3d_t& d) -> std::ostream&; auto operator<<(std::ostream& os, diff --git a/src/test/unit_test/xad_test.cc b/src/test/unit_test/xad_test.cc index ae1caf3f..60a34072 100644 --- a/src/test/unit_test/xad_test.cc +++ b/src/test/unit_test/xad_test.cc @@ -66,6 +66,42 @@ TEST(XadTest, Vector) { // NOLINT tape.computeAdjoints(); // output results + auto y1 = value(y.first); + auto y2 = value(y.second); + auto dy1_dx0 = x0_ad.getAdjoint()[0]; + auto dy1_dx1 = x1_ad.getAdjoint()[0]; + auto dy1_dx2 = x2_ad.getAdjoint()[0]; + auto dy1_dx3 = x3_ad.getAdjoint()[0]; + auto dy2_dx0 = x0_ad.getAdjoint()[1]; + auto dy2_dx1 = x1_ad.getAdjoint()[1]; + auto dy2_dx2 = x2_ad.getAdjoint()[1]; + auto dy2_dx3 = x3_ad.getAdjoint()[1]; + + tape.newRecording(); + tape.registerInput(x0_ad); + tape.registerInput(x1_ad); + tape.registerInput(x2_ad); + tape.registerInput(x3_ad); + tape.registerOutput(y.first); + tape.registerOutput(y.second); + y = f2(x0_ad, x1_ad, x2_ad, x3_ad); + y.first.setAdjoint({1.0, 0.0}); + y.second.setAdjoint({0.0, 1.0}); + tape.computeAdjoints(); + + ASSERT_DOUBLE_EQ(y1, value(y.first)); + ASSERT_DOUBLE_EQ(y2, value(y.second)); + + EXPECT_DOUBLE_EQ(dy1_dx0, x0_ad.getAdjoint()[0]); + EXPECT_DOUBLE_EQ(dy1_dx1, x1_ad.getAdjoint()[0]); + EXPECT_DOUBLE_EQ(dy1_dx2, x2_ad.getAdjoint()[0]); + EXPECT_DOUBLE_EQ(dy1_dx3, x3_ad.getAdjoint()[0]); + + EXPECT_DOUBLE_EQ(dy2_dx0, x0_ad.getAdjoint()[1]); + EXPECT_DOUBLE_EQ(dy2_dx1, x1_ad.getAdjoint()[1]); + EXPECT_DOUBLE_EQ(dy2_dx2, x2_ad.getAdjoint()[1]); + EXPECT_DOUBLE_EQ(dy2_dx3, x3_ad.getAdjoint()[1]); + std::cout << "y1 = " << value(y.first) << "\n" << "y2 = " << value(y.second) << "\n" << "\nfirst order derivatives of y1:\n" From bd0f2819a22f78820f412a9af6378f00acfceb5c Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Tue, 12 May 2026 07:42:21 +0000 Subject: [PATCH 33/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 161 ++++++++++++++++++++++++++----- src/localization/joint_solver.h | 14 ++- 2 files changed, 145 insertions(+), 30 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index e66e4794..5f5634d7 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -1,4 +1,5 @@ #include "src/localization/joint_solver.h" +#include #include #include #include "src/localization/position_solver.h" @@ -8,11 +9,7 @@ namespace localization { -#define IMAGE_STRIDE 4 -#define LOG_PATH "/bos-logs/log181/right" -#define LR 0.05 -#define EPOCHS 1 -#define BETA 0.3 +#define EPOCHS 200 using frc::AprilTagFieldLayout; using utils::CameraMatrixFromJson; @@ -74,6 +71,17 @@ auto JointSolver::DifferentiableTransform3d::ToMatrix() matrix[3][2] = 0; return matrix; } +void JointSolver::DifferentiableTransform3d::Update( + const Eigen::VectorXd& update) { + CHECK(update.size() == 6); + t_x += update[0]; + t_y += update[1]; + t_z += update[2]; + + r_x += update[3]; + r_y += update[4]; + r_z += update[5]; +} auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, const Eigen::Matrix4d& feild_to_tag, @@ -200,6 +208,57 @@ void JointSolver::SaveResidual(Eigen::VectorXd& residual, double u_residual, residual[1 + offset] = v_residual; } +auto JointSolver::CalculateUpdate(const Eigen::MatrixXd& J, + const Eigen::VectorXd& residual, + double lambda) -> Eigen::VectorXd { + constexpr double kEpsilon = 1e-9; + + const Eigen::MatrixXd j_t_j = J.transpose() * J; + Eigen::VectorXd diagonal = j_t_j.diagonal(); + for (int i = 0; i < diagonal.size(); ++i) { + if (diagonal[i] <= kEpsilon) { + diagonal[i] = kEpsilon; + } + } + + const Eigen::MatrixXd damping = + (lambda * diagonal).asDiagonal().toDenseMatrix(); + const Eigen::MatrixXd damped_hessian = + j_t_j + damping + + kEpsilon * Eigen::MatrixXd::Identity(J.cols(), J.cols()); + const Eigen::VectorXd gradient = J.transpose() * residual; + + const Eigen::LDLT solver(damped_hessian); + if (solver.info() != Eigen::Success) { + return Eigen::VectorXd::Zero(J.cols()); + } + + return solver.solve(-gradient); +} + +auto JointSolver::CalculateResidualLoss( + const differentiable_transform3d_t& correction, + const std::vector& data_points) -> double { + Eigen::VectorXd params(6); + params << value(correction.t_x), value(correction.t_y), value(correction.t_z), + value(correction.r_x), value(correction.r_y), value(correction.r_z); + + const Eigen::Matrix4d correction_matrix = CreateTransformationMatrix(params); + + double loss = 0.0; + for (const auto& data_point : data_points) { + const Eigen::Vector3d projected_point = + ProjectPoints(data_point.A, correction_matrix, data_point.x); + const double u_residual = + projected_point[1] - data_point.normalized_image_point[1]; + const double v_residual = + projected_point[2] - data_point.normalized_image_point[2]; + loss += u_residual * u_residual + v_residual * v_residual; + } + + return loss; +} + JointSolver::JointSolver( const std::vector& camera_constants, const AprilTagFieldLayout& layout) @@ -327,12 +386,22 @@ auto JointSolver::EstimatePosition( } differentiable_transform3d_t correction; - value(correction.t_x) = 0.0; - value(correction.t_y) = 0.0; - value(correction.t_z) = 0.0; - value(correction.r_x) = 0.0; - value(correction.r_y) = 0.0; - value(correction.r_z) = 0.0; + value(correction.t_x) = 0.5; + value(correction.t_y) = -0.5; + value(correction.t_z) = 0.3; + value(correction.r_x) = 0.3; + value(correction.r_y) = -0.3; + value(correction.r_z) = 0.4; + + double checkpoint_loss = CalculateResidualLoss(correction, data_points_); + double lambda = 1e-4; + constexpr double kMinLambda = 1e-12; + constexpr double kMaxLambda = 1e12; + constexpr double kMinUpdateNorm = 1e-14; + constexpr double kMinLossImprovement = 1e-15; + constexpr int kMaxAttemptsPerEpoch = 10; + constexpr int kMaxStaleEpochs = 20; + int stale_epochs = 0; for (int epoch = 0; epoch < EPOCHS; epoch++) { Eigen::MatrixXd J(data_points_.size() * 2, 6); @@ -356,7 +425,6 @@ auto JointSolver::EstimatePosition( tape_.registerOutput(v); u.setAdjoint({1.0, 0.0}); v.setAdjoint({0.0, 1.0}); - LOG(INFO) << u.value() << " " << data_point.normalized_image_point[1]; tape_.computeAdjoints(); SaveJacobian(J, correction, index); SaveResidual(residual, u.value() - data_point.normalized_image_point[1], @@ -364,21 +432,62 @@ auto JointSolver::EstimatePosition( index++; } - LOG(INFO) << "J\n" << J; - LOG(INFO) << "Residual\n" << residual; - // J(0, 0) = correction.t_x.getAdjoint()[0]; - // J(0, 1) = correction.t_y.getAdjoint()[0]; - // J(0, 2) = correction.t_z.getAdjoint()[0]; - // J(0, 3) = correction.r_x.getAdjoint()[0]; - // J(0, 4) = correction.r_y.getAdjoint()[0]; - // J(0, 5) = correction.r_z.getAdjoint()[0]; - // J(1, 0) = correction.t_x.getAdjoint()[1]; - // J(1, 1) = correction.t_y.getAdjoint()[1]; - // J(1, 2) = correction.t_z.getAdjoint()[1]; - // J(1, 3) = correction.r_x.getAdjoint()[1]; - // J(1, 4) = correction.r_y.getAdjoint()[1]; - // J(1, 5) = correction.r_z.getAdjoint()[1]; + const double loss = residual.squaredNorm(); + bool accepted = false; + double best_candidate_loss = checkpoint_loss; + Eigen::VectorXd accepted_update = Eigen::VectorXd::Zero(6); + double trial_lambda = lambda; + + for (int attempt = 0; attempt < kMaxAttemptsPerEpoch; ++attempt) { + const Eigen::VectorXd update = CalculateUpdate(J, residual, trial_lambda); + if (update.norm() < kMinUpdateNorm) { + break; + } + + differentiable_transform3d_t candidate = correction; + candidate.Update(update); + const double candidate_loss = + CalculateResidualLoss(candidate, data_points_); + + if (candidate_loss < checkpoint_loss) { + accepted = true; + accepted_update = update; + best_candidate_loss = candidate_loss; + lambda = std::max(kMinLambda, trial_lambda * 0.25); + break; + } + + trial_lambda = std::min(kMaxLambda, trial_lambda * 8.0); + } + + LOG(INFO) << "--------------------------------------"; // LOG(INFO) << "J\n" << J; + LOG(INFO) << "Residual\n" << residual; + LOG(INFO) << "Loss: " << loss; + LOG(INFO) << "Checkpoint Loss: " << checkpoint_loss; + LOG(INFO) << "Candidate Loss: " << best_candidate_loss; + LOG(INFO) << "lambda: " << lambda; + + if (accepted) { + const double loss_improvement = checkpoint_loss - best_candidate_loss; + correction.Update(accepted_update); + checkpoint_loss = best_candidate_loss; + if (loss_improvement < kMinLossImprovement) { + stale_epochs++; + } else { + stale_epochs = 0; + } + } else { + stale_epochs++; + lambda = std::min(kMaxLambda, trial_lambda); + if (lambda >= kMaxLambda) { + break; + } + } + + if (stale_epochs >= kMaxStaleEpochs) { + break; + } } return position_estimate_t{ diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 51303ead..1e623eb3 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -37,6 +37,7 @@ class JointSolver { AD r_z = 0; auto ToMatrix() -> std::array, 4>; + void Update(const Eigen::VectorXd& update); }; public: @@ -77,6 +78,15 @@ class JointSolver { void static SaveResidual(Eigen::VectorXd& residual, double u_residual, double v_residual, int index); + auto static CalculateUpdate(const Eigen::MatrixXd& J, + const Eigen::VectorXd& residual, + double lambda) + -> Eigen::VectorXd; + + auto static CalculateResidualLoss( + const differentiable_transform3d_t& correction, + const std::vector& data_points) -> double; + public: JointSolver(const std::vector& camera_constants, const frc::AprilTagFieldLayout& layout = kapriltag_layout); @@ -101,10 +111,6 @@ class JointSolver { tape_type tape_; }; -auto operator<<(std::ostream& os, - const JointSolver::differentiable_transform3d_t& d) - -> std::ostream&; - auto operator<<(std::ostream& os, const JointSolver::DifferentiableTransform3d& t) -> std::ostream&; From b7193a1bfa2d63960bdc6b34c438d5259556e850 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 01:42:35 +0000 Subject: [PATCH 34/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 130 +++++++++---------------- src/localization/joint_solver.h | 10 +- src/test/unit_test/joint_solve_test.cc | 17 ++++ 3 files changed, 68 insertions(+), 89 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 5f5634d7..5156485b 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -35,6 +35,16 @@ const Eigen::Matrix rotate_yaw = constexpr auto square(double x) -> double { return x * x; } +auto JointSolver::DifferentiableTransform3d::ToEigen() -> Eigen::Matrix4d { + const auto matrix = ToMatrix(); + Eigen::Matrix4d out = Eigen::Matrix4d::Zero(); + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + out(i, j) = value(matrix[i][j]); + } + } + return out; +} auto JointSolver::DifferentiableTransform3d::ToMatrix() -> std::array, 4> { @@ -88,8 +98,8 @@ auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, const Eigen::Matrix4d& camera_to_robot, const Eigen::Matrix3d& camera_matrix, const Eigen::Vector3d& image_point, - int corner_index) -> double { + int corner_index) -> double { Eigen::Vector3d projected_point = camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * rotate_yaw * @@ -160,6 +170,20 @@ auto JointSolver::CreateTransformationMatrix(const Eigen::VectorXd& params) .finished(); // clang-format on } + +auto JointSolver::Multiply(const std::array, 4>& a, + const Eigen::Matrix4d& b) -> Eigen::Matrix4d { + Eigen::Matrix4d out = Eigen::Matrix4d::Zero(); + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + for (int k = 0; k < 4; ++k) { + out(i, j) += value(a[i][k]) * b(k, j); + } + } + } + return out; +} + auto JointSolver::Multiply(const std::array, 4>& a, const Eigen::Vector4d& b) -> std::array { std::array out{}; @@ -279,71 +303,6 @@ JointSolver::JointSolver( } } -auto JointSolver::CalculateResidual(const Eigen::VectorXd& params) - -> Eigen::MatrixXd { - const Eigen::MatrixXd correction = CreateTransformationMatrix(params); - Eigen::MatrixXd residual(data_points_.size(), 3); - for (size_t i = 0; i < data_points_.size(); i++) { - const auto& data_point = data_points_[i]; - auto projected_point = - ProjectPoints(data_point.A, correction, data_point.x); - const auto point_residual = - projected_point - data_point.normalized_image_point; - residual(i, 0) = point_residual[0]; - residual(i, 1) = point_residual[1]; - residual(i, 2) = point_residual[2]; - } - LOG(INFO) << "residual\n" << residual; - return residual; -} - -auto JointSolver::CalculateJacobian(const Eigen::VectorXd& params) - -> Eigen::MatrixXd { - - const Eigen::Matrix4d correction = CreateTransformationMatrix(params); - - for (size_t i = 0; i < data_points_.size(); i++) { - const auto& data_point = data_points_[i]; - - Eigen::Vector3d projected_point = - data_points_[i].A * correction * data_points_[i].x; - auto normalized_point = projected_point / projected_point[0]; - - // [z u v] - const double z = projected_point[0]; - const double u = projected_point[1]; - const double v = projected_point[2]; - - // clang-format off - const Eigen::Matrix normalized_point_d = - (Eigen::Matrix() << - 0, 0, 0, - -u / square(z), 1/z, 0, - -v / square(z), 0, 1/z).finished(); - // clang-format on - - const Eigen::MatrixXd A_d = normalized_point_d * data_points_[i].A; - - Eigen::Matrix J; - J(0, 0) = 0; // dz/t_x - J(0, 1) = 0; // dz/t_y - J(0, 2) = 0; // dz/t_z - J(0, 3) = 0; // dz/r_x - J(0, 4) = 0; // dz/r_y - J(0, 5) = 0; // dz/r_z - - J(1, 0) = A_d(1, 0) * data_points_[i].x(3); // du/t_x - J(0, 1) = A_d(1, 1) * data_points_[i].x(3); // du/t_y - J(0, 2) = A_d(1, 2) * data_points_[i].x(3); // du/t_z - J(0, 3) = 0; // du/r_x - J(0, 4) = 0; // du/r_y - J(0, 5) = 0; // du/r_z - - // LOG(INFO) << "A_d\n" << A_d; - } - return {}; -} - auto JointSolver::EstimatePosition( const std::map>& camera_detections, @@ -351,8 +310,9 @@ auto JointSolver::EstimatePosition( std::vector tag_ids; double average_timestamp = 0; int total_detections = 0; - field_to_robot_ = initial_pose.value_or(position_receiver_.Get()).ToMatrix(); - const auto robot_to_feild = field_to_robot_.inverse(); + const Eigen::Matrix4d field_to_robot = + initial_pose.value_or(position_receiver_.Get()).ToMatrix(); + const Eigen::Matrix4d robot_to_feild = field_to_robot.inverse(); data_points_.clear(); @@ -386,12 +346,6 @@ auto JointSolver::EstimatePosition( } differentiable_transform3d_t correction; - value(correction.t_x) = 0.5; - value(correction.t_y) = -0.5; - value(correction.t_z) = 0.3; - value(correction.r_x) = 0.3; - value(correction.r_y) = -0.3; - value(correction.r_z) = 0.4; double checkpoint_loss = CalculateResidualLoss(correction, data_points_); double lambda = 1e-4; @@ -460,14 +414,14 @@ auto JointSolver::EstimatePosition( trial_lambda = std::min(kMaxLambda, trial_lambda * 8.0); } - LOG(INFO) << "--------------------------------------"; - // LOG(INFO) << "J\n" << J; - LOG(INFO) << "Residual\n" << residual; - LOG(INFO) << "Loss: " << loss; - LOG(INFO) << "Checkpoint Loss: " << checkpoint_loss; - LOG(INFO) << "Candidate Loss: " << best_candidate_loss; - LOG(INFO) << "lambda: " << lambda; - + VLOG(1) << "--------------------------------------"; + VLOG(1) << "Residual\n" << residual; + VLOG(1) << "Loss: " << loss; + VLOG(1) << "Checkpoint Loss: " << checkpoint_loss; + VLOG(1) << "Candidate Loss: " << best_candidate_loss; + VLOG(1) << "lambda: " << lambda; + VLOG(1) << "update\n" << accepted_update; + VLOG(1) << "correction\n" << correction; if (accepted) { const double loss_improvement = checkpoint_loss - best_candidate_loss; correction.Update(accepted_update); @@ -490,17 +444,25 @@ auto JointSolver::EstimatePosition( } } + Eigen::Matrix4d tmp = + (Multiply(correction.ToMatrix(), robot_to_feild)).inverse(); + tmp(3, 0) = 0; + tmp(3, 1) = 0; + tmp(3, 2) = 0; + tmp(3, 3) = 1; + frc::Pose3d pose(tmp); + return position_estimate_t{ .tag_ids = tag_ids, .rejected_tag_ids = {}, - .pose = {}, + .pose = pose, .variance = 1, .timestamp = average_timestamp, .num_tags = total_detections, .avg_tag_dist = -1, .latency = -1, .invalid = false, - .loss = 0, + .loss = checkpoint_loss, }; } diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 1e623eb3..ed65bace 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -36,6 +36,7 @@ class JointSolver { AD r_y = 0; AD r_z = 0; + auto ToEigen() -> Eigen::Matrix4d; auto ToMatrix() -> std::array, 4>; void Update(const Eigen::VectorXd& update); }; @@ -66,6 +67,9 @@ class JointSolver { auto static Multiply(const std::array, 4>& a, const Eigen::Vector4d& b) -> std::array; + auto static Multiply(const std::array, 4>& a, + const Eigen::Matrix4d& b) -> Eigen::Matrix4d; + auto static Multiply(const Eigen::Matrix& a, const std::array& b) -> std::array; @@ -79,8 +83,7 @@ class JointSolver { double v_residual, int index); auto static CalculateUpdate(const Eigen::MatrixXd& J, - const Eigen::VectorXd& residual, - double lambda) + const Eigen::VectorXd& residual, double lambda) -> Eigen::VectorXd; auto static CalculateResidualLoss( @@ -95,8 +98,6 @@ class JointSolver { camera_detections, std::optional intial_pose = std::nullopt) -> position_estimate_t; - auto CalculateResidual(const Eigen::VectorXd& candidate) -> Eigen::MatrixXd; - auto CalculateJacobian(const Eigen::VectorXd& candidate) -> Eigen::MatrixXd; private: std::unordered_map camera_name_to_index; @@ -105,7 +106,6 @@ class JointSolver { std::vector camera_constant_; std::vector data_points_; - Eigen::Matrix4d field_to_robot_; PositionReceiver position_receiver_; tape_type tape_; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 1ee02078..653fcc60 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -40,11 +40,28 @@ TEST(JointSolveTest, TestJointSolve) { // NOLINT auto detection = detections[0]; auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; + const frc::Pose3d expected_pose = square_solver_solution.pose; const std::map> camera_detections{ {camera_constant.name, {detection}}, }; + // LOG(INFO) << "square_solver_solution\n" + // << square_solver_solution.pose.ToMatrix(); + + square_solver_solution.pose = + square_solver_solution.pose.TransformBy(frc::Transform3d( + frc::Translation3d(units::meter_t{0.1}, units::meter_t{0.1}, + units::meter_t{0}), + frc::Rotation3d(units::radian_t{0}, units::radian_t{0}, + units::radian_t{0}))); + auto joint_solve_solution = joint_solver->EstimatePosition( camera_detections, square_solver_solution.pose); + + ASSERT_LT(joint_solve_solution.loss, 1e-7); + ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( + expected_pose.ToMatrix(), 0.01)); + + // LOG(INFO) << "joint_solve_solution\n" << joint_solve_solution.pose.ToMatrix(); } From d86d9e2c80068830ae911942a050e38dce9ae074 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 02:02:14 +0000 Subject: [PATCH 35/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/joint_solve_test.cc | 106 +++++++++++++++++++------ 1 file changed, 80 insertions(+), 26 deletions(-) diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 653fcc60..4ecebef8 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -3,6 +3,7 @@ #include #include #include "src/localization/joint_solver.h" +#include "src/localization/multi_tag_solver.h" #include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" #include "src/utils/camera_utils.h" @@ -17,6 +18,7 @@ using camera::GetCameraConstants; using camera::timestamped_frame_t; using localization::JointSolver; using localization::kapriltag_layout; +using localization::MultiTagSolver; using localization::OpenCVAprilTagDetector; using localization::position_estimate_t; using localization::SquareSolver; @@ -24,44 +26,96 @@ using localization::tag_detection_t; using utils::CameraMatrixFromJson; using utils::ReadIntrinsics; -TEST(JointSolveTest, TestJointSolve) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); - auto camera_constant = GetCameraConstants().at("second_bot_right"); - auto square_solver = std::make_unique(camera_constant); +// TEST(JointSolveTest, TestJointSolve) { // NOLINT +// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); +// auto camera_constant = GetCameraConstants().at("second_bot_right"); +// auto square_solver = std::make_unique(camera_constant); +// auto joint_solver = +// std::make_unique(std::vector{camera_constant}); +// auto detector = std::make_unique( +// camera_constant.frame_width.value(), camera_constant.frame_height.value(), +// ReadIntrinsics(camera_constant.intrinsics_path.value())); +// +// timestamped_frame_t timestamped_frame{ +// .frame = std::move(image), .timestamp = 0, .invalid = false}; +// auto detections = detector->GetTagDetections(timestamped_frame); +// auto detection = detections[0]; +// +// auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; +// const frc::Pose3d expected_pose = square_solver_solution.pose; +// +// const std::map> camera_detections{ +// {camera_constant.name, {detection}}, +// }; +// +// // LOG(INFO) << "square_solver_solution\n" +// // << square_solver_solution.pose.ToMatrix(); +// +// square_solver_solution.pose = +// square_solver_solution.pose.TransformBy(frc::Transform3d( +// frc::Translation3d(units::meter_t{0.2}, units::meter_t{-0.15}, +// units::meter_t{0.05}), +// frc::Rotation3d(units::radian_t{0.06}, units::radian_t{-0.05}, +// units::radian_t{0.04}))); +// +// auto joint_solve_solution = joint_solver->EstimatePosition( +// camera_detections, square_solver_solution.pose); +// +// ASSERT_LT(joint_solve_solution.loss, 1e-7); +// ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( +// expected_pose.ToMatrix(), 0.01)); +// +// // LOG(INFO) << "joint_solve_solution\n" << joint_solve_solution.pose.ToMatrix(); +// } + +TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT + const auto camera_constant = GetCameraConstants().at("second_bot_right"); + + cv::Mat image_1 = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + cv::Mat image_2 = cv::imread("/bos-logs/log181/right/7.143727.jpg"); + ASSERT_FALSE(image_1.empty()); + ASSERT_FALSE(image_2.empty()); + + auto multi_tag_solver = std::make_unique(camera_constant); auto joint_solver = std::make_unique(std::vector{camera_constant}); auto detector = std::make_unique( camera_constant.frame_width.value(), camera_constant.frame_height.value(), ReadIntrinsics(camera_constant.intrinsics_path.value())); - timestamped_frame_t timestamped_frame{ - .frame = std::move(image), .timestamp = 0, .invalid = false}; - auto detections = detector->GetTagDetections(timestamped_frame); - auto detection = detections[0]; + timestamped_frame_t timestamped_frame_1{ + .frame = std::move(image_1), .timestamp = 0, .invalid = false}; + timestamped_frame_t timestamped_frame_2{ + .frame = std::move(image_2), .timestamp = 0.1, .invalid = false}; + + auto detections_1 = detector->GetTagDetections(timestamped_frame_1); + auto detections_2 = detector->GetTagDetections(timestamped_frame_2); - auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; - const frc::Pose3d expected_pose = square_solver_solution.pose; + ASSERT_FALSE(detections_1.empty()); + ASSERT_FALSE(detections_2.empty()); + + std::vector all_detections = detections_1; + all_detections.insert(all_detections.end(), detections_2.begin(), + detections_2.end()); + + auto multi_tag_solver_solution = + multi_tag_solver->EstimatePosition(all_detections)[0]; const std::map> camera_detections{ - {camera_constant.name, {detection}}, + {camera_constant.name, all_detections}, }; - // LOG(INFO) << "square_solver_solution\n" - // << square_solver_solution.pose.ToMatrix(); - - square_solver_solution.pose = - square_solver_solution.pose.TransformBy(frc::Transform3d( - frc::Translation3d(units::meter_t{0.1}, units::meter_t{0.1}, - units::meter_t{0}), - frc::Rotation3d(units::radian_t{0}, units::radian_t{0}, - units::radian_t{0}))); + multi_tag_solver_solution.pose = + multi_tag_solver_solution.pose.TransformBy(frc::Transform3d( + frc::Translation3d(units::meter_t{0.25}, units::meter_t{-0.2}, + units::meter_t{0.08}), + frc::Rotation3d(units::radian_t{0.08}, units::radian_t{-0.07}, + units::radian_t{0.06}))); auto joint_solve_solution = joint_solver->EstimatePosition( - camera_detections, square_solver_solution.pose); - - ASSERT_LT(joint_solve_solution.loss, 1e-7); - ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( - expected_pose.ToMatrix(), 0.01)); + camera_detections, multi_tag_solver_solution.pose); - // LOG(INFO) << "joint_solve_solution\n" << joint_solve_solution.pose.ToMatrix(); + ASSERT_LT(joint_solve_solution.loss, 5e-3); + ASSERT_FALSE(joint_solve_solution.invalid); + ASSERT_EQ(joint_solve_solution.num_tags, all_detections.size()); } From 447c702b6814324943255a9b9f85cea4fabf839e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 03:21:22 +0000 Subject: [PATCH 36/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/joint_solve_test.cc | 82 +++++++++++++------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 4ecebef8..5fd0e80f 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -26,47 +26,47 @@ using localization::tag_detection_t; using utils::CameraMatrixFromJson; using utils::ReadIntrinsics; -// TEST(JointSolveTest, TestJointSolve) { // NOLINT -// cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); -// auto camera_constant = GetCameraConstants().at("second_bot_right"); -// auto square_solver = std::make_unique(camera_constant); -// auto joint_solver = -// std::make_unique(std::vector{camera_constant}); -// auto detector = std::make_unique( -// camera_constant.frame_width.value(), camera_constant.frame_height.value(), -// ReadIntrinsics(camera_constant.intrinsics_path.value())); -// -// timestamped_frame_t timestamped_frame{ -// .frame = std::move(image), .timestamp = 0, .invalid = false}; -// auto detections = detector->GetTagDetections(timestamped_frame); -// auto detection = detections[0]; -// -// auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; -// const frc::Pose3d expected_pose = square_solver_solution.pose; -// -// const std::map> camera_detections{ -// {camera_constant.name, {detection}}, -// }; -// -// // LOG(INFO) << "square_solver_solution\n" -// // << square_solver_solution.pose.ToMatrix(); -// -// square_solver_solution.pose = -// square_solver_solution.pose.TransformBy(frc::Transform3d( -// frc::Translation3d(units::meter_t{0.2}, units::meter_t{-0.15}, -// units::meter_t{0.05}), -// frc::Rotation3d(units::radian_t{0.06}, units::radian_t{-0.05}, -// units::radian_t{0.04}))); -// -// auto joint_solve_solution = joint_solver->EstimatePosition( -// camera_detections, square_solver_solution.pose); -// -// ASSERT_LT(joint_solve_solution.loss, 1e-7); -// ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( -// expected_pose.ToMatrix(), 0.01)); -// -// // LOG(INFO) << "joint_solve_solution\n" << joint_solve_solution.pose.ToMatrix(); -// } +TEST(JointSolveTest, TestJointSolve) { // NOLINT + cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + auto camera_constant = GetCameraConstants().at("second_bot_right"); + auto square_solver = std::make_unique(camera_constant); + auto joint_solver = + std::make_unique(std::vector{camera_constant}); + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + ReadIntrinsics(camera_constant.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; + auto detections = detector->GetTagDetections(timestamped_frame); + auto detection = detections[0]; + + auto square_solver_solution = square_solver->EstimatePosition({detection})[0]; + const frc::Pose3d expected_pose = square_solver_solution.pose; + + const std::map> camera_detections{ + {camera_constant.name, {detection}}, + }; + + // LOG(INFO) << "square_solver_solution\n" + // << square_solver_solution.pose.ToMatrix(); + + square_solver_solution.pose = + square_solver_solution.pose.TransformBy(frc::Transform3d( + frc::Translation3d(units::meter_t{0.2}, units::meter_t{-0.15}, + units::meter_t{0.05}), + frc::Rotation3d(units::radian_t{0.06}, units::radian_t{-0.05}, + units::radian_t{0.04}))); + + auto joint_solve_solution = joint_solver->EstimatePosition( + camera_detections, square_solver_solution.pose); + + ASSERT_LT(joint_solve_solution.loss, 1e-7); + ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( + expected_pose.ToMatrix(), 0.01)); + + // LOG(INFO) << "joint_solve_solution\n" << joint_solve_solution.pose.ToMatrix(); +} TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT const auto camera_constant = GetCameraConstants().at("second_bot_right"); From 9fc3bc29817b34b0237c1af0a968cdf80ee3e155 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 04:14:10 +0000 Subject: [PATCH 37/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 34 ++++++----- src/localization/joint_solver.h | 2 +- src/test/unit_test/joint_solve_test.cc | 83 ++++++++++++++------------ 3 files changed, 65 insertions(+), 54 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 7779ea13..f94d39b1 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -9,7 +9,7 @@ namespace localization { -#define EPOCHS 200 +#define EPOCHS 1000 using frc::AprilTagFieldLayout; using utils::CameraMatrixFromJson; @@ -235,7 +235,7 @@ void JointSolver::SaveResidual(Eigen::VectorXd& residual, double u_residual, auto JointSolver::CalculateUpdate(const Eigen::MatrixXd& J, const Eigen::VectorXd& residual, double lambda) -> Eigen::VectorXd { - constexpr double kEpsilon = 1e-9; + constexpr double kEpsilon = 1e-12; const Eigen::MatrixXd j_t_j = J.transpose() * J; Eigen::VectorXd diagonal = j_t_j.diagonal(); @@ -280,13 +280,15 @@ auto JointSolver::CalculateResidualLoss( loss += u_residual * u_residual + v_residual * v_residual; } - return loss; + const double residual_count = + static_cast(std::max(1, data_points.size() * 2)); + return loss / residual_count; } JointSolver::JointSolver( const std::vector& camera_constants, const AprilTagFieldLayout& layout) - : camera_constant_(camera_constants) { + : camera_constants_(camera_constants) { for (size_t i = 0; i < camera_constants.size(); i++) { camera_name_to_index[camera_constants[i].name] = i; @@ -332,7 +334,7 @@ auto JointSolver::EstimatePosition( const auto camera_to_robot = robot_to_camera_[index].inverse(); data_points_.push_back(datapoint_t{ .normalized_image_point = - NormalizePoint(corner, camera_constant_[index]), + NormalizePoint(corner, camera_constants_[index]), .normalized_camera_matrix = normalized_camera_matrix, .camera_to_robot = camera_to_robot, .feild_to_tag = field_to_tag, @@ -348,16 +350,17 @@ auto JointSolver::EstimatePosition( differentiable_transform3d_t correction; double checkpoint_loss = CalculateResidualLoss(correction, data_points_); - double lambda = 1e-4; - constexpr double kMinLambda = 1e-12; - constexpr double kMaxLambda = 1e12; - constexpr double kMinUpdateNorm = 1e-14; - constexpr double kMinLossImprovement = 1e-15; - constexpr int kMaxAttemptsPerEpoch = 10; - constexpr int kMaxStaleEpochs = 20; + double lambda = 1e-6; + constexpr double kMinLambda = 1e-16; + constexpr double kMaxLambda = 1e16; + constexpr double kMinUpdateNorm = 1e-18; + constexpr double kMinLossImprovement = 1e-17; + constexpr int kMaxAttemptsPerEpoch = 40; + constexpr int kMaxStaleEpochs = 400; int stale_epochs = 0; for (int epoch = 0; epoch < EPOCHS; epoch++) { + LOG(INFO) << epoch; Eigen::MatrixXd J(data_points_.size() * 2, 6); Eigen::VectorXd residual(data_points_.size() * 2); @@ -407,16 +410,16 @@ auto JointSolver::EstimatePosition( accepted = true; accepted_update = update; best_candidate_loss = candidate_loss; - lambda = std::max(kMinLambda, trial_lambda * 0.25); + lambda = std::max(kMinLambda, trial_lambda * 0.5); break; } - trial_lambda = std::min(kMaxLambda, trial_lambda * 8.0); + trial_lambda = std::min(kMaxLambda, trial_lambda * 2.0); } VLOG(1) << "--------------------------------------"; VLOG(1) << "Residual\n" << residual; - VLOG(1) << "Loss: " << loss; + VLOG(0) << "Loss: " << loss; VLOG(1) << "Checkpoint Loss: " << checkpoint_loss; VLOG(1) << "Candidate Loss: " << best_candidate_loss; VLOG(1) << "lambda: " << lambda; @@ -443,6 +446,7 @@ auto JointSolver::EstimatePosition( break; } } + LOG(INFO) << "correction\n" << correction; Eigen::Matrix4d tmp = (Multiply(correction.ToMatrix(), robot_to_feild)).inverse(); diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index ed65bace..fdac5609 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -103,7 +103,7 @@ class JointSolver { std::unordered_map camera_name_to_index; std::vector normalized_camera_matrix_; std::vector robot_to_camera_; - std::vector camera_constant_; + std::vector camera_constants_; std::vector data_points_; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 5fd0e80f..35eb81f2 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -27,7 +27,7 @@ using utils::CameraMatrixFromJson; using utils::ReadIntrinsics; TEST(JointSolveTest, TestJointSolve) { // NOLINT - cv::Mat image = cv::imread("/bos-logs/log181/right/7.047703.jpg"); + cv::Mat image = cv::imread("/bos-logs/log181/right/15.739774.jpg"); auto camera_constant = GetCameraConstants().at("second_bot_right"); auto square_solver = std::make_unique(camera_constant); auto joint_solver = @@ -48,9 +48,6 @@ TEST(JointSolveTest, TestJointSolve) { // NOLINT {camera_constant.name, {detection}}, }; - // LOG(INFO) << "square_solver_solution\n" - // << square_solver_solution.pose.ToMatrix(); - square_solver_solution.pose = square_solver_solution.pose.TransformBy(frc::Transform3d( frc::Translation3d(units::meter_t{0.2}, units::meter_t{-0.15}, @@ -64,50 +61,60 @@ TEST(JointSolveTest, TestJointSolve) { // NOLINT ASSERT_LT(joint_solve_solution.loss, 1e-7); ASSERT_TRUE(joint_solve_solution.pose.ToMatrix().isApprox( expected_pose.ToMatrix(), 0.01)); - - // LOG(INFO) << "joint_solve_solution\n" << joint_solve_solution.pose.ToMatrix(); } TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT - const auto camera_constant = GetCameraConstants().at("second_bot_right"); - - cv::Mat image_1 = cv::imread("/bos-logs/log181/right/7.047703.jpg"); - cv::Mat image_2 = cv::imread("/bos-logs/log181/right/7.143727.jpg"); - ASSERT_FALSE(image_1.empty()); - ASSERT_FALSE(image_2.empty()); - - auto multi_tag_solver = std::make_unique(camera_constant); - auto joint_solver = - std::make_unique(std::vector{camera_constant}); - auto detector = std::make_unique( - camera_constant.frame_width.value(), camera_constant.frame_height.value(), - ReadIntrinsics(camera_constant.intrinsics_path.value())); - - timestamped_frame_t timestamped_frame_1{ - .frame = std::move(image_1), .timestamp = 0, .invalid = false}; - timestamped_frame_t timestamped_frame_2{ - .frame = std::move(image_2), .timestamp = 0.1, .invalid = false}; - - auto detections_1 = detector->GetTagDetections(timestamped_frame_1); - auto detections_2 = detector->GetTagDetections(timestamped_frame_2); - - ASSERT_FALSE(detections_1.empty()); - ASSERT_FALSE(detections_2.empty()); - - std::vector all_detections = detections_1; - all_detections.insert(all_detections.end(), detections_2.begin(), - detections_2.end()); + const auto camera_constant_right = + GetCameraConstants().at("second_bot_right"); + const auto camera_constant_left = GetCameraConstants().at("second_bot_left"); + + cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.971783.jpg"); + cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); + ASSERT_FALSE(image_right.empty()); + ASSERT_FALSE(image_left.empty()); + + auto multi_tag_solver = + std::make_unique(camera_constant_right); + + auto joint_solver = std::make_unique( + std::vector{camera_constant_right, camera_constant_left}); + auto detector_right = std::make_unique( + camera_constant_right.frame_width.value(), + camera_constant_right.frame_height.value(), + ReadIntrinsics(camera_constant_right.intrinsics_path.value())); + auto detector_left = std::make_unique( + camera_constant_left.frame_width.value(), + camera_constant_left.frame_height.value(), + ReadIntrinsics(camera_constant_left.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame_right{ + .frame = std::move(image_right), .timestamp = 0, .invalid = false}; + timestamped_frame_t timestamped_frame_left{ + .frame = std::move(image_left), .timestamp = 0, .invalid = false}; + + auto detections_right = + detector_right->GetTagDetections(timestamped_frame_right); + auto detections_left = + detector_left->GetTagDetections(timestamped_frame_left); + + ASSERT_FALSE(detections_right.empty()); + ASSERT_FALSE(detections_left.empty()); + + std::vector all_detections = detections_right; + all_detections.insert(all_detections.end(), detections_left.begin(), + detections_left.end()); auto multi_tag_solver_solution = - multi_tag_solver->EstimatePosition(all_detections)[0]; + multi_tag_solver->EstimatePosition(detections_right)[0]; const std::map> camera_detections{ - {camera_constant.name, all_detections}, + {camera_constant_right.name, detections_right}, + {camera_constant_left.name, detections_left}, }; multi_tag_solver_solution.pose = multi_tag_solver_solution.pose.TransformBy(frc::Transform3d( - frc::Translation3d(units::meter_t{0.25}, units::meter_t{-0.2}, + frc::Translation3d(units::meter_t{5.25}, units::meter_t{-0.2}, units::meter_t{0.08}), frc::Rotation3d(units::radian_t{0.08}, units::radian_t{-0.07}, units::radian_t{0.06}))); @@ -115,7 +122,7 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT auto joint_solve_solution = joint_solver->EstimatePosition( camera_detections, multi_tag_solver_solution.pose); - ASSERT_LT(joint_solve_solution.loss, 5e-3); + ASSERT_LT(joint_solve_solution.loss, 1e-2); ASSERT_FALSE(joint_solve_solution.invalid); ASSERT_EQ(joint_solve_solution.num_tags, all_detections.size()); } From 91b9f6ba8bb42a5152884b79a5b25e4d88fed49e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 04:57:41 +0000 Subject: [PATCH 38/47] wip Signed-off-by: Charlie Huang --- constants/second_bot/left_intrinsics.json | 10 +++---- constants/second_bot/right_intrinsics.json | 10 +++---- src/localization/joint_solver.cc | 34 ++++++++++++++++++---- src/localization/joint_solver.h | 6 +++- 4 files changed, 44 insertions(+), 16 deletions(-) diff --git a/constants/second_bot/left_intrinsics.json b/constants/second_bot/left_intrinsics.json index db6395b4..0414268f 100644 --- a/constants/second_bot/left_intrinsics.json +++ b/constants/second_bot/left_intrinsics.json @@ -3,9 +3,9 @@ "cy": 399.3051603994019, "fx": 905.4343172084684, "fy": 905.0217994058659, - "k1": 0.0, - "k2": 0.0, - "k3": 0.0, - "p1": 0.0, - "p2": 0.0 + "k1": 0.04175770867385965, + "k2": -0.07919120051340409, + "k3": 0.014128797448067021, + "p1": -0.0023943526206403067, + "p2": -0.003217715458550516 }% diff --git a/constants/second_bot/right_intrinsics.json b/constants/second_bot/right_intrinsics.json index e9dcb014..672dae0d 100644 --- a/constants/second_bot/right_intrinsics.json +++ b/constants/second_bot/right_intrinsics.json @@ -3,9 +3,9 @@ "cy": 421.9852711165147, "fx": 896.4156102015556, "fy": 896.5943754377341, - "k1": 0.0, - "k2": 0.0, - "k3": 0.0, - "p1": 0.0, - "p2": 0.0 + "k1": 0.037473669453764555, + "k2": -0.08209649993614554, + "k3": 0.02896141051361048, + "p1": 0.00537699080347837, + "p2": 0.00035625234846317763 } diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index f94d39b1..e628d4f2 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -13,6 +13,7 @@ namespace localization { using frc::AprilTagFieldLayout; using utils::CameraMatrixFromJson; +using utils::DistortionCoefficientsFromJson; using utils::ExtrinsicsJsonToCameraToRobot; using utils::ReadExtrinsics; using utils::ReadIntrinsics; @@ -111,14 +112,29 @@ auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, return normalized_points_d.array().square().sum(); } +auto UndistortPixelPoint(const cv::Point2f& distortedPoint, + const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs) + -> cv::Point2f { + std::vector src = {distortedPoint}; + std::vector dst; + cv::undistortPoints(src, dst, cameraMatrix, distCoeffs, cv::noArray(), + cameraMatrix); + return dst[0]; +} + auto JointSolver::NormalizePoint( const cv::Point2d& image_point, - const camera::camera_constant_t& camera_constant) -> Eigen::Vector3d { + const camera::camera_constant_t& camera_constant, + const cv::Mat& camera_matrix, const cv::Mat& distortion_coefficients) + -> Eigen::Vector3d { + + cv::Point2f undistorted_point = + UndistortPixelPoint(image_point, camera_matrix, distortion_coefficients); // clang-format off return (Eigen::Vector3d() << 1, - image_point.x / camera_constant.frame_width.value(), - image_point.y / camera_constant.frame_height.value()) + undistorted_point.x / camera_constant.frame_width.value(), + undistorted_point.y / camera_constant.frame_height.value()) .finished(); // clang-format on } @@ -292,6 +308,13 @@ JointSolver::JointSolver( for (size_t i = 0; i < camera_constants.size(); i++) { camera_name_to_index[camera_constants[i].name] = i; + distortion_coefficients_.emplace_back( + DistortionCoefficientsFromJson( + ReadIntrinsics(camera_constants[i].intrinsics_path.value()))); + + camera_matrix_.emplace_back(CameraMatrixFromJson( + ReadIntrinsics(camera_constants[i].intrinsics_path.value()))); + normalized_camera_matrix_.emplace_back(NormalizeCameraMatrix( CameraMatrixFromJson( ReadIntrinsics(camera_constants[i].intrinsics_path.value())), @@ -333,8 +356,9 @@ auto JointSolver::EstimatePosition( const auto normalized_camera_matrix = normalized_camera_matrix_[index]; const auto camera_to_robot = robot_to_camera_[index].inverse(); data_points_.push_back(datapoint_t{ - .normalized_image_point = - NormalizePoint(corner, camera_constants_[index]), + .normalized_image_point = NormalizePoint( + corner, camera_constants_[index], camera_matrix_[index], + distortion_coefficients_[index]), .normalized_camera_matrix = normalized_camera_matrix, .camera_to_robot = camera_to_robot, .feild_to_tag = field_to_tag, diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index fdac5609..21168a93 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -50,7 +50,9 @@ class JointSolver { int corner_index) -> double; auto static NormalizePoint(const cv::Point2d& image_point, - const camera::camera_constant_t& camera_constant) + const camera::camera_constant_t& camera_constant, + const cv::Mat& camera_matrix, + const cv::Mat& distortion_coefficients) -> Eigen::Vector3d; auto static ProjectPoints(const Eigen::MatrixXd& A, @@ -101,6 +103,8 @@ class JointSolver { private: std::unordered_map camera_name_to_index; + std::vector camera_matrix_; + std::vector distortion_coefficients_; std::vector normalized_camera_matrix_; std::vector robot_to_camera_; std::vector camera_constants_; From 7bb305f814c7c781775be19ddfd2407c6da48ef4 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 05:06:41 +0000 Subject: [PATCH 39/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 49 +++++++++----------------------- src/localization/joint_solver.h | 26 ----------------- 2 files changed, 13 insertions(+), 62 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index e628d4f2..be35e4ff 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -9,8 +9,6 @@ namespace localization { -#define EPOCHS 1000 - using frc::AprilTagFieldLayout; using utils::CameraMatrixFromJson; using utils::DistortionCoefficientsFromJson; @@ -94,24 +92,6 @@ void JointSolver::DifferentiableTransform3d::Update( r_z += update[5]; } -auto JointSolver::CalculateLoss(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, - - int corner_index) -> double { - Eigen::Vector3d projected_point = - camera_matrix * PI * camera_to_robot * robot_to_feild * feild_to_tag * - rotate_yaw * - localization::kapriltag_corners_eigen_homogenized[corner_index]; - auto normalized_point = projected_point / projected_point[0]; - - auto normalized_points_d = normalized_point - image_point; - - return normalized_points_d.array().square().sum(); -} - auto UndistortPixelPoint(const cv::Point2f& distortedPoint, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs) -> cv::Point2f { @@ -122,12 +102,10 @@ auto UndistortPixelPoint(const cv::Point2f& distortedPoint, return dst[0]; } -auto JointSolver::NormalizePoint( - const cv::Point2d& image_point, - const camera::camera_constant_t& camera_constant, - const cv::Mat& camera_matrix, const cv::Mat& distortion_coefficients) - -> Eigen::Vector3d { - +auto NormalizePoint(const cv::Point2d& image_point, + const camera::camera_constant_t& camera_constant, + const cv::Mat& camera_matrix, + const cv::Mat& distortion_coefficients) -> Eigen::Vector3d { cv::Point2f undistorted_point = UndistortPixelPoint(image_point, camera_matrix, distortion_coefficients); // clang-format off @@ -139,18 +117,17 @@ auto JointSolver::NormalizePoint( // clang-format on } -auto JointSolver::ProjectPoints(const Eigen::MatrixXd& A, - const Eigen::MatrixXd& correction, - const Eigen::Vector4d& x) -> Eigen::Vector3d { +auto ProjectPoints(const Eigen::MatrixXd& A, const Eigen::MatrixXd& correction, + const Eigen::Vector4d& x) -> Eigen::Vector3d { Eigen::Vector3d projected_point = A * correction * x; auto normalized_point = projected_point / projected_point[0]; return normalized_point; } -auto JointSolver::NormalizeCameraMatrix( - Eigen::Matrix3d camera_matrix, - const camera::camera_constant_t& camera_constant) -> Eigen::Matrix3d { +auto NormalizeCameraMatrix(Eigen::Matrix3d camera_matrix, + const camera::camera_constant_t& camera_constant) + -> Eigen::Matrix3d { camera_matrix(1, 0) /= camera_constant.frame_width.value(); camera_matrix(2, 0) /= camera_constant.frame_height.value(); @@ -159,7 +136,7 @@ auto JointSolver::NormalizeCameraMatrix( return camera_matrix; } -auto JointSolver::CreateTransformationMatrix(const Eigen::VectorXd& params) +auto CreateTransformationMatrix(const Eigen::VectorXd& params) -> Eigen::Matrix4d { double tx = params[0]; double ty = params[1]; @@ -371,7 +348,7 @@ auto JointSolver::EstimatePosition( } } - differentiable_transform3d_t correction; + differentiable_transform3d_t correction{}; double checkpoint_loss = CalculateResidualLoss(correction, data_points_); double lambda = 1e-6; @@ -381,10 +358,10 @@ auto JointSolver::EstimatePosition( constexpr double kMinLossImprovement = 1e-17; constexpr int kMaxAttemptsPerEpoch = 40; constexpr int kMaxStaleEpochs = 400; + constexpr int kepochs = 100; int stale_epochs = 0; - for (int epoch = 0; epoch < EPOCHS; epoch++) { - LOG(INFO) << epoch; + for (int epoch = 0; epoch < kepochs; epoch++) { Eigen::MatrixXd J(data_points_.size() * 2, 6); Eigen::VectorXd residual(data_points_.size() * 2); diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 21168a93..3fe5f90f 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -8,8 +8,6 @@ #include "src/localization/position_solver.h" #include "src/localization/square_solver.h" -using json = nlohmann::json; - namespace localization { class JointSolver { @@ -42,30 +40,6 @@ class JointSolver { }; public: - auto static CalculateLoss(const Eigen::Matrix4d& robot_to_feild, - const Eigen::Matrix4d& feild_to_tag, - const Eigen::Matrix4d& camera_to_robot, - const Eigen::Matrix3d& camera_matrix, - const Eigen::Vector3d& image_point, - int corner_index) -> double; - - auto static NormalizePoint(const cv::Point2d& image_point, - const camera::camera_constant_t& camera_constant, - const cv::Mat& camera_matrix, - const cv::Mat& distortion_coefficients) - -> Eigen::Vector3d; - - auto static ProjectPoints(const Eigen::MatrixXd& A, - const Eigen::MatrixXd& correction, - const Eigen::Vector4d& x) -> Eigen::Vector3d; - - auto static NormalizeCameraMatrix( - Eigen::Matrix3d camera_matrix, - const camera::camera_constant_t& camera_constant) -> Eigen::Matrix3d; - - auto static CreateTransformationMatrix(const Eigen::VectorXd& params) - -> Eigen::Matrix4d; - auto static Multiply(const std::array, 4>& a, const Eigen::Vector4d& b) -> std::array; From 3fbaa7b64e19be5914bd392ae488d56d300cfa0c Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 05:35:36 +0000 Subject: [PATCH 40/47] wip Signed-off-by: Charlie Huang --- src/localization/CMakeLists.txt | 14 +++- src/localization/joint_solver.cc | 22 +++++- src/localization/joint_solver.h | 3 +- src/localization/levenberg_marquardt.cc | 100 ------------------------ src/localization/levenberg_marquardt.h | 26 ------ 5 files changed, 33 insertions(+), 132 deletions(-) delete mode 100644 src/localization/levenberg_marquardt.cc delete mode 100644 src/localization/levenberg_marquardt.h diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index 2ee11092..cff6a5b7 100644 --- a/src/localization/CMakeLists.txt +++ b/src/localization/CMakeLists.txt @@ -1,4 +1,14 @@ -cmake_minimum_required(VERSION 3.10) +add_library(localization + 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 + unambiguous_estimator.cc + simulation_sender.cc + networktable_sender.cc +) -add_library(localization 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 unambiguous_estimator.cc simulation_sender.cc networktable_sender.cc levenberg_marquardt.cc) target_link_libraries(localization PUBLIC 971apriltag utils camera wpilibc wpiutil vpi absl::status XAD::xad) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index be35e4ff..9e33a805 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -280,8 +280,8 @@ auto JointSolver::CalculateResidualLoss( JointSolver::JointSolver( const std::vector& camera_constants, - const AprilTagFieldLayout& layout) - : camera_constants_(camera_constants) { + AprilTagFieldLayout layout) + : layout_(std::move(layout)), camera_constants_(camera_constants) { for (size_t i = 0; i < camera_constants.size(); i++) { camera_name_to_index[camera_constants[i].name] = i; @@ -347,6 +347,22 @@ auto JointSolver::EstimatePosition( } } } + average_timestamp /= total_detections; + + if (data_points_.size() < 3) { + return position_estimate_t{ + .tag_ids = tag_ids, + .rejected_tag_ids = {}, + .pose = {}, + .variance = 1, + .timestamp = average_timestamp, + .num_tags = total_detections, + .avg_tag_dist = -1, + .latency = -1, + .invalid = true, + .loss = -1, + }; + } differentiable_transform3d_t correction{}; @@ -420,7 +436,7 @@ auto JointSolver::EstimatePosition( VLOG(1) << "--------------------------------------"; VLOG(1) << "Residual\n" << residual; - VLOG(0) << "Loss: " << loss; + VLOG(1) << "Loss: " << loss; VLOG(1) << "Checkpoint Loss: " << checkpoint_loss; VLOG(1) << "Candidate Loss: " << best_candidate_loss; VLOG(1) << "lambda: " << lambda; diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index 3fe5f90f..daa7df5d 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -68,7 +68,7 @@ class JointSolver { public: JointSolver(const std::vector& camera_constants, - const frc::AprilTagFieldLayout& layout = kapriltag_layout); + frc::AprilTagFieldLayout layout = kapriltag_layout); auto EstimatePosition( const std::map>& camera_detections, @@ -77,6 +77,7 @@ class JointSolver { private: std::unordered_map camera_name_to_index; + frc::AprilTagFieldLayout layout_; std::vector camera_matrix_; std::vector distortion_coefficients_; std::vector normalized_camera_matrix_; diff --git a/src/localization/levenberg_marquardt.cc b/src/localization/levenberg_marquardt.cc deleted file mode 100644 index 56ed86a0..00000000 --- a/src/localization/levenberg_marquardt.cc +++ /dev/null @@ -1,100 +0,0 @@ -#include "levenberg_marquardt.h" - -// AI -auto levenberg_marquardt( - Eigen::VectorXd& params, - const std::function& - compute_residual, - const std::function& - compute_jacobian, - double& lambda, double tolerance) -> bool { - const Eigen::VectorXd residual = compute_residual(params); - if (!residual.allFinite()) { - return false; - } - - const double old_loss = residual.squaredNorm(); - - const Eigen::MatrixXd jacobian = compute_jacobian(params); - if (!jacobian.allFinite()) {} - - const Eigen::MatrixXd JTJ = jacobian.transpose() * jacobian; - return false; - Eigen::MatrixXd A = JTJ; - A.diagonal().array() += lambda; - - const Eigen::VectorXd g = jacobian.transpose() * residual; - - Eigen::VectorXd delta = A.ldlt().solve(g); - if (!delta.allFinite()) { - return false; - } - - const Eigen::VectorXd trial_params = params + delta; - const Eigen::VectorXd trial_residual = compute_residual(trial_params); - if (!trial_residual.allFinite()) { - return false; - } - const double new_loss = trial_residual.squaredNorm(); - - if (new_loss <= old_loss) { - params = trial_params; - lambda *= 0.5; - return delta.norm() < tolerance; - } - - lambda *= 2.0; - return false; -} - -auto levenberg_marquardt( - Eigen::VectorXd& params, - const std::function& - compute_residual, - const std::function& - compute_jacobian, - double& lambda, double tolerance) -> bool { - const Eigen::MatrixXd residual_matrix = compute_residual(params); - if (residual_matrix.size() == 0 || !residual_matrix.allFinite()) { - return false; - } - - const Eigen::Map residual(residual_matrix.data(), - residual_matrix.size()); - const double old_loss = residual.squaredNorm(); - - const Eigen::MatrixXd jacobian = compute_jacobian(params); - if (!jacobian.allFinite() || jacobian.rows() != residual.size()) { - return false; - } - - const Eigen::MatrixXd JTJ = jacobian.transpose() * jacobian; - Eigen::MatrixXd A = JTJ; - A.diagonal().array() += lambda; - - const Eigen::VectorXd g = jacobian.transpose() * residual; - - Eigen::VectorXd delta = A.ldlt().solve(g); - if (!delta.allFinite()) { - return false; - } - - const Eigen::VectorXd trial_params = params + delta; - const Eigen::MatrixXd trial_residual_matrix = compute_residual(trial_params); - if (trial_residual_matrix.size() != residual.size() || - !trial_residual_matrix.allFinite()) { - return false; - } - const Eigen::Map trial_residual( - trial_residual_matrix.data(), trial_residual_matrix.size()); - const double new_loss = trial_residual.squaredNorm(); - - if (new_loss <= old_loss) { - params = trial_params; - lambda *= 0.5; - return delta.norm() < tolerance; - } - - lambda *= 2.0; - return false; -} diff --git a/src/localization/levenberg_marquardt.h b/src/localization/levenberg_marquardt.h deleted file mode 100644 index f060e45c..00000000 --- a/src/localization/levenberg_marquardt.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include -#include - -// Performs a single damped Gauss-Newton step using user-provided callbacks to -// compute the residual vector and Jacobian at arbitrary parameter values. The -// trial step is accepted (and lambda halved) when the recomputed residual norm -// does not increase; otherwise lambda is doubled and the parameters remain -// unchanged. Returns true when an accepted step falls below the provided -// tolerance. -auto levenberg_marquardt( - Eigen::VectorXd& params, - const std::function& - compute_residual, - const std::function& - compute_jacobian, - double& lambda, double tolerance = 1e-8) -> bool; - -auto levenberg_marquardt( - Eigen::VectorXd& params, - const std::function& - compute_residual, - const std::function& - compute_jacobian, - double& lambda, double tolerance = 1e-8) -> bool; From 7e942ec407edfad60f449912d3177f82d7d22a9e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 05:36:00 +0000 Subject: [PATCH 41/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 9e33a805..032a013c 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -463,7 +463,6 @@ auto JointSolver::EstimatePosition( break; } } - LOG(INFO) << "correction\n" << correction; Eigen::Matrix4d tmp = (Multiply(correction.ToMatrix(), robot_to_feild)).inverse(); From ca3fb64e6357d4ef68188280e13808b116fc323e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 05:42:46 +0000 Subject: [PATCH 42/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 5 +++-- src/localization/square_solver.cc | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 032a013c..7fc0eab5 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -327,7 +327,7 @@ auto JointSolver::EstimatePosition( int corner_index = 0; for (const auto& corner : tag_detection.corners) { const auto field_to_tag = - kapriltag_layout.GetTagPose(tag_detection.tag_id)->ToMatrix(); + layout_.GetTagPose(tag_detection.tag_id)->ToMatrix(); const auto homogenized_apriltag_corner = localization::kapriltag_corners_eigen_homogenized[corner_index]; const auto normalized_camera_matrix = normalized_camera_matrix_[index]; @@ -347,7 +347,6 @@ auto JointSolver::EstimatePosition( } } } - average_timestamp /= total_detections; if (data_points_.size() < 3) { return position_estimate_t{ @@ -364,6 +363,8 @@ auto JointSolver::EstimatePosition( }; } + average_timestamp /= total_detections; + differentiable_transform3d_t correction{}; double checkpoint_loss = CalculateResidualLoss(correction, data_points_); diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index 3ac00446..3be1496a 100644 --- a/src/localization/square_solver.cc +++ b/src/localization/square_solver.cc @@ -127,7 +127,7 @@ auto SquareSolver::EstimatePosition( try { cv::solvePnP(tag_corners_, detection.corners, camera_matrix_, distortion_coefficients_, rvec, tvec, false, - cv::SOLVEPNP_ITERATIVE); + cv::SOLVEPNP_IPPE_SQUARE); } catch (std::exception& e) { LOG(WARNING) << "Caught solve pnp exception:\n" << e.what(); return {}; From b0fa47c792d591817cd120589d202117faa36a08 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Thu, 14 May 2026 05:58:14 +0000 Subject: [PATCH 43/47] wip Signed-off-by: Charlie Huang --- src/localization/square_solver.cc | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index 3be1496a..d8da439d 100644 --- a/src/localization/square_solver.cc +++ b/src/localization/square_solver.cc @@ -62,19 +62,7 @@ auto SquareSolver::EstimatePositionAmbiguous( cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, distortion_coefficients_, rvecs, tvecs, false, - cv::SOLVEPNP_ITERATIVE); - - cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, - distortion_coefficients_, rvecs, tvecs, true, - cv::SOLVEPNP_ITERATIVE); - - cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, - distortion_coefficients_, rvecs, tvecs, true, - cv::SOLVEPNP_ITERATIVE); - - cv::solvePnPGeneric(tag_corners_, detection.corners, camera_matrix_, - distortion_coefficients_, rvecs, tvecs, true, - cv::SOLVEPNP_ITERATIVE); + cv::SOLVEPNP_IPPE_SQUARE); if (rvecs.size() < 2 || tvecs.size() < 2) { continue; From d4a1b6071f4d3e51bb00047fdbee2c18824654d7 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 15 May 2026 05:05:20 +0000 Subject: [PATCH 44/47] wip Signed-off-by: Charlie Huang --- src/test/unit_test/joint_solve_test.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 35eb81f2..da78eba2 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -112,15 +112,15 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT {camera_constant_left.name, detections_left}, }; - multi_tag_solver_solution.pose = + auto fudged_pose = multi_tag_solver_solution.pose.TransformBy(frc::Transform3d( frc::Translation3d(units::meter_t{5.25}, units::meter_t{-0.2}, units::meter_t{0.08}), frc::Rotation3d(units::radian_t{0.08}, units::radian_t{-0.07}, units::radian_t{0.06}))); - auto joint_solve_solution = joint_solver->EstimatePosition( - camera_detections, multi_tag_solver_solution.pose); + auto joint_solve_solution = + joint_solver->EstimatePosition(camera_detections, fudged_pose); ASSERT_LT(joint_solve_solution.loss, 1e-2); ASSERT_FALSE(joint_solve_solution.invalid); From 6525d681f81e9415212d2813f522c24ba09c806e Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 15 May 2026 05:08:36 +0000 Subject: [PATCH 45/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 2 +- src/test/unit_test/joint_solve_test.cc | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 7fc0eab5..1eb476c7 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -382,13 +382,13 @@ auto JointSolver::EstimatePosition( Eigen::MatrixXd J(data_points_.size() * 2, 6); Eigen::VectorXd residual(data_points_.size() * 2); + tape_.newRecording(); tape_.registerInput(correction.t_x); tape_.registerInput(correction.t_y); tape_.registerInput(correction.t_z); tape_.registerInput(correction.r_x); tape_.registerInput(correction.r_y); tape_.registerInput(correction.r_z); - tape_.newRecording(); int index = 0; for (auto const& data_point : data_points_) { const auto correction_matrix = correction.ToMatrix(); diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index da78eba2..502652b8 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -118,9 +118,12 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT units::meter_t{0.08}), frc::Rotation3d(units::radian_t{0.08}, units::radian_t{-0.07}, units::radian_t{0.06}))); + fudged_pose = multi_tag_solver_solution.pose; auto joint_solve_solution = joint_solver->EstimatePosition(camera_detections, fudged_pose); + LOG(INFO) << joint_solve_solution; + LOG(INFO) << multi_tag_solver_solution; ASSERT_LT(joint_solve_solution.loss, 1e-2); ASSERT_FALSE(joint_solve_solution.invalid); From bddcebbb6792e2dc46b0ece3e4016c07340871e4 Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Fri, 15 May 2026 17:30:00 +0000 Subject: [PATCH 46/47] wip Signed-off-by: Charlie Huang --- src/localization/joint_solver.cc | 6 +-- src/test/unit_test/joint_solve_test.cc | 6 +-- src/test/unit_test/multi_tag_test.cc | 54 +++++++++++++++++++------- 3 files changed, 46 insertions(+), 20 deletions(-) diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 1eb476c7..3ccb62e1 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -375,7 +375,7 @@ auto JointSolver::EstimatePosition( constexpr double kMinLossImprovement = 1e-17; constexpr int kMaxAttemptsPerEpoch = 40; constexpr int kMaxStaleEpochs = 400; - constexpr int kepochs = 100; + constexpr int kepochs = 20; int stale_epochs = 0; for (int epoch = 0; epoch < kepochs; epoch++) { @@ -436,8 +436,8 @@ auto JointSolver::EstimatePosition( } VLOG(1) << "--------------------------------------"; - VLOG(1) << "Residual\n" << residual; - VLOG(1) << "Loss: " << loss; + VLOG(0) << "Residual\n" << residual; + VLOG(0) << "Loss: " << loss; VLOG(1) << "Checkpoint Loss: " << checkpoint_loss; VLOG(1) << "Candidate Loss: " << best_candidate_loss; VLOG(1) << "lambda: " << lambda; diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 502652b8..1816ffdf 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -75,9 +75,9 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT auto multi_tag_solver = std::make_unique(camera_constant_right); - auto joint_solver = std::make_unique( std::vector{camera_constant_right, camera_constant_left}); + auto detector_right = std::make_unique( camera_constant_right.frame_width.value(), camera_constant_right.frame_height.value(), @@ -122,8 +122,8 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT auto joint_solve_solution = joint_solver->EstimatePosition(camera_detections, fudged_pose); - LOG(INFO) << joint_solve_solution; - LOG(INFO) << multi_tag_solver_solution; + LOG(INFO) << "joint\n" << joint_solve_solution; + LOG(INFO) << "multi\n" << multi_tag_solver_solution; ASSERT_LT(joint_solve_solution.loss, 1e-2); ASSERT_FALSE(joint_solve_solution.invalid); diff --git a/src/test/unit_test/multi_tag_test.cc b/src/test/unit_test/multi_tag_test.cc index 1a4d283f..2fc6e328 100644 --- a/src/test/unit_test/multi_tag_test.cc +++ b/src/test/unit_test/multi_tag_test.cc @@ -1,24 +1,50 @@ #include #include "src/localization/multi_tag_solver.h" +#include "src/localization/opencv_apriltag_detector.h" #include "src/localization/square_solver.h" +#include "src/utils/camera_utils.h" +#include "src/utils/constants_from_json.h" + +using camera::camera_constant_t; +using camera::GetCameraConstants; +using camera::timestamped_frame_t; +using localization::kapriltag_layout; +using localization::MultiTagSolver; +using localization::OpenCVAprilTagDetector; +using localization::position_estimate_t; +using localization::SquareSolver; +using localization::tag_detection_t; +using utils::CameraMatrixFromJson; +using utils::ReadIntrinsics; TEST(MultiTagTest, EqualToSquareSolve) { // NOLINT - camera::camera_constant_t camera_constant = - camera::GetCameraConstants().at("dev_orin"); + const auto camera_constant = GetCameraConstants().at("second_bot_right"); + + cv::Mat image = cv::imread("/bos-logs/log181/right/15.739774.jpg"); + + auto multi_tag_solver = std::make_unique(camera_constant); + auto square_solver = std::make_unique(camera_constant); + + auto detector = std::make_unique( + camera_constant.frame_width.value(), camera_constant.frame_height.value(), + ReadIntrinsics(camera_constant.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame{ + .frame = std::move(image), .timestamp = 0, .invalid = false}; - std::vector detections( - {{.tag_id = 25, - .corners = {cv::Point2d(100.0f, 100.0f), cv::Point2d(200.0f, 100.0f), - cv::Point2d(200.0f, 200.0f), cv::Point2d(100.0f, 200.0f)}, - .timestamp = 0.0, - .confidence = 0.0}}); + auto detections = detector->GetTagDetections(timestamped_frame); + auto detection = std::vector{detections[0]}; - localization::SquareSolver square_solver(camera_constant); - localization::MultiTagSolver multi_tag_solver(camera_constant); + position_estimate_t multi_tag_solution = + multi_tag_solver->EstimatePosition(detection)[0]; + position_estimate_t square_solution = + square_solver->EstimatePosition(detection)[0]; - auto square_solver_solution = square_solver.EstimatePosition(detections)[0]; - auto multi_tag_solution = square_solver.EstimatePosition(detections)[0]; + // LOG(INFO) << multi_tag_solution.pose.ToMatrix(); + // LOG(INFO) << square_solution.pose.ToMatrix(); + // LOG(INFO) << multi_tag_solution; + // LOG(INFO) << square_solution; - ASSERT_TRUE(square_solver_solution.pose.ToMatrix() == - multi_tag_solution.pose.ToMatrix()); + ASSERT_TRUE(multi_tag_solution.pose.ToMatrix().isApprox( + square_solution.pose.ToMatrix(), 0.1)); } From 173f7b1fa0b4ea75203caf7ba2fe00add8d4f69b Mon Sep 17 00:00:00 2001 From: Charlie Huang Date: Sat, 16 May 2026 01:26:20 +0000 Subject: [PATCH 47/47] wip Signed-off-by: Charlie Huang --- scripts/build.sh | 1 - src/localization/joint_solver.cc | 136 +++++++++++++++---------- src/localization/joint_solver.h | 23 +++-- src/localization/multi_tag_solver.cc | 15 +++ src/localization/square_solver.cc | 3 + src/test/unit_test/joint_solve_test.cc | 76 ++++++++++---- 6 files changed, 174 insertions(+), 80 deletions(-) diff --git a/scripts/build.sh b/scripts/build.sh index 7c5446f9..c2e6e595 100755 --- a/scripts/build.sh +++ b/scripts/build.sh @@ -24,7 +24,6 @@ fi git submodule update --init --progress --depth 1 if [ "$(pwd)" != "/bos" ]; then mkdir -p /bos - sudo cp -r constants /bos fi cmake -DENABLE_CLANG_TIDY=OFF -DCMAKE_BUILD_TYPE=Release -B "$BUILD_DIR" -G Ninja . cmake --build "$BUILD_DIR" diff --git a/src/localization/joint_solver.cc b/src/localization/joint_solver.cc index 3ccb62e1..81e5bafc 100644 --- a/src/localization/joint_solver.cc +++ b/src/localization/joint_solver.cc @@ -254,13 +254,8 @@ auto JointSolver::CalculateUpdate(const Eigen::MatrixXd& J, } auto JointSolver::CalculateResidualLoss( - const differentiable_transform3d_t& correction, + const Eigen::Matrix4d& correction_matrix, const std::vector& data_points) -> double { - Eigen::VectorXd params(6); - params << value(correction.t_x), value(correction.t_y), value(correction.t_z), - value(correction.r_x), value(correction.r_y), value(correction.r_z); - - const Eigen::Matrix4d correction_matrix = CreateTransformationMatrix(params); double loss = 0.0; for (const auto& data_point : data_points) { @@ -308,47 +303,22 @@ JointSolver::JointSolver( auto JointSolver::EstimatePosition( const std::map>& camera_detections, - std::optional initial_pose) -> position_estimate_t { + std::optional initial_pose_maybe) -> position_estimate_t { std::vector tag_ids; + std::vector data_points; double average_timestamp = 0; - int total_detections = 0; - const Eigen::Matrix4d field_to_robot = - initial_pose.value_or(frc::Pose3d(position_receiver_.Get())).ToMatrix(); + + const auto initial_pose = + initial_pose_maybe.value_or(frc::Pose3d(position_receiver_.Get())); + const Eigen::Matrix4d field_to_robot = initial_pose.ToMatrix(); const Eigen::Matrix4d robot_to_feild = field_to_robot.inverse(); - data_points_.clear(); + CreateDataPoints(initial_pose, camera_detections, data_points, + average_timestamp, tag_ids); - for (const auto& [camera_name, tag_detections] : camera_detections) { - int index = camera_name_to_index[camera_name]; - for (const auto& tag_detection : tag_detections) { - tag_ids.push_back(tag_detection.tag_id); - average_timestamp += tag_detection.timestamp; - total_detections++; - int corner_index = 0; - for (const auto& corner : tag_detection.corners) { - const auto field_to_tag = - layout_.GetTagPose(tag_detection.tag_id)->ToMatrix(); - const auto homogenized_apriltag_corner = - localization::kapriltag_corners_eigen_homogenized[corner_index]; - const auto normalized_camera_matrix = normalized_camera_matrix_[index]; - const auto camera_to_robot = robot_to_camera_[index].inverse(); - data_points_.push_back(datapoint_t{ - .normalized_image_point = NormalizePoint( - corner, camera_constants_[index], camera_matrix_[index], - distortion_coefficients_[index]), - .normalized_camera_matrix = normalized_camera_matrix, - .camera_to_robot = camera_to_robot, - .feild_to_tag = field_to_tag, - .homogenized_apriltag_corner = homogenized_apriltag_corner, - .x = robot_to_feild * field_to_tag * rotate_yaw * - homogenized_apriltag_corner, - .A = normalized_camera_matrix * PI * camera_to_robot}); - corner_index++; - } - } - } + const int total_detections = data_points.size() / 4; - if (data_points_.size() < 3) { + if (data_points.size() < 3) { return position_estimate_t{ .tag_ids = tag_ids, .rejected_tag_ids = {}, @@ -363,11 +333,10 @@ auto JointSolver::EstimatePosition( }; } - average_timestamp /= total_detections; - differentiable_transform3d_t correction{}; - double checkpoint_loss = CalculateResidualLoss(correction, data_points_); + double checkpoint_loss = + CalculateResidualLoss(correction.ToEigen(), data_points); double lambda = 1e-6; constexpr double kMinLambda = 1e-16; constexpr double kMaxLambda = 1e16; @@ -375,12 +344,12 @@ auto JointSolver::EstimatePosition( constexpr double kMinLossImprovement = 1e-17; constexpr int kMaxAttemptsPerEpoch = 40; constexpr int kMaxStaleEpochs = 400; - constexpr int kepochs = 20; + constexpr int kepochs = 100; int stale_epochs = 0; for (int epoch = 0; epoch < kepochs; epoch++) { - Eigen::MatrixXd J(data_points_.size() * 2, 6); - Eigen::VectorXd residual(data_points_.size() * 2); + Eigen::MatrixXd J(data_points.size() * 2, 6); + Eigen::VectorXd residual(data_points.size() * 2); tape_.newRecording(); tape_.registerInput(correction.t_x); @@ -390,7 +359,7 @@ auto JointSolver::EstimatePosition( tape_.registerInput(correction.r_y); tape_.registerInput(correction.r_z); int index = 0; - for (auto const& data_point : data_points_) { + for (auto const& data_point : data_points) { const auto correction_matrix = correction.ToMatrix(); auto a = Multiply(correction_matrix, data_point.x); std::array projected_point = Multiply(data_point.A, a); @@ -422,7 +391,7 @@ auto JointSolver::EstimatePosition( differentiable_transform3d_t candidate = correction; candidate.Update(update); const double candidate_loss = - CalculateResidualLoss(candidate, data_points_); + CalculateResidualLoss(candidate.ToEigen(), data_points); if (candidate_loss < checkpoint_loss) { accepted = true; @@ -436,8 +405,8 @@ auto JointSolver::EstimatePosition( } VLOG(1) << "--------------------------------------"; - VLOG(0) << "Residual\n" << residual; - VLOG(0) << "Loss: " << loss; + VLOG(1) << "Residual\n" << residual; + VLOG(1) << "Loss: " << loss; VLOG(1) << "Checkpoint Loss: " << checkpoint_loss; VLOG(1) << "Candidate Loss: " << best_candidate_loss; VLOG(1) << "lambda: " << lambda; @@ -487,6 +456,71 @@ auto JointSolver::EstimatePosition( }; } +auto JointSolver::CalculateResidualLoss( + frc::Pose3d pose, const std::map>& + camera_detections) const -> double { + std::vector tag_ids; + std::vector data_points; + double average_timestamp = 0; + + CreateDataPoints(pose, camera_detections, data_points, average_timestamp, + tag_ids); + + differentiable_transform3d_t correction{}; + + double checkpoint_loss = + CalculateResidualLoss(correction.ToEigen(), data_points); + + return checkpoint_loss; +} + +void JointSolver::CreateDataPoints( + const frc::Pose3d& initial_pose, + const std::map>& + camera_detections, + std::vector& data_points, double& average_timestamp, + std::vector& tag_ids) const { + + average_timestamp = 0; + const Eigen::Matrix4d field_to_robot = initial_pose.ToMatrix(); + const Eigen::Matrix4d robot_to_feild = field_to_robot.inverse(); + + for (const auto& [camera_name, tag_detections] : camera_detections) { + int index = camera_name_to_index.at(camera_name); + for (const auto& tag_detection : tag_detections) { + tag_ids.push_back(tag_detection.tag_id); + average_timestamp += tag_detection.timestamp; + int corner_index = 0; + for (const auto& corner : tag_detection.corners) { + const auto field_to_tag = + layout_.GetTagPose(tag_detection.tag_id)->ToMatrix(); + const auto homogenized_apriltag_corner = + localization::kapriltag_corners_eigen_homogenized[corner_index]; + const auto normalized_camera_matrix = normalized_camera_matrix_[index]; + const auto camera_to_robot = robot_to_camera_[index].inverse(); + data_points.push_back(datapoint_t{ + .normalized_image_point = NormalizePoint( + corner, camera_constants_[index], camera_matrix_[index], + distortion_coefficients_[index]), + .normalized_camera_matrix = normalized_camera_matrix, + .camera_to_robot = camera_to_robot, + .feild_to_tag = field_to_tag, + .homogenized_apriltag_corner = homogenized_apriltag_corner, + .x = robot_to_feild * field_to_tag * rotate_yaw * + homogenized_apriltag_corner, + .A = normalized_camera_matrix * PI * camera_to_robot}); + corner_index++; + } + } + } + + if (data_points.size() == 0) { + return; + } + + average_timestamp /= data_points.size() / 4; +} + auto operator<<(std::ostream& os, const JointSolver::differentiable_transform3d_t& t) -> std::ostream& { diff --git a/src/localization/joint_solver.h b/src/localization/joint_solver.h index daa7df5d..34dd982d 100644 --- a/src/localization/joint_solver.h +++ b/src/localization/joint_solver.h @@ -62,9 +62,9 @@ class JointSolver { const Eigen::VectorXd& residual, double lambda) -> Eigen::VectorXd; - auto static CalculateResidualLoss( - const differentiable_transform3d_t& correction, - const std::vector& data_points) -> double; + auto static CalculateResidualLoss(const Eigen::Matrix4d& correction_matrix, + const std::vector& data_points) + -> double; public: JointSolver(const std::vector& camera_constants, @@ -72,9 +72,22 @@ class JointSolver { auto EstimatePosition( const std::map>& camera_detections, - std::optional intial_pose = std::nullopt) + std::optional intial_pose_maybe = std::nullopt) -> position_estimate_t; + auto CalculateResidualLoss( + frc::Pose3d pose, + const std::map>& + camera_detections) const -> double; + + private: + void CreateDataPoints( + const frc::Pose3d& initial_pose, + const std::map>& + camera_detections, + std::vector& data_points, double& average_timestamp, + std::vector& tag_ids) const; + private: std::unordered_map camera_name_to_index; frc::AprilTagFieldLayout layout_; @@ -84,8 +97,6 @@ class JointSolver { std::vector robot_to_camera_; std::vector camera_constants_; - std::vector data_points_; - PositionReceiver position_receiver_; tape_type tape_; }; diff --git a/src/localization/multi_tag_solver.cc b/src/localization/multi_tag_solver.cc index 1c5ab9f6..8795878a 100644 --- a/src/localization/multi_tag_solver.cc +++ b/src/localization/multi_tag_solver.cc @@ -4,6 +4,8 @@ #include "src/utils/constants_from_json.h" #include "src/utils/transform.h" +#include + static const cv::Mat zero_vec = (cv::Mat_(3, 1) << 0, 0, 0); namespace localization { @@ -114,6 +116,19 @@ auto MultiTagSolver::EstimatePosition( cv::solvePnP(object_points, image_points, camera_matrix_, distortion_coefficients_, rvec, tvec, false, cv::SOLVEPNP_SQPNP); + + std::vector reprojected_points; + cv::projectPoints(object_points, rvec, tvec, camera_matrix_, + distortion_coefficients_, reprojected_points); + double squared_error_sum = 0.0; + for (size_t i = 0; i < image_points.size(); ++i) { + const cv::Point2d delta = image_points[i] - reprojected_points[i]; + squared_error_sum += delta.dot(delta); + } + const double reprojection_error = + std::sqrt(squared_error_sum / image_points.size()); + LOG(INFO) << "MultiTagSolver reprojection RMSE (px): " + << reprojection_error; } catch (const std::exception& e) { LOG(WARNING) << "Caught solve pnp exception:\n" << e.what(); return {}; diff --git a/src/localization/square_solver.cc b/src/localization/square_solver.cc index d8da439d..bf5a5d2b 100644 --- a/src/localization/square_solver.cc +++ b/src/localization/square_solver.cc @@ -1,4 +1,7 @@ #include "src/localization/square_solver.h" + +#include + #include #include #include "Eigen/Dense" diff --git a/src/test/unit_test/joint_solve_test.cc b/src/test/unit_test/joint_solve_test.cc index 1816ffdf..9df60819 100644 --- a/src/test/unit_test/joint_solve_test.cc +++ b/src/test/unit_test/joint_solve_test.cc @@ -68,13 +68,12 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT GetCameraConstants().at("second_bot_right"); const auto camera_constant_left = GetCameraConstants().at("second_bot_left"); - cv::Mat image_right = cv::imread("/bos-logs/log181/right/20.971783.jpg"); - cv::Mat image_left = cv::imread("/bos-logs/log181/left/20.935361.jpg"); + cv::Mat image_right = cv::imread("/bos-logs/log181/right/11.067757.jpg"); + cv::Mat image_left = cv::imread("/bos-logs/log181/left/11.031403.jpg"); ASSERT_FALSE(image_right.empty()); ASSERT_FALSE(image_left.empty()); - auto multi_tag_solver = - std::make_unique(camera_constant_right); + auto square_solver = std::make_unique(camera_constant_right); auto joint_solver = std::make_unique( std::vector{camera_constant_right, camera_constant_left}); @@ -100,32 +99,65 @@ TEST(JointSolveTest, TestJointSolveMultipleInputImages) { // NOLINT ASSERT_FALSE(detections_right.empty()); ASSERT_FALSE(detections_left.empty()); - std::vector all_detections = detections_right; - all_detections.insert(all_detections.end(), detections_left.begin(), - detections_left.end()); - - auto multi_tag_solver_solution = - multi_tag_solver->EstimatePosition(detections_right)[0]; + auto square_solution_right = + square_solver->EstimatePosition(detections_right)[0]; const std::map> camera_detections{ {camera_constant_right.name, detections_right}, - {camera_constant_left.name, detections_left}, + // {camera_constant_left.name, detections_left}, }; - auto fudged_pose = - multi_tag_solver_solution.pose.TransformBy(frc::Transform3d( - frc::Translation3d(units::meter_t{5.25}, units::meter_t{-0.2}, - units::meter_t{0.08}), - frc::Rotation3d(units::radian_t{0.08}, units::radian_t{-0.07}, - units::radian_t{0.06}))); - fudged_pose = multi_tag_solver_solution.pose; + auto fudged_pose = square_solution_right.pose.TransformBy(frc::Transform3d( + frc::Translation3d(units::meter_t{1.25}, units::meter_t{-0.2}, + units::meter_t{0.98}), + frc::Rotation3d(units::radian_t{0.58}, units::radian_t{-0.07}, + units::radian_t{0.06}))); auto joint_solve_solution = joint_solver->EstimatePosition(camera_detections, fudged_pose); - LOG(INFO) << "joint\n" << joint_solve_solution; - LOG(INFO) << "multi\n" << multi_tag_solver_solution; - ASSERT_LT(joint_solve_solution.loss, 1e-2); + ASSERT_LT(joint_solve_solution.loss, 1e-8); ASSERT_FALSE(joint_solve_solution.invalid); - ASSERT_EQ(joint_solve_solution.num_tags, all_detections.size()); +} + +TEST(JointSolveTest, TestJointSolveLoss) { // NOLINT + const auto camera_constant_right = + GetCameraConstants().at("second_bot_right"); + + cv::Mat image_right = cv::imread("/bos-logs/log181/right/17.979762.jpg"); + ASSERT_FALSE(image_right.empty()); + + auto square_solver = std::make_unique(camera_constant_right); + auto joint_solver = + std::make_unique(std::vector{camera_constant_right}); + + auto detector_right = std::make_unique( + camera_constant_right.frame_width.value(), + camera_constant_right.frame_height.value(), + ReadIntrinsics(camera_constant_right.intrinsics_path.value())); + + timestamped_frame_t timestamped_frame_right{ + .frame = std::move(image_right), .timestamp = 0, .invalid = false}; + + auto detections_right = + detector_right->GetTagDetections(timestamped_frame_right); + + ASSERT_GE(detections_right.size(), 4); + + const auto single_tag_detection = std::vector{detections_right[0]}; + + std::vector square_solutions = + square_solver->EstimatePosition(detections_right); + + ASSERT_FALSE(square_solutions.empty()); + + const std::map> camera_detections{ + {camera_constant_right.name, detections_right}, + }; + + for (const auto& square_solution : square_solutions) { + double square_loss = joint_solver->CalculateResidualLoss( + square_solution.pose, camera_detections); + ASSERT_LE(square_loss, 1e-3); + } }