Add New Joint Solver

constants/dev_orin_extrinsics.json

@@ -0,0 +1,8 @@
+{
+  "translation_x": 0,
+  "translation_y": 0,
+  "translation_z": 0.7493,
+  "rotation_x": 0,
+  "rotation_y": 0,
+  "rotation_z": 0
+}

constants/dev_orin_intrinsics.json

@@ -0,0 +1,12 @@
+{
+    "cx": 625.3426025032783,
+    "cy": 372.4797842477203,
+    "fx": 898.0928868060495,
+    "fy": 897.7157683218877,
+    "k1": 0.04705864839856624,
+    "k2": -0.08074506402566872,
+    "k3": 0.01743537811199019,
+    "p1": 0.001136572471268671,
+    "p2": -0.0003280265291867128
+}
+

src/camera/cv_camera.cc

@@ -5,7 +5,6 @@ namespace camera {
 
 CVCamera::CVCamera(const CameraConstant& c)
     : cap_(cv::VideoCapture(c.pipeline)) {
-
   auto set_if = [&](int prop, const auto& opt) {
     if (opt) {
       cap_.set(prop, *opt);

src/localization/joint_solver.cc

@@ -19,7 +19,7 @@ JointSolver::JointSolver(const std::string& intrinsics_path,
     : layout_(std::move(layout)),
       camera_matrix_(utils::camera_matrix_from_json<Eigen::MatrixXd>(
           utils::read_intrinsics(intrinsics_path))),
-      initial_solver_(utils::read_extrinsics(extrinsics_path)) {}
+      initial_solver_(camera::Camera::DUMMY_CAMERA) {}
 
 JointSolver::JointSolver(camera::Camera camera_config,
                          const frc::AprilTagFieldLayout& layout,

src/test/CMakeLists.txt

@@ -19,5 +19,8 @@ target_link_libraries(gamepiece_test PRIVATE gamepiece yolo camera utils localiz
 add_executable(gamepiece_test_no_img gamepiece_test_no_img.cc)
 target_link_libraries(gamepiece_test_no_img PRIVATE gamepiece yolo camera utils localization)
 
+add_executable(joint_solve_test joint_solver_test.cc)
+target_link_libraries(joint_solve_test localization absl::flags absl::flags_parse utils wpilibc apriltag GTest::gtest GTest::gtest_main)
+
 add_executable(solver_test solver_test.cc)
 target_link_libraries(solver_test absl::flags absl::flags_parse utils localization)

src/test/joint_solver_test.cc

@@ -0,0 +1,119 @@
+#include "src/localization/joint_solver.h"
+#include <gtest/gtest.h>
+#include <opencv2/opencv.hpp>
+#include "src/camera/camera_constants.h"
+#include "src/localization/square_solver.h"
+
+constexpr double ktag_size = 0.1651;
+const frc::AprilTagFieldLayout kapriltag_layout =
+    frc::AprilTagFieldLayout("/bos/constants/2026-rebuilt-andymark.json");
+
+auto GetTagCorners(double tag_size) -> std::vector<cv::Point3f> {
+  double half_size = tag_size / 2.0;
+  return {
+      cv::Point3f(-half_size, half_size, 0.0f),  // top-left
+      cv::Point3f(half_size, half_size, 0.0f),   // top-right
+      cv::Point3f(half_size, -half_size, 0.0f),  // bottom-right
+      cv::Point3f(-half_size, -half_size, 0.0f)  // bottom-left
+  };
+}
+
+TEST(LocalizationTest, AverageSquareSolveVsJointSolve) {
+  auto config = camera::Camera::DUMMY_CAMERA;
+
+  std::vector<cv::Point3f> tag_corners = GetTagCorners(ktag_size);
+
+  localization::SquareSolver square_solver(config);
+  LOG(INFO) << "init square solver";
+
+  localization::JointSolver joint_solver(config);
+  LOG(INFO) << "init joint solver";
+
+  std::vector<localization::tag_detection_t> tag_detections;
+
+  localization::tag_detection_t detection1;
+  detection1.tag_id = 1;
+  detection1.timestamp = 0.0;
+  detection1.corners = {
+      cv::Point2f(100.0f, 100.0f), cv::Point2f(200.0f, 100.0f),
+      cv::Point2f(200.0f, 200.0f), cv::Point2f(100.0f, 200.0f)};
+  tag_detections.push_back(detection1);
+
+  localization::tag_detection_t detection2;
+  detection2.tag_id = 1;
+  detection2.timestamp = 0.0;
+  detection2.corners = {
+      cv::Point2f(100.0f, 100.0f), cv::Point2f(200.0f, 100.0f),
+      cv::Point2f(200.0f, 200.0f), cv::Point2f(100.0f, 200.0f)};
+  tag_detections.push_back(detection2);
+
+  localization::tag_detection_t detection3;
+  detection3.tag_id = 1;
+  detection3.timestamp = 0.0;
+  detection3.corners = {
+      cv::Point2f(100.0f, 100.0f), cv::Point2f(200.0f, 100.0f),
+      cv::Point2f(200.0f, 200.0f), cv::Point2f(100.0f, 200.0f)};
+  tag_detections.push_back(detection3);
+
+  LOG(INFO) << "Testing with " << tag_detections.size()
+            << " detections of the same tag (ID " << detection1.tag_id << ")";
+
+  LOG(INFO) << "Individual tag solving (SquareSolver)";
+  std::vector<localization::position_estimate_t> individual_estimates =
+      square_solver.EstimatePosition(tag_detections);
+
+  ASSERT_FALSE(individual_estimates.empty())
+      << "Square solver returned no estimates";
+
+  for (size_t i = 0; i < individual_estimates.size(); ++i) {
+    LOG(INFO) << "Detection " << i + 1 << " (Tag " << tag_detections[i].tag_id
+              << ") - Position: " << individual_estimates[i];
+  }
+
+  double avg_x = 0.0, avg_y = 0.0, avg_z = 0.0;
+  double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
+  for (const auto& est : individual_estimates) {
+    avg_x += est.pose.X().value();
+    avg_y += est.pose.Y().value();
+    avg_z += est.pose.Z().value();
+    avg_roll += est.pose.Rotation().X().value();
+    avg_pitch += est.pose.Rotation().Y().value();
+    avg_yaw += est.pose.Rotation().Z().value();
+  }
+  avg_x /= individual_estimates.size();
+  avg_y /= individual_estimates.size();
+  avg_z /= individual_estimates.size();
+  avg_roll /= individual_estimates.size();
+  avg_pitch /= individual_estimates.size();
+  avg_yaw /= individual_estimates.size();
+
+  LOG(INFO) << "Average of individual estimates:";
+  LOG(INFO) << "  Position: (" << avg_x << " m, " << avg_y << " m, " << avg_z
+            << " m)";
+  LOG(INFO) << "  Rotation: (" << avg_roll * 180 / M_PI << " deg, "
+            << avg_pitch * 180 / M_PI << " deg, " << avg_yaw * 180 / M_PI
+            << " deg)";
+
+  LOG(INFO) << "Joint tag solving (JointSolver)";
+  auto joint_estimates = joint_solver.EstimatePosition(tag_detections);
+
+  ASSERT_FALSE(joint_estimates.empty()) << "Joint solver returned no estimates";
+
+  const auto& joint_estimate = joint_estimates[0];
+  LOG(INFO) << "Joint estimate: " << joint_estimate;
+
+  double diff_x = joint_estimate.pose.X().value() - avg_x;
+  double diff_y = joint_estimate.pose.Y().value() - avg_y;
+  double diff_z = joint_estimate.pose.Z().value() - avg_z;
+  double diff_magnitude =
+      std::sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
+
+  LOG(INFO) << "Position difference (joint vs avg individual):";
+  LOG(INFO) << "  Delta: (" << diff_x << " m, " << diff_y << " m, " << diff_z
+            << " m)";
+  LOG(INFO) << "  Magnitude: " << diff_magnitude << " m";
+
+  EXPECT_LT(diff_magnitude, 0.001)
+      << "Results differ by more than 1mm. Joint vs averaged individual "
+         "estimates should be essentially identical.";
+}

src/utils/CMakeLists.txt

@@ -1,4 +1,4 @@
-add_library(utils timer.cc nt_utils.cc camera_utils.cc log.cc intrinsics_from_json.cc) 
+add_library(utils timer.cc nt_utils.cc camera_utils.cc log.cc intrinsics_from_json.cc extrinsics_from_json.cc) 
 target_link_libraries(utils PUBLIC wpilibc nlohmann_json::nlohmann_json absl::log absl::check)
 
 if(!ENABLE_CLANG_TIDY)

src/utils/extrinsics_from_json.cc

@@ -0,0 +1,16 @@
+#include "src/utils/extrinsics_from_json.h"
+namespace utils {
+
+auto ExtrinsicsJsonToCameraToRobot(nlohmann::json extrinsics_json)
+    -> frc::Transform3d {
+  frc::Pose3d camera_pose(
+      units::meter_t{extrinsics_json["translation_x"]},
+      units::meter_t{extrinsics_json["translation_y"]},
+      units::meter_t{extrinsics_json["translation_z"]},
+      frc::Rotation3d(units::radian_t{extrinsics_json["rotation_x"]},
+                      units::radian_t{extrinsics_json["rotation_y"]},
+                      units::radian_t{extrinsics_json["rotation_z"]}));
+  frc::Transform3d robot_to_camera(frc::Pose3d(), camera_pose);
+  return robot_to_camera.Inverse();
+}
+}  // namespace utils

src/utils/extrinsics_from_json.h

@@ -0,0 +1,9 @@
+#pragma once
+#include <frc/geometry/Transform3d.h>
+#include "src/utils/pch.h"
+
+namespace utils {
+
+auto ExtrinsicsJsonToCameraToRobot(nlohmann::json extrinsics_json)
+    -> frc::Transform3d;
+}