Add numeric Jacobians

CMakeLists.txt

@@ -157,6 +157,10 @@ if(NOT BUILD_PYTHON_BINDINGS)
   )
 endif()
 
+option(CUNLS_BUILD_NUMERIC_DIFF_E2E_PERF_TEST
+  "Build the numeric-diff end-to-end Minimize() perf sweep (slow: up to 1M-pose/point problems, several minutes)"
+  OFF)
+
 if(BUILD_TESTING)
   FetchContent_Declare(
     googletest
@@ -219,8 +223,15 @@ if(BUILD_TESTING)
     tests/sba_minimizer_test.cpp
     tests/pgo_minimizer_test.cpp
     tests/weighted_factor_batch_test.cpp
+    tests/numeric_diff_jacobian_test.cpp
+    tests/numeric_diff_minimizer_test.cpp
+    tests/numeric_diff_perf_test.cpp
   )
 
+  if(CUNLS_BUILD_NUMERIC_DIFF_E2E_PERF_TEST)
+    target_sources(nls_tests PRIVATE tests/numeric_diff_e2e_perf_test.cpp)
+  endif()
+
   set_target_properties(nls_tests PROPERTIES
     RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin"
   )

README.md

@@ -51,6 +51,7 @@ cuNLS refining two large estimation problems, one Gauss-Newton/LM iteration per
 | **Robust losses** | Huber, Cauchy, Arctan, SoftL1, Tolerant, Tukey, Scaled |
 | **Built-in factors** | Reprojection, PnP, between (SO(2)/SO(3)/SE(2)/SE(3)/Sim(2)/Sim(3)/SL(4)/vector), point-to-point, point-to-plane, symmetric point-to-plane, prior, constant-velocity/constant-acceleration motion priors (SO(2)/SO(3)/SE(2)/SE(3)) |
 | **Custom factors** | User-defined CUDA kernels via `SizedFactorBatch` |
+| **Numeric Jacobians** | Finite-difference Jacobians for any factor batch (manifold-aware, reuses each state's `Plus` retraction), selectable globally (`MinimizerOptions::jacobian_mode`) or per factor group (`Problem::AddFactorBatch`'s override) — write a factor with only a residual and let cuNLS differentiate it; see [Numeric Jacobians](docs/sphinx/numeric_jacobians.rst) |
 | **Linear solver** | Block-sparse PCG (variable block-Jacobi preconditioner, default), NVIDIA cuDSS (optional, loaded via `dlopen()` at runtime — see [Installation](docs/sphinx/installation.rst)), dense LDLT, dense Cholesky (cuSOLVER), dense QR (cuSOLVER) |
 | **Safety checks** | Optional runtime validation (linear-solver diagnostics and more) — disable via `MinimizerOptions::disable_safety_checks` for low-latency solves |
 | **Execution model** | Fully asynchronous via CUDA streams |

cunls/factor/between/se3_between_factor_batch.cu

@@ -110,18 +110,25 @@ __global__ void collect_and_compute_se3_between_error_kernel(float const *const
 // ---------------------------------------------------------------------------
 // Fused kernel: computes BOTH left and right SE3 Jacobians in one pass.
 //
-// Left  Jacobian (cols  0..5): J_left  = -Ad(Delta) * J_l^{-1}(twist)
+// Residual: r = Log(E), E = Delta * T_left^{-1} * T_right. SE3StateBatch::Plus
+// applies a *right* local update (T' = T * Exp(eps)), so for the left pose:
+//   E' = Exp(-Ad(Delta) * eps_l) * E  =>  J_left = -J_l^{-1}(twist) * Ad(Delta)
+// and for the right pose (perturbation appears at the rightmost position of
+// E with no conjugation):
+//   E' = E * Exp(eps_r)               =>  J_right = J_r^{-1}(twist)
+//
+// Left  Jacobian (cols  0..5): J_left  = -J_l^{-1}(twist) * Ad(Delta)
 // Right Jacobian (cols 6..11): J_right =  J_r^{-1}(twist)
 //
 // Cooperative design: 6 threads per factor. Each thread owns one output row
 // of the 6x12 Jacobian. The 6 threads share J_so3[9] and Q[9] through
 // shared memory, so no thread needs to materialize a full 6x6 matrix.
 //
-// Shared memory per factor: twist[6] + J_so3[9] + Q[9] + Jl_inv[36] = 60 floats
-// Per-thread registers: ~6 (jl_row) + 6 (ad_row) + 6 (jr_row) + temps ≈ 40
+// Shared memory per factor: twist[6] + J_so3[9] + Q[9] = 24 floats
+// Per-thread registers: ~6 (jl_row) + 6 (jr_row) + temps ≈ 30
 //
-// The left-Jacobian multiply (-Ad * Jl_inv) requires column access to Jl_inv,
-// so Jl_inv rows are exchanged through shared memory.
+// The left-Jacobian multiply (-Jl_inv * Ad) uses each thread's own Jl_inv row
+// (jl_row, kept in registers) against columns of Ad read from global memory.
 //
 // Right Jacobian uses the identity J_r_inv(xi) = J_l_inv(-xi):
 //   SO(3): J_l_inv(-phi) = J_l_inv(phi)^T  (read J columns as rows from smem)
@@ -276,11 +283,11 @@ __device__ __forceinline__ void compute_Q_full(const float *tw, float *Q) {
 }
 
 // 6 threads per factor. Each thread computes one row of the 6x12 Jacobian.
-// Shared memory per factor: twist[6] + J_so3[9] + Q[9] + Jl_inv[36] = 60 floats
+// Shared memory per factor: twist[6] + J_so3[9] + Q[9] = 24 floats
 constexpr int kThreadsPerFactor = 6;
 constexpr int kFactorsPerBlock = 32;
 constexpr int kJacBlockSize = kFactorsPerBlock * kThreadsPerFactor;  // 192
-constexpr int kSmemPerFactor = 60;
+constexpr int kSmemPerFactor = 24;
 
 __global__ void __launch_bounds__(kJacBlockSize, 5)
     se3_between_fused_jacobians_kernel(const float *__restrict__ residuals,
@@ -293,10 +300,9 @@ __global__ void __launch_bounds__(kJacBlockSize, 5)
   const int global_factor = blockIdx.x * kFactorsPerBlock + local_factor;
 
   float *s_base = smem + local_factor * kSmemPerFactor;
-  float *s_twist = s_base;    // [6]
-  float *s_J = s_base + 6;    // [9]  -- J_so3 (3x3 row-major)
-  float *s_Q = s_base + 15;   // [9]  -- Q (3x3 row-major)
-  float *s_jl = s_base + 24;  // [36] -- full J_l_inv (6x6) for column access
+  float *s_twist = s_base;   // [6]
+  float *s_J = s_base + 6;   // [9]  -- J_so3 (3x3 row-major)
+  float *s_Q = s_base + 15;  // [9]  -- Q (3x3 row-major)
 
   const bool active = global_factor < num_factors;
 
@@ -350,27 +356,29 @@ __global__ void __launch_bounds__(kJacBlockSize, 5)
       jl_row[5] = Jr[2];
     }
   }
-
-#pragma unroll
-  for (int j = 0; j < 6; j++) s_jl[row * 6 + j] = jl_row[j];
   __syncthreads();
 
-  // --- Phase 4: left output = -Ad[row] . Jl_inv (read Jl_inv columns from
-  // smem) ---
+  // --- Phase 4: left output = -Jl_inv[row] . Ad (read Ad columns from
+  // global memory; Jl_inv row is already held in this thread's jl_row) ---
+  //
+  // Residual r = Log(E), E = Delta * T_left^{-1} * T_right. Under the
+  // right-multiplicative retraction T' = T * Exp(eps) (see
+  // SE3StateBatch::Plus), perturbing T_left gives
+  //   E' = Delta * Exp(-eps_l) * T_left^{-1} * T_right
+  //      = Exp(-Ad(Delta) * eps_l) * E
+  // so d r / d eps_l = -J_l^{-1}(r) * Ad(Delta)  (Jl_inv on the LEFT of the
+  // matrix product, Ad(Delta) on the RIGHT) -- not Ad(Delta) * Jl_inv(r).
   constexpr int jac_pitch = 12;
   if (active) {
     float *out = jacobians + global_factor * (6 * jac_pitch) + row * jac_pitch;
 
     const float *ad_src = delta_adjoints[global_factor].data();
-    float ad_row[6];
-#pragma unroll
-    for (int i = 0; i < 6; i++) ad_row[i] = ad_src[row * 6 + i];
 
 #pragma unroll
     for (int j = 0; j < 6; j++) {
       float s = 0.f;
 #pragma unroll
-      for (int k = 0; k < 6; k++) s += ad_row[k] * s_jl[k * 6 + j];
+      for (int k = 0; k < 6; k++) s += jl_row[k] * ad_src[k * 6 + j];
       out[j] = -s;
     }
 

cunls/factor/between/se3_between_factor_batch.h

@@ -37,7 +37,9 @@ namespace cunls {
  * matrix)
  *
  * The Jacobians are computed with respect to both state blocks using the
- * left and right Jacobians of SE(3).
+ * left and right Jacobians of SE(3): J_left = -J_l^{-1}(r) * Ad(Delta),
+ * J_right = J_r^{-1}(r). These follow from SE3StateBatch::Plus applying a
+ * right-multiplicative local update (T' = T * Exp(eps)).
  *
  * @note The pose_deltas pointer must point to GPU device memory and remain
  *       valid for the lifetime of this object. The memory layout is:

cunls/factor/between/so3_between_factor_batch.cu

@@ -123,8 +123,17 @@ __device__ __forceinline__ void so3_jl_inv_row(const float *phi, int r, float *r
 }
 
 // Fused kernel: computes BOTH left and right SO(3) Jacobians in one pass.
-// Left  Jacobian (cols 0..2): -D * J_l^{-1}(r)  where D = Ad(Delta)
-// Right Jacobian (cols 3..5):  J_r^{-1}(r) = J_l^{-1}(-r)
+//
+// Residual: r = Log(E), E = L^T * R * Delta^T  (see
+// collect_and_compute_so3_between_error_kernel). SO3StateBatch::Plus applies
+// a *right* local update (X' = X * Exp(eps)), so for the left pose L:
+//   E' = Exp(-eps_l) * E  =>  d r/d eps_l = -J_l^{-1}(r)         (no D factor)
+// and for the right pose R (perturbation passes through D via the SO(3)
+// adjoint Ad(D) = D):
+//   E' = E * Exp(D * eps_r)  =>  d r/d eps_r = J_r^{-1}(r) * D
+//
+// Left  Jacobian (cols 0..2): -J_l^{-1}(r)
+// Right Jacobian (cols 3..5):  J_r^{-1}(r) * D,  J_r^{-1}(r) = J_l^{-1}(-r)
 // 1 thread per factor, ~25 regs. Replaces 4 separate kernel launches.
 __global__ void __launch_bounds__(256, 4)
     so3_between_fused_jacobians_kernel(const float *__restrict__ residuals,
@@ -139,31 +148,32 @@ __global__ void __launch_bounds__(256, 4)
   const float *D = delta_adjoints[tid].data();
   float *J = jacobians + tid * 18;
 
-  // Compute J_l^{-1}(phi) rows, multiply by -D, write left block (pitch 6)
+  // Left block: -J_l_inv(phi)  (right-perturbation retraction; no D factor)
   float jl[9];
 #pragma unroll
   for (int row = 0; row < 3; ++row) {
     so3_jl_inv_row(phi, row, &jl[row * 3]);
   }
-
-  // Left block: -D * J_l_inv
 #pragma unroll
   for (int row = 0; row < 3; ++row) {
-    float d0 = D[row * 3], d1 = D[row * 3 + 1], d2 = D[row * 3 + 2];
-    J[row * 6 + 0] = -(d0 * jl[0] + d1 * jl[3] + d2 * jl[6]);
-    J[row * 6 + 1] = -(d0 * jl[1] + d1 * jl[4] + d2 * jl[7]);
-    J[row * 6 + 2] = -(d0 * jl[2] + d1 * jl[5] + d2 * jl[8]);
+    J[row * 6 + 0] = -jl[row * 3 + 0];
+    J[row * 6 + 1] = -jl[row * 3 + 1];
+    J[row * 6 + 2] = -jl[row * 3 + 2];
   }
 
-  // Right block: J_r^{-1}(phi) = J_l^{-1}(-phi)
+  // Right block: J_r_inv(phi) * D,  J_r_inv(phi) = J_l_inv(-phi)
   float neg_phi[3] = {-phi[0], -phi[1], -phi[2]};
+  float jr[9];
+#pragma unroll
+  for (int row = 0; row < 3; ++row) {
+    so3_jl_inv_row(neg_phi, row, &jr[row * 3]);
+  }
 #pragma unroll
   for (int row = 0; row < 3; ++row) {
-    float jr[3];
-    so3_jl_inv_row(neg_phi, row, jr);
-    J[row * 6 + 3] = jr[0];
-    J[row * 6 + 4] = jr[1];
-    J[row * 6 + 5] = jr[2];
+    float a0 = jr[row * 3 + 0], a1 = jr[row * 3 + 1], a2 = jr[row * 3 + 2];
+    J[row * 6 + 3] = a0 * D[0] + a1 * D[3] + a2 * D[6];
+    J[row * 6 + 4] = a0 * D[1] + a1 * D[4] + a2 * D[7];
+    J[row * 6 + 5] = a0 * D[2] + a1 * D[5] + a2 * D[8];
   }
 }
 

cunls/factor/between/so3_between_factor_batch.h

@@ -15,10 +15,11 @@ namespace cunls {
 /**
  * @brief Batch factor for SO(3) between constraints (no cuBLAS handle).
  *
- * residual = Log( Delta * R_left^{-1} * R_right )  (3-vector).
+ * residual = Log( R_left^{-1} * R_right * Delta^{-1} )  (3-vector).
  *
- * Jacobians follow the SE(3) between pattern with SO(3) adjoint Ad(R_delta) =
- * R_delta.
+ * Left Jacobian: -J_l^{-1}(r). Right Jacobian: J_r^{-1}(r) * Ad(Delta), with
+ * SO(3) adjoint Ad(R_delta) = R_delta. These follow from SO3StateBatch::Plus
+ * applying a right-multiplicative local update (X' = X * Exp(eps)).
  */
 class SO3BetweenFactorBatch : public SizedFactorBatch<3, 3, 3> {
   using Base = SizedFactorBatch<3, 3, 3>;