extract ComputeResidual3D/ComputeJacobians3D, add 3D Jacobian unit test (2/4)

mins/cmake/ROS1.cmake

@@ -154,3 +154,17 @@ else ()
             )
 
 endif ()
+
+# Unit tests (opt-in: cmake -DBUILD_TESTS=ON)
+option(BUILD_TESTS "Build unit tests (requires GTest; or use tests/CMakeLists.txt standalone)" OFF)
+if (BUILD_TESTS)
+    find_package(GTest REQUIRED)
+    enable_testing()
+    file(GLOB MINS_TEST_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/tests/test_*.cpp)
+    foreach(TEST_SOURCE ${MINS_TEST_SOURCES})
+        get_filename_component(TEST_NAME ${TEST_SOURCE} NAME_WE)
+        add_executable(${TEST_NAME} ${TEST_SOURCE})
+        target_link_libraries(${TEST_NAME} mins_lib GTest::gtest_main)
+        add_test(NAME ${TEST_NAME} COMMAND ${TEST_NAME})
+    endforeach()
+endif ()

mins/src/update/wheel/UpdaterWheel.cpp

@@ -326,101 +326,84 @@ void UpdaterWheel::compute_linear_system_2D(MatrixXd &H, VectorXd &res, double t
   }
 }
 
-/**
- * Given a measurement, this will compute the linear system of the new measurements in respect to the state
- * This will return a "small" H, res, and R which are only of a single measurement and sub-set of the state
- */
-void UpdaterWheel::compute_linear_system_3D(MatrixXd &H, VectorXd &res, double time0, double time1) {
+Matrix<double, 6, 1> UpdaterWheel::ComputeResidual3D(const Matrix3d &R_GtoI0, const Vector3d &p_I0inG,
+                                                       const Matrix3d &R_GtoI1, const Vector3d &p_I1inG,
+                                                       const Matrix3d &R_ItoO, const Vector3d &p_IinO,
+                                                       const Matrix3d &R_3D, const Vector3d &p_3D) {
+  Vector3d pOinI = -R_ItoO.transpose() * p_IinO;
+  Matrix3d RO0toO1 = R_ItoO * R_GtoI1 * R_GtoI0.transpose() * R_ItoO.transpose();
+  Matrix<double, 6, 1> res = Matrix<double, 6, 1>::Zero();
+  res.head(3) = -log_so3(R_3D * RO0toO1.transpose());
+  res.tail(3) = p_3D - R_ItoO * R_GtoI0 * (p_I1inG + R_GtoI1.transpose() * pOinI - p_I0inG - R_GtoI0.transpose() * pOinI);
+  return res;
+}
 
-  // Load state values
-  shared_ptr<PoseJPL> pose0 = state->clones.at(time0);
-  shared_ptr<PoseJPL> pose1 = state->clones.at(time1);
-  Vector3d pI0inG = pose0->pos();
-  Vector3d pI1inG = pose1->pos();
-  Matrix3d RGtoI0 = pose0->Rot();
-  Matrix3d RGtoI1 = pose1->Rot();
-  Vector3d pIinO = state->wheel_extrinsic->pos();
-  Matrix3d RItoO = state->wheel_extrinsic->Rot();
-  Vector3d pOinI = -RItoO.transpose() * pIinO;
-  Matrix3d RO0toO1 = RItoO * RGtoI1 * RGtoI0.transpose() * RItoO.transpose();
+pair<MatrixXd, MatrixXd> UpdaterWheel::ComputeJacobians3D(const Matrix3d &R_GtoI0, const Vector3d &p_I0inG,
+                                                            const Matrix3d &R_GtoI1, const Vector3d &p_I1inG,
+                                                            const Matrix3d &R_ItoO, const Vector3d &p_IinO) {
+  Vector3d pOinI = -R_ItoO.transpose() * p_IinO;
+  Matrix3d RO0toO1 = R_ItoO * R_GtoI1 * R_GtoI0.transpose() * R_ItoO.transpose();
   Matrix3d RO1toO0 = RO0toO1.transpose();
 
-  // Compute Orientation and position measurement residual
-  res = Matrix<double, 6, 1>::Zero();
-  // orientation
-  Matrix3d R_est = RO0toO1;
-  res.block(0, 0, 3, 1) = -log_so3(R_3D * R_est.transpose());
-  // position
-  Vector3d p_est = RItoO * RGtoI0 * (pI1inG + RGtoI1.transpose() * pOinI - pI0inG - RGtoI0.transpose() * pOinI);
-  res.block(3, 0, 3, 1) = p_3D - p_est;
+  Matrix3d dzr_dth0 = -R_ItoO * R_GtoI1 * R_GtoI0.transpose();
+  Matrix3d dzr_dth1 = R_ItoO;
+  Matrix3d dzp_dth0 = R_ItoO * skew_x(R_GtoI0 * (p_I1inG + R_GtoI1.transpose() * pOinI - p_I0inG));
+  Matrix3d dzp_dp0 = -R_ItoO * R_GtoI0;
+  Matrix3d dzp_dth1 = -R_ItoO * R_GtoI0 * R_GtoI1.transpose() * skew_x(pOinI);
+  Matrix3d dzp_dp1 = R_ItoO * R_GtoI0;
+
+  MatrixXd H_poses = MatrixXd::Zero(6, 12);
+  H_poses.block(0, 0, 3, 3) = dzr_dth0;
+  H_poses.block(0, 6, 3, 3) = dzr_dth1;
+  H_poses.block(3, 0, 3, 3) = dzp_dth0;
+  H_poses.block(3, 3, 3, 3) = dzp_dp0;
+  H_poses.block(3, 6, 3, 3) = dzp_dth1;
+  H_poses.block(3, 9, 3, 3) = dzp_dp1;
+
+  Matrix3d dzr_dthcalib = Matrix3d::Identity() - RO0toO1;
+  Matrix3d dzp_dthcalib = skew_x(R_ItoO * R_GtoI0 * (p_I1inG - p_I0inG) - RO1toO0 * p_IinO) + RO1toO0 * skew_x(p_IinO);
+  Matrix3d dzp_dpcalib = -RO1toO0 + Matrix3d::Identity();
+
+  MatrixXd H_ext = MatrixXd::Zero(6, 6);
+  H_ext.block(0, 0, 3, 3) = dzr_dthcalib;
+  H_ext.block(3, 0, 3, 3) = dzp_dthcalib;
+  H_ext.block(3, 3, 3, 3) = dzp_dpcalib;
 
-  // Now compute Jacobians!
+  return {H_poses, H_ext};
+}
 
-  // compute the size of the Jacobian
-  int H_size = 12; // Default size for pose of clone 1 and 2
+void UpdaterWheel::compute_linear_system_3D(MatrixXd &H, VectorXd &res, double time0, double time1) {
+  shared_ptr<PoseJPL> pose0 = state->clones.at(time0);
+  shared_ptr<PoseJPL> pose1 = state->clones.at(time1);
+  Matrix3d RItoO = state->wheel_extrinsic->Rot();
+  Vector3d pIinO = state->wheel_extrinsic->pos();
+  // Residual at current state values
+  res = ComputeResidual3D(pose0->Rot(), pose0->pos(), pose1->Rot(), pose1->pos(), RItoO, pIinO, R_3D, p_3D);
+  // Jacobians at FEJ state values
+  int H_size = 12;
   int H_count = 12;
   H_size += (state->op->wheel->do_calib_ext) ? 6 : 0;
   H_size += (state->op->wheel->do_calib_dt) ? 1 : 0;
   H_size += (state->op->wheel->do_calib_int) ? 3 : 0;
   H = MatrixXd::Zero(6, H_size);
-
-  // Overwrite FEJ
-  pI0inG = pose0->pos_fej();
-  pI1inG = pose1->pos_fej();
-  RGtoI0 = pose0->Rot_fej();
-  RGtoI1 = pose1->Rot_fej();
-  RO0toO1 = RItoO * RGtoI1 * RGtoI0.transpose() * RItoO.transpose();
-  RO1toO0 = RO0toO1.transpose();
-
-  // Jacobians in respect to current state
-  // orientation
-  Matrix3d dzr_dth0 = -RItoO * RGtoI1 * RGtoI0.transpose();
-  Matrix3d dzr_dth1 = RItoO;
-  // position
-  Matrix3d dzp_dth0 = RItoO * skew_x(RGtoI0 * pI1inG + RGtoI0 * RGtoI1.transpose() * pOinI - RGtoI0 * pI0inG);
-  Matrix3d dzp_dp0 = -RItoO * RGtoI0;
-  Matrix3d dzp_dth1 = -RItoO * RGtoI0 * RGtoI1.transpose() * skew_x(pOinI);
-  Matrix3d dzp_dp1 = RItoO * RGtoI0;
-
-  // Derivative theta change wrt oldest pose0 and pose1
-  H.block(0, 0, 3, 3) = dzr_dth0;
-  H.block(0, 6, 3, 3) = dzr_dth1;
-  // Derivative position change wrt oldest pose0 and pose1
-  H.block(3, 0, 3, 3) = dzp_dth0;
-  H.block(3, 3, 3, 3) = dzp_dp0;
-  H.block(3, 6, 3, 3) = dzp_dth1;
-  H.block(3, 9, 3, 3) = dzp_dp1;
-
-  // Jacobian wrt wheel to IMU extrinsics
+  const auto [H_poses, H_ext] = ComputeJacobians3D(pose0->Rot_fej(), pose0->pos_fej(), pose1->Rot_fej(), pose1->pos_fej(), RItoO, pIinO);
+  H.block(0, 0, 6, 12) = H_poses;
   if (state->op->wheel->do_calib_ext) {
-    Matrix3d dzr_dthcalib = (Matrix3d::Identity() - RO0toO1);
-    Matrix3d dzp_dpcalib = -RO1toO0 + Matrix3d::Identity();
-    Matrix3d dzp_dthcalib = skew_x(RItoO * RGtoI0 * (pI1inG - pI0inG) - RO1toO0 * pIinO) + RO1toO0 * skew_x(pIinO);
-    H.block(0, H_count, 3, 3) = dzr_dthcalib;
-    H.block(3, H_count, 3, 3) = dzp_dthcalib;
-    H.block(3, H_count + 3, 3, 3) = dzp_dpcalib;
+    H.block(0, H_count, 6, 6) = H_ext;
     H_count += 6;
   }
-
-  // Jacobian wrt wheel timeoffset.
   if (state->op->wheel->do_calib_dt) {
-    // should be able to find imu wv
     assert(state->cpis.find(time0) != state->cpis.end());
     assert(state->cpis.find(time1) != state->cpis.end());
     Vector3d w0 = state->cpis.at(time0).w;
     Vector3d v0 = state->cpis.at(time0).v;
     Vector3d w1 = state->cpis.at(time1).w;
     Vector3d v1 = state->cpis.at(time1).v;
-
-    // Put it in the Jacobian matrix
-    H.block(0, H_count, 3, 1) = dzr_dth0 * w0 + dzr_dth1 * w1;
-    H.block(3, H_count, 3, 1) = dzp_dth0 * w0 + dzp_dp0 * v0 + dzp_dth1 * w1 + dzp_dp1 * v1;
+    H.block(0, H_count, 3, 1) = H_poses.block(0, 0, 3, 3) * w0 + H_poses.block(0, 6, 3, 3) * w1;
+    H.block(3, H_count, 3, 1) = H_poses.block(3, 0, 3, 3) * w0 + H_poses.block(3, 3, 3, 3) * v0 + H_poses.block(3, 6, 3, 3) * w1 + H_poses.block(3, 9, 3, 3) * v1;
     H_count += 1;
   }
-
-  // Jacobian wrt wheel intrinsics.
   if (state->op->wheel->do_calib_int) {
-    // Note they are the opposite sign
     H.block(0, H_count, 3, 3) = -dR_di_3D;
     H.block(3, H_count, 3, 3) = -dp_di_3D;
   }

mins/src/update/wheel/UpdaterWheel.h

@@ -84,6 +84,40 @@ class UpdaterWheel {
                                                                           const Eigen::Matrix3d &R_GtoI1, const Eigen::Vector3d &p_I1inG,
                                                                           const Eigen::Matrix3d &R_ItoO, const Eigen::Vector3d &p_IinO);
 
+  /**
+   * \brief Compute the 6-DOF 3D wheel odometry residual.
+   * \param[in] R_GtoI0 Rotation from global to IMU frame at time 0.
+   * \param[in] p_I0inG Position of IMU at time 0 expressed in global frame.
+   * \param[in] R_GtoI1 Rotation from global to IMU frame at time 1.
+   * \param[in] p_I1inG Position of IMU at time 1 expressed in global frame.
+   * \param[in] R_ItoO Rotation from IMU frame to wheel odometry frame.
+   * \param[in] p_IinO Position of IMU expressed in wheel odometry frame.
+   * \param[in] R_3D Preintegrated 3D orientation from wheel odometry.
+   * \param[in] p_3D Preintegrated 3D position from wheel odometry.
+   * \return Residual 6-vector [orientation_error_3, position_error_3].
+   */
+  static Eigen::Matrix<double, 6, 1> ComputeResidual3D(const Eigen::Matrix3d &R_GtoI0, const Eigen::Vector3d &p_I0inG,
+                                                         const Eigen::Matrix3d &R_GtoI1, const Eigen::Vector3d &p_I1inG,
+                                                         const Eigen::Matrix3d &R_ItoO, const Eigen::Vector3d &p_IinO,
+                                                         const Eigen::Matrix3d &R_3D, const Eigen::Vector3d &p_3D);
+
+  /**
+   * \brief Compute analytical Jacobians of the 3D wheel odometry residual.
+   * Returns Jacobians w.r.t. the two IMU poses (6x12) and the wheel extrinsic (6x6).
+   * Pose columns are ordered [delta_th0(3), delta_p0(3), delta_th1(3), delta_p1(3)].
+   * Extrinsic columns are ordered [delta_thO(3), delta_pO(3)].
+   * \param[in] R_GtoI0 Rotation from global to IMU frame at time 0.
+   * \param[in] p_I0inG Position of IMU at time 0 expressed in global frame.
+   * \param[in] R_GtoI1 Rotation from global to IMU frame at time 1.
+   * \param[in] p_I1inG Position of IMU at time 1 expressed in global frame.
+   * \param[in] R_ItoO Rotation from IMU frame to wheel odometry frame.
+   * \param[in] p_IinO Position of IMU expressed in wheel odometry frame.
+   * \return {H_poses (6x12), H_ext (6x6)} Jacobians w.r.t. IMU poses and extrinsic calibration.
+   */
+  static std::pair<Eigen::MatrixXd, Eigen::MatrixXd> ComputeJacobians3D(const Eigen::Matrix3d &R_GtoI0, const Eigen::Vector3d &p_I0inG,
+                                                                          const Eigen::Matrix3d &R_GtoI1, const Eigen::Vector3d &p_I1inG,
+                                                                          const Eigen::Matrix3d &R_ItoO, const Eigen::Vector3d &p_IinO);
+
 private:
   friend class Initializer;
   friend class IW_Initializer;

mins/tests/test_UpdaterWheel3D.cpp

@@ -0,0 +1,108 @@
+// Numerically verifies UpdaterWheel's 3D analytical Jacobians using central finite differences.
+#include <gtest/gtest.h>
+#include <Eigen/Core>
+#include "update/wheel/UpdaterWheel.h"
+#include "utils/quat_ops.h"
+
+using namespace mins;
+using Eigen::Matrix3d;
+using Eigen::MatrixXd;
+using Eigen::Vector3d;
+
+TEST(WheelJacobian3D, AnalyticalMatchesNumerical) {
+    const double epsilon = 1e-6;
+    const double tolerance = 1e-6;
+
+    Matrix3d R_GtoI0 = ov_core::exp_so3(Vector3d(0.1, -0.2, 0.15));
+    Vector3d p_I0inG(1.0, 0.5, 0.0);
+    Matrix3d R_GtoI1 = ov_core::exp_so3(Vector3d(0.05, -0.15, 0.3));
+    Vector3d p_I1inG(2.0, 1.0, 0.0);
+    Matrix3d R_ItoO = ov_core::exp_so3(Vector3d(0.0, 0.0, 0.2));
+    Vector3d p_IinO(0.1, 0.0, -0.2);
+
+    // Zero-residual nominal measurement.
+    Vector3d pOinI = -R_ItoO.transpose() * p_IinO;
+    Matrix3d R_3D = R_ItoO * R_GtoI1 * R_GtoI0.transpose() * R_ItoO.transpose();
+    Vector3d p_3D = R_ItoO * R_GtoI0 * (p_I1inG + R_GtoI1.transpose() * pOinI - p_I0inG - R_GtoI0.transpose() * pOinI);
+
+    const auto [H_poses, H_ext] = UpdaterWheel::ComputeJacobians3D(R_GtoI0, p_I0inG, R_GtoI1, p_I1inG, R_ItoO, p_IinO);
+
+    // --- H_poses: perturb pose0 (cols 0-5) and pose1 (cols 6-11) ---
+    MatrixXd H_poses_numerical = MatrixXd::Zero(6, 12);
+    for (int i = 0; i < 6; i++) {
+        // pose0
+        Matrix3d R0_plus = R_GtoI0, R0_minus = R_GtoI0;
+        Vector3d p0_plus = p_I0inG, p0_minus = p_I0inG;
+        if (i < 3) {
+            Vector3d axis = Vector3d::Zero();
+            axis(i) = 1.0;
+            R0_plus = ov_core::exp_so3(epsilon * axis) * R_GtoI0;
+            R0_minus = ov_core::exp_so3(-epsilon * axis) * R_GtoI0;
+        } else {
+            p0_plus(i - 3) += epsilon;
+            p0_minus(i - 3) -= epsilon;
+        }
+        Eigen::Matrix<double, 6, 1> res_plus = UpdaterWheel::ComputeResidual3D(R0_plus, p0_plus, R_GtoI1, p_I1inG, R_ItoO, p_IinO, R_3D, p_3D);
+        Eigen::Matrix<double, 6, 1> res_minus = UpdaterWheel::ComputeResidual3D(R0_minus, p0_minus, R_GtoI1, p_I1inG, R_ItoO, p_IinO, R_3D, p_3D);
+        double sign = (i < 3) ? 1.0 : -1.0;
+        H_poses_numerical.col(i) = sign * (res_plus - res_minus) / (2.0 * epsilon);
+
+        // pose1
+        Matrix3d R1_plus = R_GtoI1, R1_minus = R_GtoI1;
+        Vector3d p1_plus = p_I1inG, p1_minus = p_I1inG;
+        if (i < 3) {
+            Vector3d axis = Vector3d::Zero();
+            axis(i) = 1.0;
+            R1_plus = ov_core::exp_so3(epsilon * axis) * R_GtoI1;
+            R1_minus = ov_core::exp_so3(-epsilon * axis) * R_GtoI1;
+        } else {
+            p1_plus(i - 3) += epsilon;
+            p1_minus(i - 3) -= epsilon;
+        }
+        res_plus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R1_plus, p1_plus, R_ItoO, p_IinO, R_3D, p_3D);
+        res_minus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R1_minus, p1_minus, R_ItoO, p_IinO, R_3D, p_3D);
+        H_poses_numerical.col(6 + i) = sign * (res_plus - res_minus) / (2.0 * epsilon);
+    }
+
+    for (int row = 0; row < 6; row++) {
+        for (int col = 0; col < 12; col++) {
+            EXPECT_NEAR(H_poses(row, col), H_poses_numerical(row, col), tolerance)
+                << "H_poses mismatch at row=" << row << " col=" << col;
+            if (std::abs(H_poses_numerical(row, col)) > tolerance) {
+                EXPECT_GT(H_poses(row, col) * H_poses_numerical(row, col), 0.0)
+                    << "H_poses sign mismatch at row=" << row << " col=" << col;
+            }
+        }
+    }
+
+    // --- H_ext: perturb extrinsic (R_ItoO, p_IinO) ---
+    MatrixXd H_ext_numerical = MatrixXd::Zero(6, 6);
+    for (int i = 0; i < 6; i++) {
+        Matrix3d R_plus = R_ItoO, R_minus = R_ItoO;
+        Vector3d p_plus = p_IinO, p_minus = p_IinO;
+        if (i < 3) {
+            Vector3d axis = Vector3d::Zero();
+            axis(i) = 1.0;
+            R_plus = ov_core::exp_so3(epsilon * axis) * R_ItoO;
+            R_minus = ov_core::exp_so3(-epsilon * axis) * R_ItoO;
+        } else {
+            p_plus(i - 3) += epsilon;
+            p_minus(i - 3) -= epsilon;
+        }
+        Eigen::Matrix<double, 6, 1> res_plus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R_GtoI1, p_I1inG, R_plus, p_plus, R_3D, p_3D);
+        Eigen::Matrix<double, 6, 1> res_minus = UpdaterWheel::ComputeResidual3D(R_GtoI0, p_I0inG, R_GtoI1, p_I1inG, R_minus, p_minus, R_3D, p_3D);
+        double sign = (i < 3) ? 1.0 : -1.0;
+        H_ext_numerical.col(i) = sign * (res_plus - res_minus) / (2.0 * epsilon);
+    }
+
+    for (int row = 0; row < 6; row++) {
+        for (int col = 0; col < 6; col++) {
+            EXPECT_NEAR(H_ext(row, col), H_ext_numerical(row, col), tolerance)
+                << "H_ext mismatch at row=" << row << " col=" << col;
+            if (std::abs(H_ext_numerical(row, col)) > tolerance) {
+                EXPECT_GT(H_ext(row, col) * H_ext_numerical(row, col), 0.0)
+                    << "H_ext sign mismatch at row=" << row << " col=" << col;
+            }
+        }
+    }
+}