Add New Joint Solver

:w

@@ -0,0 +1,238 @@
+#include <apriltag/frc/apriltag/AprilTagFieldLayout.h>
+#include <opencv2/calib3d.hpp>
+#include "src/camera/camera_constants.h"
+#include "src/utils/camera_utils.h"
+#include "src/utils/intrinsics_from_json.h"
+#include "src/utils/log.h"
+#include "src/utils/pch.h"
+
+// https://firstfrc.blob.core.windows.net/frc2026/FieldAssets/2026-apriltag-images-user-guide.pdf
+// Big tag size = close to tag
+// Small tag size = far away from tag
+constexpr double ktag_size = 0.1651;  // meters
+const frc::AprilTagFieldLayout kapriltag_layout =
+    frc::AprilTagFieldLayout("/bos/constants/2026-rebuilt-andymark.json");
+constexpr int kimage_tag_size = 25;
+constexpr int ktag_id = 26;
+constexpr int ktag_offset_y = 0;
+constexpr int ktag_offset_x = -10;
+
+void decomposeMatrix(const cv::Mat& T, cv::Mat& rvec, cv::Mat& tvec) {
+  // Check that the matrix is 4x4
+  if (T.rows != 4 || T.cols != 4) {
+    throw std::runtime_error("Input matrix must be 4x4");
+  }
+
+  // Extract translation vector
+  tvec = (cv::Mat_<double>(3, 1) << T.at<double>(0, 3), T.at<double>(1, 3),
+          T.at<double>(2, 3));
+
+  // Extract rotation matrix
+  cv::Mat R = T(cv::Range(0, 3), cv::Range(0, 3));
+
+  // Convert rotation matrix to rotation vector
+  cv::Rodrigues(R, rvec);
+}
+
+auto cvPoseToEigen(cv::Mat pose) -> Eigen::MatrixX4d {
+  Eigen::Matrix4d eigen;
+  for (int r = 0; r < 4; ++r) {
+    for (int c = 0; c < 4; ++c) {
+      eigen(r, c) = pose.at<double>(r, c);
+    }
+  }
+  return eigen;
+}
+
+auto inverse(cv::Mat& rvec, cv::Mat& tvec) {
+  cv::Mat R;
+  cv::Rodrigues(rvec, R);
+
+  cv::Mat R_inv = R.t();  // transpose = inverse for rotation matrix
+  // Invert the rotation
+
+  // Invert the translation
+  tvec = -R_inv * tvec;
+
+  // Convert back to rvec if needed
+  cv::Mat rvec_inv;
+  cv::Rodrigues(R_inv, rvec);
+}
+
+auto createTransformMatrix(double rx, double ry, double rz, double tx,
+                           double ty, double tz) -> cv::Mat {
+  // --- Rotation matrices ---
+  cv::Mat Rx = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, cos(rx), -sin(rx), 0,
+                sin(rx), cos(rx));
+
+  cv::Mat Ry = (cv::Mat_<double>(3, 3) << cos(ry), 0, sin(ry), 0, 1, 0,
+                -sin(ry), 0, cos(ry));
+
+  cv::Mat Rz = (cv::Mat_<double>(3, 3) << cos(rz), -sin(rz), 0, sin(rz),
+                cos(rz), 0, 0, 0, 1);
+
+  // Combined rotation: Z * Y * X
+  cv::Mat R = Rz * Ry * Rx;
+
+  // --- Translation vector ---
+  cv::Mat t = (cv::Mat_<double>(3, 1) << tx, ty, tz);
+
+  // --- 4x4 Homogeneous transformation matrix ---
+  cv::Mat T = cv::Mat::eye(4, 4, CV_64F);  // initialize identity
+  R.copyTo(T(cv::Rect(0, 0, 3, 3)));       // top-left 3x3 rotation
+  t.copyTo(T(cv::Rect(3, 0, 1, 3)));       // top-right 3x1 translation
+
+  return T;
+}
+
+auto createTransformMatrix(frc::Pose3d transform) -> cv::Mat {
+  return createTransformMatrix(
+      transform.Rotation().Y().to<double>(),
+      transform.Rotation().Z().to<double>(),
+      transform.Rotation().X().to<double>(), transform.Y().to<double>(),
+      transform.Z().to<double>(), transform.X().to<double>());
+}
+
+auto createTransformMatrix(frc::Transform3d transform) -> cv::Mat {
+  return createTransformMatrix(
+      transform.Rotation().Y().to<double>(),
+      transform.Rotation().Z().to<double>(),
+      transform.Rotation().X().to<double>(), transform.Y().to<double>(),
+      transform.Z().to<double>(), transform.X().to<double>());
+}
+
+auto createTransformMatrix(cv::Mat rvec, cv::Mat tvec) -> cv::Mat {
+  return createTransformMatrix(rvec.at<double>(0), rvec.at<double>(1),
+                               rvec.at<double>(2), tvec.at<double>(0),
+                               tvec.at<double>(1), tvec.at<double>(2));
+}
+
+auto main() -> int {
+  std::vector<cv::Point3d> apriltag_corners = {
+      {-ktag_size / 2, ktag_size / 2, 0},
+      {ktag_size / 2, ktag_size / 2, 0},
+      {ktag_size / 2, -ktag_size / 2, 0},
+      {-ktag_size / 2, -ktag_size / 2, 0}};
+
+  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F);  // output rotation vector
+  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F);  // output translation vector
+  auto config = camera::Camera::STOVETOP_BOT_FRONT_RIGHT;
+  auto camera_matrix = utils::camera_matrix_from_json<cv::Mat>(
+      utils::read_intrinsics(camera::camera_constants[config].intrinsics_path));
+
+  auto distortion_coefficients =
+      utils::distortion_coefficients_from_json<cv::Mat>(utils::read_intrinsics(
+          camera::camera_constants[config].intrinsics_path));
+
+  std::vector<cv::Point2d> image_points = {
+      cv::Point2f(1000 - kimage_tag_size + ktag_offset_x,
+                  500 + kimage_tag_size + ktag_offset_y),
+      cv::Point2f(1000 + kimage_tag_size + ktag_offset_x,
+                  500 + kimage_tag_size + ktag_offset_y),
+      cv::Point2f(1000 + kimage_tag_size + ktag_offset_x,
+                  500 - kimage_tag_size + ktag_offset_y),
+      cv::Point2f(1000 - kimage_tag_size + ktag_offset_x,
+                  500 - kimage_tag_size + ktag_offset_y),
+  };
+
+  auto tag_pose = kapriltag_layout.GetTagPose(ktag_id).value().RelativeTo({});
+  auto feild_to_tag = createTransformMatrix(frc::Transform3d({}, tag_pose));
+
+  for (auto& apriltag_corner : apriltag_corners) {
+    (void)apriltag_corner;
+    apriltag_corner.x = -apriltag_corner.x;
+    // apriltag_corner.y = -apriltag_corner.y;
+    //
+    // cv::Mat pt = (cv::Mat_<double>(4, 1) << apriltag_corner.x,
+    //               apriltag_corner.y, apriltag_corner.z, 1.0);
+    // cv::Mat result = feild_to_tag * pt;
+    // apriltag_corner = cv::Point3d(result.at<double>(0), result.at<double>(1),
+    //                               result.at<double>(2));
+  }
+
+  // [886.039, 530.664][1088.88, 521.248][1074.46, 333.045][880.557, 333.923]
+  cv::solvePnP(apriltag_corners, image_points, camera_matrix,
+               distortion_coefficients, rvec, tvec, false,
+               cv::SOLVEPNP_ITERATIVE);
+
+  LOG(INFO) << "\n" << rvec;
+  LOG(INFO) << "\n" << tvec;
+
+  std::vector<cv::Point2d> projectedPoints;
+  cv::projectPoints(apriltag_corners, rvec, tvec, camera_matrix,
+                    distortion_coefficients, projectedPoints);
+
+  // Compute reprojection error
+  double totalError = 0.0;
+  for (size_t i = 0; i < image_points.size(); i++) {
+    double err = cv::norm(image_points[i] - projectedPoints[i]);
+    totalError += err * err;
+  }
+
+  double meanError = std::sqrt(totalError / image_points.size());
+  std::cout << "Mean reprojection error: " << meanError << " pixels"
+            << std::endl;
+
+  {
+      // inverse(rvec, tvec);
+      // const double translation_x = tvec.ptr<double>()[2];
+      // const double translation_y = tvec.ptr<double>()[0];
+      // const double translation_z = tvec.ptr<double>()[1];
+      //
+      // const double rotation_x = rvec.ptr<double>()[2];
+      // const double rotation_y = rvec.ptr<double>()[0];
+      // const double rotation_z = rvec.ptr<double>()[1];
+      // frc::Pose3d camera_pose(
+      //     units::meter_t{translation_x}, units::meter_t{translation_y},
+      //     units::meter_t{translation_z},
+      //     frc::Rotation3d(units::radian_t{rotation_x}, units::radian_t{rotation_y},
+      //                     units::radian_t{rotation_z}));
+      // PrintPose3d(camera_pose);
+  }
+
+  {
+    auto camera_to_tag = createTransformMatrix(rvec, tvec);
+    auto tag_to_camera = camera_to_tag.inv();
+
+    auto tag_pose = kapriltag_layout.GetTagPose(ktag_id).value();
+    auto feild_to_tag = createTransformMatrix(tag_pose);
+    auto tag_to_feild = createTransformMatrix({tag_pose, {}});
+    std::cout << tag_to_feild;
+    return 0;
+
+    auto rotaion_matrix =
+        createTransformMatrix(0, std::numbers::pi, 0, 0, 0, 0);
+    std::cout << "rotation matrix\n";
+    std::cout << rotaion_matrix << std::endl;
+
+    std::cout << "tag to camera\n";
+    std::cout << tag_to_camera << std::endl;
+
+    std::cout << "feild to tag\n";
+    std::cout << feild_to_tag << std::endl;
+
+    auto feild_to_camera = feild_to_tag * tag_to_camera;
+    std::cout << "feild to camera\n";
+    std::cout << feild_to_camera << std::endl;
+
+    decomposeMatrix(feild_to_camera, rvec, tvec);
+
+    const double translation_x = tvec.ptr<double>()[2];
+    const double translation_y = tvec.ptr<double>()[0];
+    const double translation_z = tvec.ptr<double>()[1];
+
+    const double rotation_x = rvec.ptr<double>()[2];
+    const double rotation_y = rvec.ptr<double>()[0];
+    const double rotation_z = rvec.ptr<double>()[1];
+    frc::Pose3d camera_pose(units::meter_t{translation_x},
+                            units::meter_t{-translation_y},
+                            units::meter_t{translation_z},
+                            frc::Rotation3d(units::radian_t{rotation_x},
+                                            units::radian_t{rotation_y},
+                                            units::radian_t{rotation_z}));
+    std::cout << "Camera pose\n";
+    PrintPose3d(camera_pose);
+  }
+
+  return 0;
+}

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
+}
+

constants/stovetop_bot/front_right_intrinsics.json

@@ -1,12 +1,12 @@
 {
-    "cx": 1012.0165186318737,
-    "cy": 509.96268009601044,
-    "fx": 598.8562147696354,
-    "fy": 598.7791606886024,
-    "k1": 0.01265280096846185,
-    "k2": -0.02749231422823394,
-    "k3": 0.004217234226796008,
-    "p1": 6.344713648157099e-05,
-    "p2": -0.0015890075748726767
+    "cx": 1000,
+    "cy": 500,
+    "fx": 598,
+    "fy": 598,
+    "k1": 0,
+    "k2": 0,
+    "k3": 0,
+    "p1": 0,
+    "p2": 0
 }
 

src/camera/camera_constants.h

@@ -56,6 +56,8 @@ enum Camera {
   STOVETOP_BOT_BACK_RIGHT,
   STOVETOP_BOT_BACK_LEFT,
 
+  DEV_ORIN,
+
   DEFAULT_USB0,
   REALSENSE,
   CAMERA_LENGTH,
@@ -186,6 +188,15 @@ inline const camera_constant_t camera_constants[CAMERA_LENGTH] = {
             .intrinsics_path = "/bos/constants/vision_bot_back_left_intrinsics.json",
             .extrinsics_path = "/bos/constants/vision_bot_back_left_extrinsics.json", 
             .name = "vision_bot_back_left"},
+    [Camera::DEV_ORIN] =
+        camera_constant_t{
+            .pipeline = "/dev/v4l/by-path/"
+                        "platform-3610000.usb-usb-0:2.1:1.0-video-index0",
+            .intrinsics_path =
+                "/bos/constants/dev_orin_intrinsics.json",
+            .extrinsics_path =
+                "/bos/constants/dev_orin_extrinsics.json",
+            .name = "dev_orin"},
     [Camera::DEFAULT_USB0] =
         camera_constant_t{
             .pipeline = "platform-3610000.usb-usb-0:2.2:1.0-video-index0",

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/gpu_apriltag_detector.cc

@@ -15,9 +15,9 @@ constexpr auto RadianToDegree(double radian) -> double {
 GPUAprilTagDetector::GPUAprilTagDetector(uint image_width, uint image_height,
                                          const nlohmann::json& intrinsics,
                                          bool verbose)
-    : camera_matrix_(camera_matrix_from_json<cv::Mat>(intrinsics)),
+    : camera_matrix_(utils::camera_matrix_from_json<cv::Mat>(intrinsics)),
       distortion_coefficients_(
-          distortion_coefficients_from_json<cv::Mat>(intrinsics)) {
+          utils::distortion_coefficients_from_json<cv::Mat>(intrinsics)) {
 
   apriltag_detector_ = apriltag_detector_create();
 
@@ -30,8 +30,9 @@ GPUAprilTagDetector::GPUAprilTagDetector(uint image_width, uint image_height,
 
   gpu_detector_ = std::make_unique<frc971::apriltag::GpuDetector>(
       image_width, image_height, apriltag_detector_,
-      camera_matrix_from_json<frc971::apriltag::CameraMatrix>(intrinsics),
-      distortion_coefficients_from_json<frc971::apriltag::DistCoeffs>(
+      utils::camera_matrix_from_json<frc971::apriltag::CameraMatrix>(
+          intrinsics),
+      utils::distortion_coefficients_from_json<frc971::apriltag::DistCoeffs>(
           intrinsics));
 }
 auto GPUAprilTagDetector::GetTagDetections(