diff --git a/CMakeLists.txt b/CMakeLists.txt index 1db7398..9c957fa 100644 --- a/CMakeLists.txt +++ b/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" ) diff --git a/README.md b/README.md index 2a0e9c7..be9cf79 100644 --- a/README.md +++ b/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 | diff --git a/cunls/factor/between/se3_between_factor_batch.cu b/cunls/factor/between/se3_between_factor_batch.cu index e38efbb..4973da7 100644 --- a/cunls/factor/between/se3_between_factor_batch.cu +++ b/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; } diff --git a/cunls/factor/between/se3_between_factor_batch.h b/cunls/factor/between/se3_between_factor_batch.h index 10a6177..7a9e815 100644 --- a/cunls/factor/between/se3_between_factor_batch.h +++ b/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: diff --git a/cunls/factor/between/so3_between_factor_batch.cu b/cunls/factor/between/so3_between_factor_batch.cu index 0499e92..684e215 100644 --- a/cunls/factor/between/so3_between_factor_batch.cu +++ b/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]; } } diff --git a/cunls/factor/between/so3_between_factor_batch.h b/cunls/factor/between/so3_between_factor_batch.h index 26bd881..cd57bfd 100644 --- a/cunls/factor/between/so3_between_factor_batch.h +++ b/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>; diff --git a/cunls/math/so_se_lie_math.cu b/cunls/math/so_se_lie_math.cu index aa2a8a4..be0b8ce 100644 --- a/cunls/math/so_se_lie_math.cu +++ b/cunls/math/so_se_lie_math.cu @@ -26,8 +26,7 @@ namespace cunls { -constexpr size_t block_size = - 256; ///< Default thread block size for CUDA kernels. +constexpr size_t block_size = 256; ///< Default thread block size for CUDA kernels. /** * @brief Device helper to swap two values. @@ -36,7 +35,8 @@ constexpr size_t block_size = * @param a First value (swapped with b). * @param b Second value (swapped with a). */ -template __device__ void swap(T &a, T &b) { +template +__device__ void swap(T &a, T &b) { T temp = a; a = b; b = temp; @@ -51,8 +51,7 @@ template __device__ void swap(T &a, T &b) { * @param ptr Output matrix pointer (3x3, row-major) * @param pitch Pitch (stride between rows) of the output matrix */ -__device__ void compute_skew_matrix(const float *translation, float *ptr, - const size_t pitch) { +__device__ void compute_skew_matrix(const float *translation, float *ptr, const size_t pitch) { ptr[0 * pitch + 0] = 0; ptr[0 * pitch + 1] = -translation[2]; ptr[0 * pitch + 2] = translation[1]; @@ -82,9 +81,8 @@ __device__ void compute_skew_matrix(const float *translation, float *ptr, * @param pitch Pitch (stride between rows) of the output matrix * @param tol Tolerance for small angle approximation */ -__device__ void compute_rodrigues_matrix(const float *phi, float k1, float k2, - float k3, float k4, float *ptr, - const size_t pitch, float tol = 1e-5) { +__device__ void compute_rodrigues_matrix(const float *phi, float k1, float k2, float k3, float k4, + float *ptr, const size_t pitch, float tol = 1e-5) { float theta = norm3df(phi[0], phi[1], phi[2]); assert(theta >= 0); if (theta < tol) { @@ -134,8 +132,7 @@ __device__ void compute_rodrigues_matrix(const float *phi, float k1, float k2, * @param ptr Output rotation matrix pointer (3x3, row-major) * @param pitch Pitch (stride between rows) of the output matrix */ -__device__ void compute_exp_so3(const float *phi, float *ptr, - const size_t pitch) { +__device__ void compute_exp_so3(const float *phi, float *ptr, const size_t pitch) { float theta = norm3df(phi[0], phi[1], phi[2]); float theta_squared = powf(theta, 2); @@ -155,8 +152,7 @@ __device__ void compute_exp_so3(const float *phi, float *ptr, * @param ptr Output Jacobian matrix pointer (3x3, row-major) * @param pitch Pitch (stride between rows) of the output matrix */ -__device__ void compute_so3_jacobian_left(const float *phi, float *ptr, - const size_t pitch) { +__device__ void compute_so3_jacobian_left(const float *phi, float *ptr, const size_t pitch) { float theta = norm3df(phi[0], phi[1], phi[2]); float theta_squared = powf(theta, 2); @@ -199,9 +195,8 @@ __device__ void compute_so3_jacobian_left_inverse(const float *phi, float *ptr, * @param twist Output 3D twist vector * @param tol Tolerance for detecting identity rotation */ -__device__ void compute_log_so3(const float *rotation_matrix, - const size_t rotation_pitch, float *twist, - float tol = 1e-5) { +__device__ void compute_log_so3(const float *rotation_matrix, const size_t rotation_pitch, + float *twist, float tol = 1e-5) { float trace = 0; #pragma unroll for (int i = 0; i < 3; i++) { @@ -238,10 +233,8 @@ __device__ void compute_log_so3(const float *rotation_matrix, // Read only the chosen column of (R + I) and normalize float v0 = rotation_matrix[best] + ((best == 0) ? 1.0f : 0.0f); - float v1 = - rotation_matrix[rotation_pitch + best] + ((best == 1) ? 1.0f : 0.0f); - float v2 = rotation_matrix[2 * rotation_pitch + best] + - ((best == 2) ? 1.0f : 0.0f); + float v1 = rotation_matrix[rotation_pitch + best] + ((best == 1) ? 1.0f : 0.0f); + float v2 = rotation_matrix[2 * rotation_pitch + best] + ((best == 2) ? 1.0f : 0.0f); float sq = v0 * v0 + v1 * v1 + v2 * v2; if (sq > 0.0f) { float scale = theta * __frsqrt_rn(sq); @@ -254,12 +247,12 @@ __device__ void compute_log_so3(const float *rotation_matrix, float k = (0.5f * theta) / sin_theta; - twist[0] = k * (rotation_matrix[2 * rotation_pitch + 1] - - rotation_matrix[1 * rotation_pitch + 2]); - twist[1] = k * (rotation_matrix[0 * rotation_pitch + 2] - - rotation_matrix[2 * rotation_pitch + 0]); - twist[2] = k * (rotation_matrix[1 * rotation_pitch + 0] - - rotation_matrix[0 * rotation_pitch + 1]); + twist[0] = + k * (rotation_matrix[2 * rotation_pitch + 1] - rotation_matrix[1 * rotation_pitch + 2]); + twist[1] = + k * (rotation_matrix[0 * rotation_pitch + 2] - rotation_matrix[2 * rotation_pitch + 0]); + twist[2] = + k * (rotation_matrix[1 * rotation_pitch + 0] - rotation_matrix[0 * rotation_pitch + 1]); } /** @@ -277,8 +270,8 @@ __device__ void matmul_3x3(const float *A, const float *B, float *C) { for (uint8_t i = 0; i < 3; i++) { #pragma unroll for (uint8_t j = 0; j < 3; j++) { - C[i * 3 + j] = A[i * 3 + 0] * B[0 * 3 + j] + A[i * 3 + 1] * B[1 * 3 + j] + - A[i * 3 + 2] * B[2 * 3 + j]; + C[i * 3 + j] = + A[i * 3 + 0] * B[0 * 3 + j] + A[i * 3 + 1] * B[1 * 3 + j] + A[i * 3 + 2] * B[2 * 3 + j]; } } } @@ -306,15 +299,13 @@ __device__ void scale_add_3x3(const float *A, float scale, float *B) { * @param scale Scalar multiplier applied to the product. * @param C Output matrix (3x3, contiguous row-major), accumulated in-place. */ -__device__ void matmul_add_3x3(const float *A, const float *B, float scale, - float *C) { +__device__ void matmul_add_3x3(const float *A, const float *B, float scale, float *C) { #pragma unroll for (uint8_t i = 0; i < 3; i++) { #pragma unroll for (uint8_t j = 0; j < 3; j++) { - C[i * 3 + j] += - scale * (A[i * 3 + 0] * B[0 * 3 + j] + A[i * 3 + 1] * B[1 * 3 + j] + - A[i * 3 + 2] * B[2 * 3 + j]); + C[i * 3 + j] += scale * (A[i * 3 + 0] * B[0 * 3 + j] + A[i * 3 + 1] * B[1 * 3 + j] + + A[i * 3 + 2] * B[2 * 3 + j]); } } } @@ -332,8 +323,8 @@ __device__ void matmul_add_3x3(const float *A, const float *B, float scale, * @param Q Output Q matrix pointer (3x3, row-major) * @param tol Tolerance for small angle approximation */ -__device__ void compute_Q_left(const float *twist, const size_t Q_pitch, - float *Q, float tol = 1e-5) { +__device__ void compute_Q_left(const float *twist, const size_t Q_pitch, float *Q, + float tol = 1e-5) { float phi = norm3df(twist[0], twist[1], twist[2]); float A = 1.f / 6.f; @@ -404,9 +395,8 @@ __device__ void compute_Q_left(const float *twist, const size_t Q_pitch, * @param skew_stride Stride between skew matrices * @param size Number of twists to process */ -__global__ void skew_so3_kernel(const float *twist, const size_t twist_stride, - float *skew, const size_t skew_pitch, - const size_t skew_stride, size_t size) { +__global__ void skew_so3_kernel(const float *twist, const size_t twist_stride, float *skew, + const size_t skew_pitch, const size_t skew_stride, size_t size) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { return; @@ -442,9 +432,8 @@ __global__ void skew_so3_kernel(const float *twist, const size_t twist_stride, * @param exp_stride Stride between consecutive rotation matrices. * @param size Number of twist vectors to process. */ -__global__ void exp_so3_kernel(const float *twist, const size_t twist_stride, - float *exp, const size_t exp_pitch, - const size_t exp_stride, size_t size) { +__global__ void exp_so3_kernel(const float *twist, const size_t twist_stride, float *exp, + const size_t exp_pitch, const size_t exp_stride, size_t size) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { return; @@ -469,10 +458,8 @@ __global__ void exp_so3_kernel(const float *twist, const size_t twist_stride, * @param size Number of rotation matrices to process. * @param twist Output twist vectors (3D, device pointer). */ -__global__ void log_so3_kernel(const float *rotation_matrix, - const size_t rotation_pitch, - const size_t rotation_stride, - const size_t twist_stride, size_t size, +__global__ void log_so3_kernel(const float *rotation_matrix, const size_t rotation_pitch, + const size_t rotation_stride, const size_t twist_stride, size_t size, float *twist) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { @@ -500,9 +487,9 @@ __global__ void log_so3_kernel(const float *rotation_matrix, * @param transform_stride Stride between consecutive transform matrices. * @param size Number of twist vectors to process. */ -__global__ void exp_se3_kernel(const float *twist, const size_t twist_stride, - float *transform, const size_t transform_pitch, - const size_t transform_stride, size_t size) { +__global__ void exp_se3_kernel(const float *twist, const size_t twist_stride, float *transform, + const size_t transform_pitch, const size_t transform_stride, + size_t size) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { return; @@ -519,7 +506,7 @@ __global__ void exp_se3_kernel(const float *twist, const size_t twist_stride, for (int i = 12; i < 15; i++) { update[i] = 0; } - update[15] = 1; // set last row to [0, 0, 0, 1] + update[15] = 1; // set last row to [0, 0, 0, 1] const size_t update_pitch = 4; @@ -561,9 +548,8 @@ __global__ void exp_se3_kernel(const float *twist, const size_t twist_stride, * @param jacobian_stride Stride between consecutive Jacobian matrices. * @param size Number of twist vectors to process. */ -__global__ void jacobian_so3_kernel(bool left, const float *twist, - const size_t twist_stride, float *jacobian, - const size_t jacobian_pitch, +__global__ void jacobian_so3_kernel(bool left, const float *twist, const size_t twist_stride, + float *jacobian, const size_t jacobian_pitch, const size_t jacobian_stride, size_t size) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { @@ -604,9 +590,8 @@ __global__ void jacobian_so3_kernel(bool left, const float *twist, * @param size Number of twist vectors to process. */ __global__ void __launch_bounds__(256, 4) - jacobian_inverse_so3_kernel(bool left, const float *twist, - const size_t twist_stride, float *jacobian_inv, - const size_t jacobian_inv_pitch, + jacobian_inverse_so3_kernel(bool left, const float *twist, const size_t twist_stride, + float *jacobian_inv, const size_t jacobian_inv_pitch, const size_t jacobian_inv_stride, size_t size) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { @@ -641,9 +626,9 @@ __global__ void __launch_bounds__(256, 4) * @param dst_matrix Output negated matrices (device pointer). */ __global__ void negate_matrices_kernel(const size_t rows, const size_t cols, - const float *src_matrix, - const size_t pitch, const size_t stride, - size_t num_matrices, float *dst_matrix) { + const float *src_matrix, const size_t pitch, + const size_t stride, size_t num_matrices, + float *dst_matrix) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= num_matrices) { return; @@ -676,11 +661,9 @@ __global__ void negate_matrices_kernel(const size_t rows, const size_t cols, * @param size Number of transforms to process. * @param twist Output twist vectors (6D, device pointer). */ -__global__ void log_se3_kernel(const float *transform, - const size_t transform_pitch, - const size_t transform_stride, - const size_t twist_stride, size_t size, - float *twist) { +__global__ void log_se3_kernel(const float *transform, const size_t transform_pitch, + const size_t transform_stride, const size_t twist_stride, + size_t size, float *twist) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { return; @@ -700,14 +683,11 @@ __global__ void log_se3_kernel(const float *transform, float J_inv[9]; compute_so3_jacobian_left_inverse(twist_se3, J_inv, 3); - twist_se3[3] = J_inv[0 * 3 + 0] * translation[0] + - J_inv[0 * 3 + 1] * translation[1] + + twist_se3[3] = J_inv[0 * 3 + 0] * translation[0] + J_inv[0 * 3 + 1] * translation[1] + J_inv[0 * 3 + 2] * translation[2]; - twist_se3[4] = J_inv[1 * 3 + 0] * translation[0] + - J_inv[1 * 3 + 1] * translation[1] + + twist_se3[4] = J_inv[1 * 3 + 0] * translation[0] + J_inv[1 * 3 + 1] * translation[1] + J_inv[1 * 3 + 2] * translation[2]; - twist_se3[5] = J_inv[2 * 3 + 0] * translation[0] + - J_inv[2 * 3 + 1] * translation[1] + + twist_se3[5] = J_inv[2 * 3 + 0] * translation[0] + J_inv[2 * 3 + 1] * translation[1] + J_inv[2 * 3 + 2] * translation[2]; memcpy(twist_ptr, twist_se3, 6 * sizeof(float)); @@ -729,11 +709,9 @@ __global__ void log_se3_kernel(const float *transform, * @param adjoint Output adjoint matrices (6x6, device pointer). */ __global__ void adjoint_se3_kernel(bool inverse, const float *transform, - const size_t transform_pitch, - const size_t transform_stride, - const size_t adjoint_pitch, - const size_t adjoint_stride, size_t size, - float *adjoint) { + const size_t transform_pitch, const size_t transform_stride, + const size_t adjoint_pitch, const size_t adjoint_stride, + size_t size, float *adjoint) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { return; @@ -742,12 +720,10 @@ __global__ void adjoint_se3_kernel(bool inverse, const float *transform, float *adjoint_ptr = adjoint + tid * adjoint_stride; const float *transform_ptr = transform + tid * transform_stride; - float k = inverse ? -1.0f : 1.0f; - float translation[3]; - translation[0] = k * transform_ptr[0 * transform_pitch + 3]; - translation[1] = k * transform_ptr[1 * transform_pitch + 3]; - translation[2] = k * transform_ptr[2 * transform_pitch + 3]; + translation[0] = transform_ptr[0 * transform_pitch + 3]; + translation[1] = transform_ptr[1 * transform_pitch + 3]; + translation[2] = transform_ptr[2 * transform_pitch + 3]; float R[9]; #pragma unroll @@ -757,21 +733,31 @@ __global__ void adjoint_se3_kernel(bool inverse, const float *transform, memcpy(dst, src, 3 * sizeof(float)); } + // For Ad(T) with T = (R, t): Ad(T) = [[R, 0], [skew(t) * R, R]]. + // For Ad(T^{-1}) = Ad(T)^{-1}, use R' = R^T and t' = -R^T * t (the + // rotation/translation of T^{-1}), then the same block formula applies: + // Ad(T^{-1}) = [[R', 0], [skew(t') * R', R']]. if (inverse) { - // transpose R + // transpose R in place -> R' swap(R[0 * 3 + 1], R[1 * 3 + 0]); swap(R[0 * 3 + 2], R[2 * 3 + 0]); swap(R[1 * 3 + 2], R[2 * 3 + 1]); + + // t' = -R' * t = -R^T * t + float t0 = translation[0], t1 = translation[1], t2 = translation[2]; + translation[0] = -(R[0] * t0 + R[1] * t1 + R[2] * t2); + translation[1] = -(R[3] * t0 + R[4] * t1 + R[5] * t2); + translation[2] = -(R[6] * t0 + R[7] * t1 + R[8] * t2); } #pragma unroll for (uint8_t i = 0; i < 3; i++) { - // adjoint[0:3, 0:3] = R + // adjoint[0:3, 0:3] = R (or R' for the inverse) float *src = &R[i * 3]; float *dst = &adjoint_ptr[i * adjoint_pitch]; memcpy(dst, src, 3 * sizeof(float)); - // adjoint[3:6, 3:6] = R + // adjoint[3:6, 3:6] = R (or R' for the inverse) dst = &adjoint_ptr[(i + 3) * adjoint_pitch + 3]; memcpy(dst, src, 3 * sizeof(float)); @@ -780,15 +766,13 @@ __global__ void adjoint_se3_kernel(bool inverse, const float *transform, memset(dst, 0, 3 * sizeof(float)); } + // adjoint[3:6, 0:3] = skew(translation) * R (matrix product order matters: + // this must be skew(t) * R, NOT R * skew(t)) float skew[9]; compute_skew_matrix(translation, skew, 3); float temp[9]; - if (inverse) { - matmul_3x3(skew, R, temp); - } else { - matmul_3x3(R, skew, temp); - } + matmul_3x3(skew, R, temp); #pragma unroll for (uint8_t i = 0; i < 3; i++) { @@ -814,9 +798,8 @@ __global__ void adjoint_se3_kernel(bool inverse, const float *transform, * @param jacobian_stride Stride between consecutive Jacobian matrices. * @param size Number of twist vectors to process. */ -__global__ void jacobian_se3_kernel(bool left, const float *twist, - const size_t twist_stride, float *jacobian, - const size_t jacobian_pitch, +__global__ void jacobian_se3_kernel(bool left, const float *twist, const size_t twist_stride, + float *jacobian, const size_t jacobian_pitch, const size_t jacobian_stride, size_t size) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { @@ -877,16 +860,14 @@ __global__ void jacobian_se3_kernel(bool left, const float *twist, * - Q-block negation fused into the final store (eliminates separate negate * loop) */ -constexpr size_t se3_jac_inv_block_size = - 128; ///< Block size for SE(3) inverse Jacobian kernel (tuned for register - ///< pressure). +constexpr size_t se3_jac_inv_block_size = 128; ///< Block size for SE(3) inverse Jacobian kernel + ///< (tuned for register pressure). __global__ void __launch_bounds__(128, 4) jacobian_inverse_se3_kernel(bool left, const float *__restrict__ twist, - const size_t twist_stride, - float *__restrict__ jacobian, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size) { + const size_t twist_stride, float *__restrict__ jacobian, + const size_t jacobian_pitch, const size_t jacobian_stride, + size_t size) { const int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= size) { return; @@ -965,10 +946,8 @@ __global__ void __launch_bounds__(128, 4) * @param inverse_transform Output inverse transformation matrices (4x4, device * pointer). */ -__global__ void inverse_se3_kernel(const float *transform, - const size_t transform_pitch, - const size_t transform_stride, - const size_t inverse_pitch, +__global__ void inverse_se3_kernel(const float *transform, const size_t transform_pitch, + const size_t transform_stride, const size_t inverse_pitch, const size_t inverse_stride, size_t size, float *inverse_transform) { int tid = threadIdx.x + blockIdx.x * blockDim.x; @@ -999,12 +978,9 @@ __global__ void inverse_se3_kernel(const float *transform, swap(pose[0 * 4 + 2], pose[2 * 4 + 0]); swap(pose[1 * 4 + 2], pose[2 * 4 + 1]); - pose[0 * 4 + 3] = - -(pose[0 * 4 + 0] * t1 + pose[0 * 4 + 1] * t2 + pose[0 * 4 + 2] * t3); - pose[1 * 4 + 3] = - -(pose[1 * 4 + 0] * t1 + pose[1 * 4 + 1] * t2 + pose[1 * 4 + 2] * t3); - pose[2 * 4 + 3] = - -(pose[2 * 4 + 0] * t1 + pose[2 * 4 + 1] * t2 + pose[2 * 4 + 2] * t3); + pose[0 * 4 + 3] = -(pose[0 * 4 + 0] * t1 + pose[0 * 4 + 1] * t2 + pose[0 * 4 + 2] * t3); + pose[1 * 4 + 3] = -(pose[1 * 4 + 0] * t1 + pose[1 * 4 + 1] * t2 + pose[1 * 4 + 2] * t3); + pose[2 * 4 + 3] = -(pose[2 * 4 + 0] * t1 + pose[2 * 4 + 1] * t2 + pose[2 * 4 + 2] * t3); #pragma unroll for (uint8_t i = 0; i < 4; i++) { @@ -1021,128 +997,114 @@ __global__ void inverse_se3_kernel(const float *transform, * Launches the CUDA kernel to compute skew-symmetric matrices from twist * vectors. */ -void ComputeSkewSO3(cudaStream_t stream, const float *twist, - const size_t twist_stride, const size_t skew_pitch, - const size_t skew_stride, size_t size, float *skew) { +void ComputeSkewSO3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t skew_pitch, const size_t skew_stride, size_t size, float *skew) { size_t num_blocks = (size + block_size - 1) / block_size; - skew_so3_kernel<<>>( - twist, twist_stride, skew, skew_pitch, skew_stride, size); + skew_so3_kernel<<>>(twist, twist_stride, skew, skew_pitch, + skew_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeNegateMatrix */ -void ComputeNegateMatrix(cudaStream_t stream, const float *matrix, size_t rows, - size_t cols, const size_t pitch, const size_t stride, - size_t size, float *negated_matrix) { +void ComputeNegateMatrix(cudaStream_t stream, const float *matrix, size_t rows, size_t cols, + const size_t pitch, const size_t stride, size_t size, + float *negated_matrix) { size_t num_blocks = (size + block_size - 1) / block_size; - negate_matrices_kernel<<>>( - rows, cols, matrix, pitch, stride, size, negated_matrix); + negate_matrices_kernel<<>>(rows, cols, matrix, pitch, stride, + size, negated_matrix); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeInverseSE3 */ -void ComputeInverseSE3(cudaStream_t stream, const float *transform, - const size_t transform_pitch, - const size_t transform_stride, - const size_t inverse_pitch, const size_t inverse_stride, - size_t size, float *inverse_transform) { +void ComputeInverseSE3(cudaStream_t stream, const float *transform, const size_t transform_pitch, + const size_t transform_stride, const size_t inverse_pitch, + const size_t inverse_stride, size_t size, float *inverse_transform) { size_t num_blocks = (size + block_size - 1) / block_size; inverse_se3_kernel<<>>( - transform, transform_pitch, transform_stride, inverse_pitch, - inverse_stride, size, inverse_transform); + transform, transform_pitch, transform_stride, inverse_pitch, inverse_stride, size, + inverse_transform); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeExpSO3 */ -void ComputeExpSO3(cudaStream_t stream, const float *twist, - const size_t twist_stride, const size_t rotation_pitch, - const size_t rotation_stride, size_t size, float *rotation) { +void ComputeExpSO3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t rotation_pitch, const size_t rotation_stride, size_t size, + float *rotation) { size_t num_blocks = (size + block_size - 1) / block_size; - exp_so3_kernel<<>>( - twist, twist_stride, rotation, rotation_pitch, rotation_stride, size); + exp_so3_kernel<<>>(twist, twist_stride, rotation, + rotation_pitch, rotation_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeLogSO3 */ -void ComputeLogSO3(cudaStream_t stream, const float *rotation, - const size_t rotation_pitch, const size_t rotation_stride, - const size_t twist_stride, size_t size, float *twist) { +void ComputeLogSO3(cudaStream_t stream, const float *rotation, const size_t rotation_pitch, + const size_t rotation_stride, const size_t twist_stride, size_t size, + float *twist) { size_t num_blocks = (size + block_size - 1) / block_size; - log_so3_kernel<<>>( - rotation, rotation_pitch, rotation_stride, twist_stride, size, twist); + log_so3_kernel<<>>(rotation, rotation_pitch, rotation_stride, + twist_stride, size, twist); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianLeftSO3 */ -void ComputeJacobianLeftSO3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size, +void ComputeJacobianLeftSO3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t jacobian_pitch, const size_t jacobian_stride, size_t size, float *jacobian) { size_t num_blocks = (size + block_size - 1) / block_size; constexpr bool left = true; - jacobian_so3_kernel<<>>( - left, twist, twist_stride, jacobian, jacobian_pitch, jacobian_stride, - size); + jacobian_so3_kernel<<>>(left, twist, twist_stride, jacobian, + jacobian_pitch, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianRightSO3 */ -void ComputeJacobianRightSO3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size, +void ComputeJacobianRightSO3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t jacobian_pitch, const size_t jacobian_stride, size_t size, float *jacobian) { size_t num_blocks = (size + block_size - 1) / block_size; constexpr bool left = false; - jacobian_so3_kernel<<>>( - left, twist, twist_stride, jacobian, jacobian_pitch, jacobian_stride, - size); + jacobian_so3_kernel<<>>(left, twist, twist_stride, jacobian, + jacobian_pitch, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianLeftInverseSO3 */ void ComputeJacobianLeftInverseSO3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_inv_pitch, - const size_t jacobian_inv_stride, - size_t size, float *jacobian_inv) { + const size_t twist_stride, const size_t jacobian_inv_pitch, + const size_t jacobian_inv_stride, size_t size, + float *jacobian_inv) { size_t num_blocks = (size + block_size - 1) / block_size; constexpr bool left = true; jacobian_inverse_so3_kernel<<>>( - left, twist, twist_stride, jacobian_inv, jacobian_inv_pitch, - jacobian_inv_stride, size); + left, twist, twist_stride, jacobian_inv, jacobian_inv_pitch, jacobian_inv_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianRightInverseSO3 */ void ComputeJacobianRightInverseSO3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_inv_pitch, - const size_t jacobian_inv_stride, - size_t size, float *jacobian_inv) { + const size_t twist_stride, const size_t jacobian_inv_pitch, + const size_t jacobian_inv_stride, size_t size, + float *jacobian_inv) { size_t num_blocks = (size + block_size - 1) / block_size; constexpr bool left = false; jacobian_inverse_so3_kernel<<>>( - left, twist, twist_stride, jacobian_inv, jacobian_inv_pitch, - jacobian_inv_stride, size); + left, twist, twist_stride, jacobian_inv, jacobian_inv_pitch, jacobian_inv_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeExpSE3 */ -void ComputeExpSE3(cudaStream_t stream, const float *twist, - const size_t twist_stride, const size_t transform_pitch, - const size_t transform_stride, size_t size, +void ComputeExpSE3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t transform_pitch, const size_t transform_stride, size_t size, float *transform) { size_t num_blocks = (size + block_size - 1) / block_size; - exp_se3_kernel<<>>( - twist, twist_stride, transform, transform_pitch, transform_stride, size); + exp_se3_kernel<<>>(twist, twist_stride, transform, + transform_pitch, transform_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeLogSE3 */ -void ComputeLogSE3(cudaStream_t stream, const float *transform, - const size_t transform_pitch, const size_t transform_stride, - const size_t twist_stride, size_t size, float *twist) { +void ComputeLogSE3(cudaStream_t stream, const float *transform, const size_t transform_pitch, + const size_t transform_stride, const size_t twist_stride, size_t size, + float *twist) { size_t num_blocks = (size + block_size - 1) / block_size; log_se3_kernel<<>>( transform, transform_pitch, transform_stride, twist_stride, size, twist); @@ -1150,91 +1112,71 @@ void ComputeLogSE3(cudaStream_t stream, const float *transform, } /** @copydoc ComputeAdjointSE3 */ -void ComputeAdjointSE3(cudaStream_t stream, const float *transform, - const size_t transform_pitch, - const size_t transform_stride, - const size_t adjoint_pitch, const size_t adjoint_stride, - size_t size, float *adjoint) { +void ComputeAdjointSE3(cudaStream_t stream, const float *transform, const size_t transform_pitch, + const size_t transform_stride, const size_t adjoint_pitch, + const size_t adjoint_stride, size_t size, float *adjoint) { size_t num_blocks = (size + block_size - 1) / block_size; bool inverse = false; - adjoint_se3_kernel<<>>( - inverse, transform, transform_pitch, transform_stride, adjoint_pitch, - adjoint_stride, size, adjoint); + adjoint_se3_kernel<<>>(inverse, transform, transform_pitch, + transform_stride, adjoint_pitch, + adjoint_stride, size, adjoint); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeInverseAdjointSE3 */ void ComputeInverseAdjointSE3(cudaStream_t stream, const float *transform, - const size_t transform_pitch, - const size_t transform_stride, - const size_t inv_adjoint_pitch, - const size_t inv_adjoint_stride, size_t size, - float *inv_adjoint) { + const size_t transform_pitch, const size_t transform_stride, + const size_t inv_adjoint_pitch, const size_t inv_adjoint_stride, + size_t size, float *inv_adjoint) { size_t num_blocks = (size + block_size - 1) / block_size; bool inverse = true; - adjoint_se3_kernel<<>>( - inverse, transform, transform_pitch, transform_stride, inv_adjoint_pitch, - inv_adjoint_stride, size, inv_adjoint); + adjoint_se3_kernel<<>>(inverse, transform, transform_pitch, + transform_stride, inv_adjoint_pitch, + inv_adjoint_stride, size, inv_adjoint); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianLeftSE3 */ -void ComputeJacobianLeftSE3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size, +void ComputeJacobianLeftSE3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t jacobian_pitch, const size_t jacobian_stride, size_t size, float *jacobian) { size_t num_blocks = (size + block_size - 1) / block_size; constexpr bool left = true; - jacobian_se3_kernel<<>>( - left, twist, twist_stride, jacobian, jacobian_pitch, jacobian_stride, - size); + jacobian_se3_kernel<<>>(left, twist, twist_stride, jacobian, + jacobian_pitch, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianLeftInverseSE3 */ void ComputeJacobianLeftInverseSE3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size, - float *jacobian) { - size_t num_blocks = - (size + se3_jac_inv_block_size - 1) / se3_jac_inv_block_size; + const size_t twist_stride, const size_t jacobian_pitch, + const size_t jacobian_stride, size_t size, float *jacobian) { + size_t num_blocks = (size + se3_jac_inv_block_size - 1) / se3_jac_inv_block_size; constexpr bool left = true; - jacobian_inverse_se3_kernel<<>>(left, twist, twist_stride, jacobian, - jacobian_pitch, jacobian_stride, - size); + jacobian_inverse_se3_kernel<<>>( + left, twist, twist_stride, jacobian, jacobian_pitch, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianRightSE3 */ -void ComputeJacobianRightSE3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size, +void ComputeJacobianRightSE3(cudaStream_t stream, const float *twist, const size_t twist_stride, + const size_t jacobian_pitch, const size_t jacobian_stride, size_t size, float *jacobian) { size_t num_blocks = (size + block_size - 1) / block_size; constexpr bool left = false; - jacobian_se3_kernel<<>>( - left, twist, twist_stride, jacobian, jacobian_pitch, jacobian_stride, - size); + jacobian_se3_kernel<<>>(left, twist, twist_stride, jacobian, + jacobian_pitch, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @copydoc ComputeJacobianRightInverseSE3 */ void ComputeJacobianRightInverseSE3(cudaStream_t stream, const float *twist, - const size_t twist_stride, - const size_t jacobian_pitch, - const size_t jacobian_stride, size_t size, - float *jacobian) { - size_t num_blocks = - (size + se3_jac_inv_block_size - 1) / se3_jac_inv_block_size; + const size_t twist_stride, const size_t jacobian_pitch, + const size_t jacobian_stride, size_t size, float *jacobian) { + size_t num_blocks = (size + se3_jac_inv_block_size - 1) / se3_jac_inv_block_size; constexpr bool left = false; - jacobian_inverse_se3_kernel<<>>(left, twist, twist_stride, jacobian, - jacobian_pitch, jacobian_stride, - size); + jacobian_inverse_se3_kernel<<>>( + left, twist, twist_stride, jacobian, jacobian_pitch, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } /** @@ -1242,8 +1184,7 @@ void ComputeJacobianRightInverseSE3(cudaStream_t stream, const float *twist, * * For R = [[a,b,c],[d,e,f],[g,h,i]], computes R^T = [[a,d,g],[b,e,h],[c,f,i]]. */ -__global__ void transpose_so3_kernel(const float *rotations, - size_t input_stride, float *transposed, +__global__ void transpose_so3_kernel(const float *rotations, size_t input_stride, float *transposed, size_t output_stride, size_t n) { int tid = threadIdx.x + blockIdx.x * blockDim.x; if (tid >= (int)n) { @@ -1263,12 +1204,11 @@ __global__ void transpose_so3_kernel(const float *rotations, } /** @copydoc ComputeTransposeSO3 */ -void ComputeTransposeSO3(cudaStream_t stream, const float *rotation, - size_t input_stride, size_t output_stride, size_t size, - float *transposed) { +void ComputeTransposeSO3(cudaStream_t stream, const float *rotation, size_t input_stride, + size_t output_stride, size_t size, float *transposed) { size_t num_blocks = (size + block_size - 1) / block_size; - transpose_so3_kernel<<>>( - rotation, input_stride, transposed, output_stride, size); + transpose_so3_kernel<<>>(rotation, input_stride, transposed, + output_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } @@ -1286,9 +1226,8 @@ constexpr size_t kSO2MathBlockSize = 256; * [sin(theta), cos(theta)]] * stored in row-major order (4 floats). */ -__global__ void exp_so2_kernel(const float *angles, size_t angle_stride, - float *rotations, size_t rotation_stride, - size_t size) { +__global__ void exp_so2_kernel(const float *angles, size_t angle_stride, float *rotations, + size_t rotation_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { return; @@ -1311,9 +1250,8 @@ __global__ void exp_so2_kernel(const float *angles, size_t angle_stride, * Extracts the angle from R via Log(R) = atan2(R[1,0], R[0,0]). * For R = [[c,-s],[s,c]], this returns atan2(s, c) = theta. */ -__global__ void log_so2_kernel(const float *rotations, size_t rotation_stride, - float *angles, size_t angle_stride, - size_t size) { +__global__ void log_so2_kernel(const float *rotations, size_t rotation_stride, float *angles, + size_t angle_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { return; @@ -1329,8 +1267,7 @@ __global__ void log_so2_kernel(const float *rotations, size_t rotation_stride, * For R = [[a,b],[c,d]], computes R^T = [[a,c],[b,d]]. * Since R is orthogonal, R^T = R^{-1}. */ -__global__ void transpose_so2_kernel(const float *rotations, - size_t input_stride, float *transposed, +__global__ void transpose_so2_kernel(const float *rotations, size_t input_stride, float *transposed, size_t output_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { @@ -1346,27 +1283,24 @@ __global__ void transpose_so2_kernel(const float *rotations, Rt[3] = R[3]; } -void ComputeExpSO2(cudaStream_t stream, const float *angles, - size_t angle_stride, size_t rotation_stride, size_t size, - float *rotations) { +void ComputeExpSO2(cudaStream_t stream, const float *angles, size_t angle_stride, + size_t rotation_stride, size_t size, float *rotations) { size_t num_blocks = (size + kSO2MathBlockSize - 1) / kSO2MathBlockSize; - exp_so2_kernel<<>>( - angles, angle_stride, rotations, rotation_stride, size); + exp_so2_kernel<<>>(angles, angle_stride, rotations, + rotation_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } -void ComputeLogSO2(cudaStream_t stream, const float *rotations, - size_t rotation_stride, size_t angle_stride, size_t size, - float *angles) { +void ComputeLogSO2(cudaStream_t stream, const float *rotations, size_t rotation_stride, + size_t angle_stride, size_t size, float *angles) { size_t num_blocks = (size + kSO2MathBlockSize - 1) / kSO2MathBlockSize; - log_so2_kernel<<>>( - rotations, rotation_stride, angles, angle_stride, size); + log_so2_kernel<<>>(rotations, rotation_stride, angles, + angle_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } -void ComputeTransposeSO2(cudaStream_t stream, const float *rotations, - size_t input_stride, size_t output_stride, size_t size, - float *transposed) { +void ComputeTransposeSO2(cudaStream_t stream, const float *rotations, size_t input_stride, + size_t output_stride, size_t size, float *transposed) { size_t num_blocks = (size + kSO2MathBlockSize - 1) / kSO2MathBlockSize; transpose_so2_kernel<<>>( rotations, input_stride, transposed, output_stride, size); @@ -1393,9 +1327,8 @@ constexpr size_t kSE2MathBlockSize = 256; * * For |theta| < 1e-3, V approaches I and [tx, ty] ~ [v_x, v_y]. */ -__global__ void exp_se2_kernel(const float *tangent, size_t tangent_stride, - float *transforms, size_t transform_stride, - size_t size) { +__global__ void exp_se2_kernel(const float *tangent, size_t tangent_stride, float *transforms, + size_t transform_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { return; @@ -1446,9 +1379,8 @@ __global__ void exp_se2_kernel(const float *tangent, size_t tangent_stride, * V^{-1} = (theta / (2*(1-cos))) * R_pi/2 * ((c-1)*I + s*J) * [tx,ty] * where R_pi/2 rotates by 90 degrees: (x,y) -> (-y, x). */ -__global__ void log_se2_kernel(const float *transforms, size_t transform_stride, - float *tangent, size_t tangent_stride, - size_t size) { +__global__ void log_se2_kernel(const float *transforms, size_t transform_stride, float *tangent, + size_t tangent_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { return; @@ -1486,10 +1418,8 @@ __global__ void log_se2_kernel(const float *transforms, size_t transform_stride, * For a 2D rigid transform T = [R t; 0 1] with R orthogonal, * the inverse is [R^T, -R^T*t; 0 1]. */ -__global__ void inverse_se2_kernel(const float *transforms, - size_t transform_stride, - float *inverse_transforms, - size_t inverse_stride, size_t size) { +__global__ void inverse_se2_kernel(const float *transforms, size_t transform_stride, + float *inverse_transforms, size_t inverse_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { return; @@ -1527,8 +1457,7 @@ __global__ void inverse_se2_kernel(const float *transforms, * For |alpha| < 1e-3 (near identity): J_r^{-1} ~ I + small corrections. */ __global__ void __launch_bounds__(256, 4) - jacobian_right_inverse_se2_kernel(const float *tangent, - size_t tangent_stride, float *jacobians, + jacobian_right_inverse_se2_kernel(const float *tangent, size_t tangent_stride, float *jacobians, size_t jacobian_stride, size_t size) { size_t idx = blockIdx.x * blockDim.x + threadIdx.x; if (idx >= size) { @@ -1568,27 +1497,24 @@ __global__ void __launch_bounds__(256, 4) } } -void ComputeExpSE2(cudaStream_t stream, const float *tangent, - size_t tangent_stride, size_t transform_stride, size_t size, - float *transforms) { +void ComputeExpSE2(cudaStream_t stream, const float *tangent, size_t tangent_stride, + size_t transform_stride, size_t size, float *transforms) { size_t num_blocks = (size + kSE2MathBlockSize - 1) / kSE2MathBlockSize; - exp_se2_kernel<<>>( - tangent, tangent_stride, transforms, transform_stride, size); + exp_se2_kernel<<>>(tangent, tangent_stride, transforms, + transform_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } -void ComputeLogSE2(cudaStream_t stream, const float *transforms, - size_t transform_stride, size_t tangent_stride, size_t size, - float *tangent) { +void ComputeLogSE2(cudaStream_t stream, const float *transforms, size_t transform_stride, + size_t tangent_stride, size_t size, float *tangent) { size_t num_blocks = (size + kSE2MathBlockSize - 1) / kSE2MathBlockSize; - log_se2_kernel<<>>( - transforms, transform_stride, tangent, tangent_stride, size); + log_se2_kernel<<>>(transforms, transform_stride, + tangent, tangent_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } -void ComputeInverseSE2(cudaStream_t stream, const float *transforms, - size_t transform_stride, size_t inverse_stride, - size_t size, float *inverse_transforms) { +void ComputeInverseSE2(cudaStream_t stream, const float *transforms, size_t transform_stride, + size_t inverse_stride, size_t size, float *inverse_transforms) { size_t num_blocks = (size + kSE2MathBlockSize - 1) / kSE2MathBlockSize; inverse_se2_kernel<<>>( transforms, transform_stride, inverse_transforms, inverse_stride, size); @@ -1596,14 +1522,12 @@ void ComputeInverseSE2(cudaStream_t stream, const float *transforms, } void ComputeJacobianRightInverseSE2(cudaStream_t stream, const float *tangent, - size_t tangent_stride, - size_t jacobian_stride, size_t size, + size_t tangent_stride, size_t jacobian_stride, size_t size, float *jacobians) { size_t num_blocks = (size + kSE2MathBlockSize - 1) / kSE2MathBlockSize; - jacobian_right_inverse_se2_kernel<<>>( + jacobian_right_inverse_se2_kernel<<>>( tangent, tangent_stride, jacobians, jacobian_stride, size); THROW_ON_CUDA_ERROR(cudaGetLastError()); } -} // namespace cunls +} // namespace cunls diff --git a/cunls/minimizer/CMakeLists.txt b/cunls/minimizer/CMakeLists.txt index 2937924..957f5f4 100644 --- a/cunls/minimizer/CMakeLists.txt +++ b/cunls/minimizer/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(cunls_minimizer OBJECT levenberg_marquardt_minimizer.cpp minimizer_state.cu normal_equations.cu + numeric_diff_jacobian.cu problem.cpp residual_batch.cu sparse_matrix.cu diff --git a/cunls/minimizer/gauss_newton_minimizer.cu b/cunls/minimizer/gauss_newton_minimizer.cu index 5b17e07..9a94ecd 100644 --- a/cunls/minimizer/gauss_newton_minimizer.cu +++ b/cunls/minimizer/gauss_newton_minimizer.cu @@ -150,6 +150,12 @@ float GaussNewtonMinimizer::ComputeCost(cudaStream_t stream, const Problem &prob * * Evaluates all factor batches to compute residual values and their Jacobian * matrices. Both are dense per-factor blocks, concatenated across batches. + * Per residual batch, either the FactorBatch's analytic Jacobian is used + * directly (via ResidualBatch::Evaluate, which also applies any registered + * loss function), or a finite-difference Jacobian is built via + * `numeric_diff_builder_` from the raw residual-only evaluation, with loss + * scaling then applied via `ResidualBatch::ApplyLoss` so both paths see + * identical loss handling. * * @param stream CUDA stream for GPU operations. * @param problem The optimization problem. @@ -157,9 +163,11 @@ float GaussNewtonMinimizer::ComputeCost(cudaStream_t stream, const Problem &prob * @param[out] residuals Output residual vector. * @param[out] jacobians Output per-factor dense Jacobian blocks. */ -void ComputeResidualAndJacobian(cudaStream_t stream, const Problem &problem, - const MinimizerState &minimizer_state, dvector &residuals, - PerFactorJacobians &jacobians, dvector &buffer) { +void GaussNewtonMinimizer::ComputeResidualAndJacobian(cudaStream_t stream, const Problem &problem, + const MinimizerState &minimizer_state, + dvector &residuals, + PerFactorJacobians &jacobians, + dvector &buffer) { const auto &state_pointers = minimizer_state.GetStatePointers(); const auto &residual_batches = problem.GetResidualBatches(); size_t max_n = 0; @@ -178,10 +186,24 @@ void ComputeResidualAndJacobian(cudaStream_t stream, const Problem &problem, for (size_t i = 0; i < residual_batches.size(); i++) { const auto &rb = residual_batches[i]; auto ptrs = state_pointers[i].data(); + const auto &factor_batch = rb.GetFactorBatch(); - rb.Evaluate(stream, workspace_ptr, residuals_ptr, ptrs, nullptr, jacobian_ptr); + JacobianMode mode = problem.JacobianModeFor(i, options_.jacobian_mode); + if (mode == JacobianMode::kAnalytic) { + rb.Evaluate(stream, workspace_ptr, residuals_ptr, ptrs, nullptr, jacobian_ptr); + } else { + // Raw (pre-loss) residual + finite-difference Jacobian, then apply any + // registered loss function to both in place -- exactly mirrors what + // ResidualBatch::Evaluate would have done after an analytic + // FactorBatch::Evaluate call. + factor_batch->Evaluate(residuals_ptr, nullptr, ptrs, stream); + numeric_diff_builder_.Compute(stream, problem, i, minimizer_state, residuals_ptr, + jacobian_ptr, options_.numeric_diff_options); + if (rb.GetLossFunction() != nullptr) { + rb.ApplyLoss(stream, workspace_ptr, residuals_ptr, nullptr, jacobian_ptr); + } + } - const auto &factor_batch = rb.GetFactorBatch(); size_t num_residuals = factor_batch->NumFactors() * factor_batch->ResidualsSize(); residuals_ptr += num_residuals; diff --git a/cunls/minimizer/gauss_newton_minimizer.h b/cunls/minimizer/gauss_newton_minimizer.h index 520ce39..7c996b9 100644 --- a/cunls/minimizer/gauss_newton_minimizer.h +++ b/cunls/minimizer/gauss_newton_minimizer.h @@ -19,6 +19,8 @@ #include +#include +#include #include #include "cunls/common/cusparse_helper.h" @@ -26,8 +28,10 @@ #include "cunls/common/profiler.h" #include "cunls/common/types.h" #include "cunls/linear_solver/sparse_linear_solver.h" +#include "cunls/minimizer/jacobian_mode.h" #include "cunls/minimizer/minimizer_state.h" #include "cunls/minimizer/normal_equations.h" +#include "cunls/minimizer/numeric_diff_jacobian.h" #include "cunls/minimizer/problem.h" #include "cunls/minimizer/sparse_matrix.h" #include "cunls/state/state_batch_ops.h" @@ -181,6 +185,24 @@ struct MinimizerOptions { * Default: true (safety checks disabled). */ bool disable_safety_checks = true; + + /** + * @brief Global default Jacobian strategy for every residual batch. + * + * `kAnalytic` (default) uses each FactorBatch's own hand-derived Jacobian. + * `kNumeric` derives Jacobians via finite differences instead (see + * `NumericDiffJacobianBuilder`), requiring only residual-only evaluation + * support from the factor batch. Individual residual batches can override + * this default via `Problem::AddFactorBatch`'s `jacobian_mode_override` + * parameter. + */ + JacobianMode jacobian_mode = JacobianMode::kAnalytic; + + /** + * @brief Tuning for numeric-diff Jacobians. Ignored when `jacobian_mode` + * (and every per-group override) is `kAnalytic`. + */ + NumericDiffOptions numeric_diff_options = {}; }; /** @@ -366,6 +388,25 @@ class GaussNewtonMinimizer { /** @brief Sizes the per-factor Jacobian buffer for the problem. */ void ResizeFactorJacobians(); + /** + * @brief Computes residuals and Jacobian for the current states. + * + * Evaluates all factor batches to compute residual values and their + * Jacobian matrices (dense per-factor blocks, concatenated across + * batches). Per residual batch, uses either the FactorBatch's analytic + * Jacobian or a finite-difference Jacobian from `numeric_diff_builder_`, + * according to `Problem::JacobianModeFor`. + * + * @param stream CUDA stream for GPU operations. + * @param problem The optimization problem. + * @param minimizer_state Current minimizer state. + * @param[out] residuals Output residual vector. + * @param[out] jacobians Output per-factor dense Jacobian blocks. + */ + void ComputeResidualAndJacobian(cudaStream_t stream, const Problem &problem, + const MinimizerState &minimizer_state, dvector &residuals, + PerFactorJacobians &jacobians, dvector &buffer); + protected: const MinimizerOptions options_; ///< Optimizer configuration options. @@ -374,6 +415,9 @@ class GaussNewtonMinimizer { StateBatchOps state_ops_; ///< Operations on state batches. + /// Builds finite-difference Jacobians for residual batches in kNumeric mode. + NumericDiffJacobianBuilder numeric_diff_builder_; + dvector residuals_; ///< Residual vector storage. /// Per-factor dense Jacobian blocks; the only Jacobian ever materialized. diff --git a/cunls/minimizer/jacobian_mode.h b/cunls/minimizer/jacobian_mode.h new file mode 100644 index 0000000..bccc3d8 --- /dev/null +++ b/cunls/minimizer/jacobian_mode.h @@ -0,0 +1,59 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +namespace cunls { + +/** + * @brief Selects how a residual batch's Jacobian is obtained. + * + * `kAnalytic` (default) uses the FactorBatch's own hand-derived Evaluate() + * Jacobian output. `kNumeric` instead derives the Jacobian via finite + * differences on the manifold tangent space of each referenced state block, + * requiring only that the factor batch support residual-only evaluation + * (`jacobians == nullptr`), which every FactorBatch implementation must + * already do. + */ +enum class JacobianMode { kAnalytic, kNumeric }; + +/** + * @brief Tuning knobs for numeric (finite-difference) Jacobian computation. + */ +struct NumericDiffOptions { + /** @brief Finite-difference scheme. */ + enum class Method { + /** @brief One-sided: (f(x+eps) - f(x)) / eps. Cheaper, less accurate. */ + kForward, + /** @brief Two-sided: (f(x+eps) - f(x-eps)) / (2*eps). Default. */ + kCentral, + }; + + /** @brief Finite-difference scheme. Default: central. */ + Method method = Method::kCentral; + + /** + * @brief Per-tangent-coordinate perturbation step size. + * + * Phase 1 uses this as a fixed scalar step (not scaled by the current + * state magnitude); see `numeric_diff_jacobian.h` for rationale. + * Default: 1e-4. + */ + float relative_step_size = 1e-4f; +}; + +} // namespace cunls diff --git a/cunls/minimizer/numeric_diff_jacobian.cu b/cunls/minimizer/numeric_diff_jacobian.cu new file mode 100644 index 0000000..b5b33bd --- /dev/null +++ b/cunls/minimizer/numeric_diff_jacobian.cu @@ -0,0 +1,492 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +#include "cunls/common/helper.h" +#include "cunls/factor/factor_batch.h" +#include "cunls/minimizer/minimizer_state.h" +#include "cunls/minimizer/numeric_diff_jacobian.h" +#include "cunls/minimizer/problem.h" +#include "cunls/state/state_batch.h" + +namespace cunls { + +namespace { + +constexpr int kBlockSize = 256; + +// One thread per (factor, tangent-dof, residual-row) element. Reads the two +// (or one, for forward diff) perturbed residual evaluations for that dof and +// writes the finite-difference column directly into the dense per-factor +// Jacobian layout (`ResidualsSize() x sum(StateBlockSizes())`, row-major, +// per factor) that analytic Jacobians also use. +__global__ void NumericDiffColumnKernel( + const float *__restrict__ perturbed_residuals, const float *__restrict__ baseline_residuals, + const int *__restrict__ col_idx, const int *__restrict__ plus_slot, + const int *__restrict__ minus_slot, const float *__restrict__ eps_arr, bool central, int F, + int W, int residual_size, int total_cols, float *__restrict__ jacobian_out) { + long long idx = static_cast(blockIdx.x) * blockDim.x + threadIdx.x; + long long total = static_cast(F) * W * residual_size; + if (idx >= total) return; + + int r = static_cast(idx % residual_size); + long long tmp = idx / residual_size; + int j = static_cast(tmp % W); + int f = static_cast(tmp / W); + + int ps = plus_slot[j]; + float rp = perturbed_residuals[(static_cast(ps) * F + f) * residual_size + r]; + + float rm; + float denom; + if (central) { + int ms = minus_slot[j]; + rm = perturbed_residuals[(static_cast(ms) * F + f) * residual_size + r]; + denom = 2.0f * eps_arr[j]; + } else { + rm = baseline_residuals[static_cast(f) * residual_size + r]; + denom = eps_arr[j]; + } + + float deriv = (rp - rm) / denom; + jacobian_out[(static_cast(f) * residual_size + r) * total_cols + col_idx[j]] = deriv; +} + +// Grows *ptr (a pinned host allocation) to at least `n` elements if needed, +// then returns it. Kept per-ComputeCache (never shared across residual +// batches) so that one batch's rebuild can never overwrite host memory a +// prior batch's still-in-flight async H2D upload is reading from. +template +T *EnsurePinnedHost(T *&ptr, size_t &capacity, size_t n) { + if (n > capacity) { + if (ptr != nullptr) THROW_ON_CUDA_ERROR(cudaFreeHost(ptr)); + THROW_ON_CUDA_ERROR(cudaMallocHost(&ptr, n * sizeof(T))); + capacity = n; + } + return ptr; +} + +} // namespace + +NumericDiffJacobianBuilder::ComputeCache::~ComputeCache() { + // A prior rebuild's async H2D uploads may still be in flight when this + // cache is torn down (e.g. Problem structure changed and + // PrepareResidualBatch erased it): wait for them before freeing the + // pinned host memory they read from, or the driver could still be + // DMA-reading from memory we're about to release. + if (pinned_upload_done_event != nullptr) { + cudaEventSynchronize(pinned_upload_done_event); + cudaEventDestroy(pinned_upload_done_event); + } + if (pinned_delta_host != nullptr) cudaFreeHost(pinned_delta_host); + if (pinned_ptrs_host != nullptr) cudaFreeHost(pinned_ptrs_host); + if (pinned_int_host != nullptr) cudaFreeHost(pinned_int_host); + if (pinned_eps_host != nullptr) cudaFreeHost(pinned_eps_host); +} + +NumericDiffJacobianBuilder::NumericDiffJacobianBuilder() { + THROW_ON_CUDA_ERROR(cudaEventCreateWithFlags(&delta_ready_event_, cudaEventDisableTiming)); +} + +NumericDiffJacobianBuilder::~NumericDiffJacobianBuilder() { + for (auto ev : pool_events_) { + if (ev != nullptr) cudaEventDestroy(ev); + } + for (auto s : pool_streams_) { + if (s != nullptr) cudaStreamDestroy(s); + } + if (delta_ready_event_ != nullptr) { + cudaEventDestroy(delta_ready_event_); + } +} + +void NumericDiffJacobianBuilder::EnsureStreamPool(size_t num_streams) { + while (pool_streams_.size() < num_streams) { + cudaStream_t s; + THROW_ON_CUDA_ERROR(cudaStreamCreateWithFlags(&s, cudaStreamNonBlocking)); + pool_streams_.push_back(s); + cudaEvent_t ev; + THROW_ON_CUDA_ERROR(cudaEventCreateWithFlags(&ev, cudaEventDisableTiming)); + pool_events_.push_back(ev); + } +} + +void NumericDiffJacobianBuilder::PrepareResidualBatch(const Problem &problem, + size_t residual_batch_index) { + const auto &residual_batches = problem.GetResidualBatches(); + if (residual_batch_index >= residual_batches.size()) { + throw std::runtime_error( + "NumericDiffJacobianBuilder::PrepareResidualBatch: index out of range"); + } + const auto &rb = residual_batches[residual_batch_index]; + const FactorBatch *factor_batch = rb.GetFactorBatch(); + const auto &state_batches = problem.GetStateBatches(); + const auto &host_ptrs = problem.GetStatePointers()[residual_batch_index]; + + const size_t F = factor_batch->NumFactors(); + auto block_sizes = factor_batch->StateBlockSizes(); + const size_t P = block_sizes.size(); + + // Address -> (state batch index, block index) lookup, built from the + // problem's own state batches. This mirrors the pointer-identity walk + // Problem::CheckGraphConnectivity already performs. + std::unordered_map> addr_to_block; + for (size_t bi = 0; bi < state_batches.size(); ++bi) { + StateBatch *sb = state_batches[bi]; + const size_t n = sb->NumStateBlocks(); + for (size_t k = 0; k < n; ++k) { + addr_to_block[sb->StateBlockDevicePtr(k)] = {bi, k}; + } + } + + BatchPlan plan; + plan.num_factors = F; + plan.num_positions = P; + plan.state_block_sizes = block_sizes; + plan.col_offsets.resize(P); + size_t running = 0; + for (size_t b = 0; b < P; ++b) { + plan.col_offsets[b] = running; + running += block_sizes[b]; + } + plan.owner_batch_index.assign(P, 0); + plan.block_idx.assign(F * P, 0); + + if (F > 0 && P > 0) { + for (size_t b = 0; b < P; ++b) { + auto it = addr_to_block.find(host_ptrs[0 * P + b]); + if (it == addr_to_block.end()) { + throw std::runtime_error( + "NumericDiffJacobianBuilder: factor state pointer is not owned by any registered " + "StateBatch"); + } + plan.owner_batch_index[b] = it->second.first; + } + for (size_t f = 0; f < F; ++f) { + for (size_t b = 0; b < P; ++b) { + auto it = addr_to_block.find(host_ptrs[f * P + b]); + if (it == addr_to_block.end()) { + throw std::runtime_error( + "NumericDiffJacobianBuilder: factor state pointer is not owned by any registered " + "StateBatch"); + } + plan.block_idx[f * P + b] = it->second.second; + } + } + } + + plans_[residual_batch_index] = std::move(plan); + // Structure changed (or is being defined for the first time): any cached + // slot layout / uploaded device scratch for this residual batch no longer + // matches, so drop it and let the next Compute() rebuild from scratch. + caches_.erase(residual_batch_index); +} + +void NumericDiffJacobianBuilder::Compute(cudaStream_t stream, const Problem &problem, + size_t residual_batch_index, + const MinimizerState &minimizer_state, + const float *baseline_residuals, float *jacobian_out, + const NumericDiffOptions &options) { + auto plan_it = plans_.find(residual_batch_index); + if (plan_it == plans_.end()) { + PrepareResidualBatch(problem, residual_batch_index); + plan_it = plans_.find(residual_batch_index); + } + const BatchPlan &plan = plan_it->second; + + const auto &rb = problem.GetResidualBatches()[residual_batch_index]; + const FactorBatch *factor_batch = rb.GetFactorBatch(); + const size_t F = plan.num_factors; + const size_t P = plan.num_positions; + if (F == 0 || P == 0) return; + + const size_t residual_size = factor_batch->ResidualsSize(); + const size_t total_cols = plan.col_offsets.back() + plan.state_block_sizes.back(); + + const auto &state_batches = problem.GetStateBatches(); + const auto &states = minimizer_state.GetStates(); + + const bool central = (options.method == NumericDiffOptions::Method::kCentral); + + std::vector tangent_size(P), ambient_size(P), num_state_blocks(P); + size_t W = 0; + for (size_t b = 0; b < P; ++b) { + StateBatch *owner = state_batches[plan.owner_batch_index[b]]; + tangent_size[b] = owner->TangentSize(); + ambient_size[b] = owner->AmbientSize(); + num_state_blocks[b] = owner->NumStateBlocks(); + W += tangent_size[b]; + } + if (W == 0) { + // No optimizable tangent dof referenced by this factor batch (all + // referenced blocks are zero-dimensional or constant); nothing to + // differentiate. Leave jacobian_out untouched (callers should not read + // it for constant-only groups anyway, mirroring analytic behavior). + return; + } + + const size_t S = central ? 2 * W : W; + + ComputeCache &cache = caches_[residual_batch_index]; + + // ---- Current address fingerprint: the owning StateBatch data pointer + // ---- per referenced position. If this matches what was uploaded last + // ---- time (and the slot layout/options/sizes haven't changed), every + // ---- host-built array this function would otherwise re-upload is + // ---- byte-for-byte identical to what's already on the device -- it + // ---- depends only on structure + options + these addresses, never on + // ---- the state *values* -- so the rebuild below can be skipped + // ---- entirely. ---- + std::vector owner_data_ptr(P); + for (size_t b = 0; b < P; ++b) owner_data_ptr[b] = states[plan.owner_batch_index[b]].data(); + + bool needs_rebuild = !cache.uploaded || cache.central != central || + cache.step_size != options.relative_step_size || cache.F != F || + cache.P != P || cache.residual_size != residual_size || + cache.total_cols != total_cols || + cache.last_owner_data_ptr != owner_data_ptr; + + if (!needs_rebuild) { + // x_plus_delta_scratch's own address can only change if it needed to + // grow, which cannot happen without S/W (and thus the layout above) + // also changing -- but check defensively anyway, it's one pointer. + needs_rebuild = (cache.x_plus_delta_scratch.data() != cache.last_xpd_base); + } + + cache.S = S; + cache.W = W; + cache.F = F; + cache.P = P; + cache.residual_size = residual_size; + cache.total_cols = total_cols; + + if (needs_rebuild) { + // A previous rebuild's async H2D uploads may still be reading from this + // cache's pinned host buffers; wait for them to finish before this + // rebuild grows, overwrites, or (via EnsurePinnedHost's cudaFreeHost + // path) frees any of them. + if (cache.pinned_upload_done_event != nullptr) { + THROW_ON_CUDA_ERROR(cudaEventSynchronize(cache.pinned_upload_done_event)); + } + + // ---- Slot layout: one (position b, tangent dof k, sign) per slot. ---- + cache.slot_b.assign(S, 0); + cache.slot_k.assign(S, 0); + cache.slot_owner.assign(S, 0); + cache.slot_sign.assign(S, 0.0f); + cache.delta_offset.assign(S, 0); + cache.xpd_offset.assign(S, 0); + cache.col_idx_h.assign(W, 0); + cache.plus_slot_h.assign(W, 0); + cache.minus_slot_h.assign(W, central ? 0 : -1); + cache.eps_h.assign(W, options.relative_step_size); + + size_t delta_total = 0, xpd_total = 0, slot = 0, j = 0; + for (size_t b = 0; b < P; ++b) { + for (size_t k = 0; k < tangent_size[b]; ++k) { + auto place_slot = [&](float sign) -> size_t { + size_t s = slot++; + cache.slot_b[s] = b; + cache.slot_k[s] = k; + cache.slot_owner[s] = plan.owner_batch_index[b]; + cache.slot_sign[s] = sign; + cache.delta_offset[s] = delta_total; + delta_total += num_state_blocks[b] * tangent_size[b]; + cache.xpd_offset[s] = xpd_total; + xpd_total += num_state_blocks[b] * ambient_size[b]; + return s; + }; + + cache.plus_slot_h[j] = static_cast(place_slot(1.0f)); + cache.minus_slot_h[j] = central ? static_cast(place_slot(-1.0f)) : -1; + cache.col_idx_h[j] = static_cast(plan.col_offsets[b] + k); + ++j; + } + } + cache.delta_total = delta_total; + cache.xpd_total = xpd_total; + + cache.delta_scratch.resize(delta_total); + cache.x_plus_delta_scratch.resize(xpd_total); + cache.perturbed_residuals.resize(S * F * residual_size); + cache.state_pointer_scratch.resize(S * F * P); + cache.col_idx_scratch.resize(W); + cache.plus_slot_scratch.resize(W); + cache.minus_slot_scratch.resize(W); + cache.eps_scratch.resize(W); + + // ---- Build & upload the one-hot tangent deltas for every slot, via a + // ---- pinned staging buffer (true async DMA, not an internally-staged + // ---- copy through a driver bounce buffer). ---- + float *delta_host = + EnsurePinnedHost(cache.pinned_delta_host, cache.pinned_delta_capacity, delta_total); + std::fill(delta_host, delta_host + delta_total, 0.0f); + for (size_t s = 0; s < S; ++s) { + const size_t b = cache.slot_b[s]; + const size_t k = cache.slot_k[s]; + float *delta_block = delta_host + cache.delta_offset[s]; + for (size_t f = 0; f < F; ++f) { + const size_t blk = plan.block_idx[f * P + b]; + delta_block[blk * tangent_size[b] + k] = cache.slot_sign[s] * options.relative_step_size; + } + } + THROW_ON_CUDA_ERROR(cudaMemcpyAsync(cache.delta_scratch.data(), delta_host, + delta_total * sizeof(float), cudaMemcpyHostToDevice, + stream)); + THROW_ON_CUDA_ERROR(cudaEventRecord(delta_ready_event_, stream)); + + // ---- Plus() calls, spread across a small stream pool so independent + // ---- perturbations overlap instead of serializing on one stream. + // ---- IMPORTANT: several shipped StateBatch::Plus implementations (e.g. + // ---- SO3StateBatch, SE3StateBatch) reuse `mutable` internal scratch + // ---- buffers across calls and are therefore not safe to invoke + // ---- concurrently on the same owner from different streams. Slots are + // ---- assigned to pool streams by *owner batch index* (not by slot + // ---- index), so every Plus() call against a given StateBatch lands on + // ---- the same stream and is naturally serialized in issue order, while + // ---- distinct owner batches can still overlap on different streams. ---- + constexpr size_t kStreamPoolSize = 8; + EnsureStreamPool(kStreamPoolSize); + for (size_t s = 0; s < S; ++s) { + cudaStream_t ps = pool_streams_[cache.slot_owner[s] % pool_streams_.size()]; + THROW_ON_CUDA_ERROR(cudaStreamWaitEvent(ps, delta_ready_event_, 0)); + StateBatch *owner = state_batches[cache.slot_owner[s]]; + const float *x = states[cache.slot_owner[s]].data(); + const float *delta = cache.delta_scratch.data() + cache.delta_offset[s]; + float *xpd = cache.x_plus_delta_scratch.data() + cache.xpd_offset[s]; + owner->Plus(x, delta, xpd, ps); + } + // Join: main stream waits for every pool stream before reading the + // perturbed buffers they wrote (harmless no-op wait for pool streams + // that received no work this call). + for (size_t i = 0; i < pool_streams_.size(); ++i) { + THROW_ON_CUDA_ERROR(cudaEventRecord(pool_events_[i], pool_streams_[i])); + THROW_ON_CUDA_ERROR(cudaStreamWaitEvent(stream, pool_events_[i], 0)); + } + + // ---- Build replicated state-pointer table (S * F * P) on the host and + // ---- upload once (pinned staging). Baseline (unperturbed) positions + // ---- reuse the current minimizer-state block pointers; the perturbed + // ---- position for a slot's own (b) reads from that slot's Plus() + // ---- output. This table is purely address-based (no state values), so + // ---- it stays valid -- and is never rebuilt -- for as long as those + // ---- addresses don't move. ---- + std::vector baseline_ptr(F * P); + for (size_t f = 0; f < F; ++f) { + for (size_t b = 0; b < P; ++b) { + const size_t owner_idx = plan.owner_batch_index[b]; + const size_t blk = plan.block_idx[f * P + b]; + baseline_ptr[f * P + b] = states[owner_idx].data() + blk * ambient_size[b]; + } + } + const float **ptrs_host = + EnsurePinnedHost(cache.pinned_ptrs_host, cache.pinned_ptrs_capacity, S * F * P); + for (size_t s = 0; s < S; ++s) { + const size_t b = cache.slot_b[s]; + const float *xpd_base = cache.x_plus_delta_scratch.data() + cache.xpd_offset[s]; + for (size_t f = 0; f < F; ++f) { + for (size_t bb = 0; bb < P; ++bb) { + if (bb == b) { + const size_t blk = plan.block_idx[f * P + bb]; + ptrs_host[(s * F + f) * P + bb] = xpd_base + blk * ambient_size[b]; + } else { + ptrs_host[(s * F + f) * P + bb] = baseline_ptr[f * P + bb]; + } + } + } + } + THROW_ON_CUDA_ERROR(cudaMemcpyAsync(cache.state_pointer_scratch.data(), ptrs_host, + S * F * P * sizeof(const float *), cudaMemcpyHostToDevice, + stream)); + + // ---- Differencing kernel's static index/epsilon arrays: also + // ---- structure-only, uploaded once via one merged pinned staging + // ---- buffer (col_idx | plus_slot | minus_slot back-to-back) instead of + // ---- three separate transfers. ---- + int *int_host = EnsurePinnedHost(cache.pinned_int_host, cache.pinned_int_capacity, 3 * W); + std::copy(cache.col_idx_h.begin(), cache.col_idx_h.end(), int_host); + std::copy(cache.plus_slot_h.begin(), cache.plus_slot_h.end(), int_host + W); + std::copy(cache.minus_slot_h.begin(), cache.minus_slot_h.end(), int_host + 2 * W); + THROW_ON_CUDA_ERROR(cudaMemcpyAsync(cache.col_idx_scratch.data(), int_host, W * sizeof(int), + cudaMemcpyHostToDevice, stream)); + THROW_ON_CUDA_ERROR(cudaMemcpyAsync(cache.plus_slot_scratch.data(), int_host + W, + W * sizeof(int), cudaMemcpyHostToDevice, stream)); + THROW_ON_CUDA_ERROR(cudaMemcpyAsync(cache.minus_slot_scratch.data(), int_host + 2 * W, + W * sizeof(int), cudaMemcpyHostToDevice, stream)); + + float *eps_host = EnsurePinnedHost(cache.pinned_eps_host, cache.pinned_eps_capacity, W); + std::copy(cache.eps_h.begin(), cache.eps_h.end(), eps_host); + THROW_ON_CUDA_ERROR(cudaMemcpyAsync(cache.eps_scratch.data(), eps_host, W * sizeof(float), + cudaMemcpyHostToDevice, stream)); + + // Mark all of this cache's pinned buffers as "safe to touch again once + // this event completes" -- checked at the top of the next rebuild (or + // in the destructor, on teardown) before any of them are reused, grown, + // or freed. + if (cache.pinned_upload_done_event == nullptr) { + THROW_ON_CUDA_ERROR( + cudaEventCreateWithFlags(&cache.pinned_upload_done_event, cudaEventDisableTiming)); + } + THROW_ON_CUDA_ERROR(cudaEventRecord(cache.pinned_upload_done_event, stream)); + + cache.uploaded = true; + cache.central = central; + cache.step_size = options.relative_step_size; + cache.last_owner_data_ptr = owner_data_ptr; + cache.last_xpd_base = cache.x_plus_delta_scratch.data(); + } else { + // ---- Fast path: structure, options, and all relevant addresses are + // ---- unchanged since the last call, so `delta`, the replicated + // ---- state-pointer table, and the differencing kernel's index/epsilon + // ---- arrays are already correct on the device -- only the actual + // ---- perturbed evaluations (which read the *current* state values) + // ---- need to happen. No H2D copies, no stream-pool synchronization. ---- + for (size_t s = 0; s < S; ++s) { + StateBatch *owner = state_batches[cache.slot_owner[s]]; + const float *x = states[cache.slot_owner[s]].data(); + const float *delta = cache.delta_scratch.data() + cache.delta_offset[s]; + float *xpd = cache.x_plus_delta_scratch.data() + cache.xpd_offset[s]; + owner->Plus(x, delta, xpd, stream); + } + } + + // ---- Residual-only evaluate, once per slot, queued back-to-back on + // ---- `stream` with no synchronization in between (see the class-level + // ---- comment in the header for why this cannot be collapsed into a + // ---- single launch for arbitrary, unmodified shipped factor batches). ---- + for (size_t s = 0; s < S; ++s) { + float *residuals_out = cache.perturbed_residuals.data() + s * F * residual_size; + const float *const *ptrs = cache.state_pointer_scratch.data() + s * F * P; + factor_batch->Evaluate(residuals_out, nullptr, ptrs, stream); + } + + // ---- Differencing kernel: one launch, fully data-parallel over + // ---- F * W * ResidualsSize() elements. ---- + const long long total_elements = static_cast(F) * W * residual_size; + const int num_blocks = static_cast((total_elements + kBlockSize - 1) / kBlockSize); + NumericDiffColumnKernel<<>>( + cache.perturbed_residuals.data(), baseline_residuals, cache.col_idx_scratch.data(), + cache.plus_slot_scratch.data(), cache.minus_slot_scratch.data(), cache.eps_scratch.data(), + central, static_cast(F), static_cast(W), static_cast(residual_size), + static_cast(total_cols), jacobian_out); + THROW_ON_CUDA_ERROR(cudaGetLastError()); +} + +} // namespace cunls diff --git a/cunls/minimizer/numeric_diff_jacobian.h b/cunls/minimizer/numeric_diff_jacobian.h new file mode 100644 index 0000000..126079f --- /dev/null +++ b/cunls/minimizer/numeric_diff_jacobian.h @@ -0,0 +1,231 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include + +#include +#include +#include + +#include "cunls/common/device_vector.h" +#include "cunls/common/types.h" +#include "cunls/minimizer/jacobian_mode.h" + +namespace cunls { + +class Problem; +class MinimizerState; + +/** + * @brief Builds finite-difference Jacobians for a residual batch, on the + * manifold tangent space of each referenced state block. + * + * This is the numeric-diff counterpart to a FactorBatch's own analytic + * `Evaluate(..., jacobians, ...)`: it fills the exact same dense per-factor + * `ResidualsSize() x sum(StateBlockSizes())` row-major Jacobian layout + * (`PerFactorJacobians`, see `cunls/common/types.h`), using only the factor + * batch's residual-only evaluation (`FactorBatch::Evaluate` with + * `jacobians == nullptr`) plus each referenced `StateBatch::Plus` for + * manifold-correct perturbations. No per-factor-type code changes are + * required. + * + * Perturbations are generated by perturbing one tangent coordinate of one + * referenced state-block *position* at a time (shared across all factors in + * the batch, since a residual batch's position `b` always refers to the same + * `StateBatch` type), but multiple positions and dofs are perturbed and + * evaluated with **no synchronization in between**: `Plus()` calls for + * different (position, dof, sign) triples are spread across a small + * `cudaStream_t` pool so they can overlap on the GPU, and the resulting + * residual evaluations are queued back-to-back on the caller's stream with + * no intervening `cudaStreamSynchronize` (only stream/event dependencies). + * See `numeric_diff_jacobian.cu` for why this design does not batch all + * perturbed evaluations into a single `FactorBatch::Evaluate` launch (as an + * idealized design would): shipped factor batches (e.g. + * `VectorBetweenFactorBatch`) bake their own `NumFactors()` into a stored + * constructor member consumed directly by the underlying kernel launch, not + * derived from the size of the `state_pointers`/`residuals` arrays passed to + * `Evaluate`, so a generic wrapper cannot present a larger factor count to + * an existing, unmodified factor batch's `Evaluate()` without also + * replicating that factor batch's private per-factor data (e.g. between- + * factor deltas), which requires factor-specific knowledge this class does + * not have. + * + * Structural (factor, state-block-position) -> (state batch, block index) + * resolution is precomputed once per residual batch (via `PrepareResidualBatch`, + * called lazily by `Compute` on first use) from `Problem`'s host-side + * pointer lists, and reused on every subsequent `Compute` call for that + * residual batch -- it depends only on factor-graph connectivity, which is + * static for the lifetime of a `Problem`/minimizer run. + */ +class NumericDiffJacobianBuilder { + public: + NumericDiffJacobianBuilder(); + ~NumericDiffJacobianBuilder(); + + NumericDiffJacobianBuilder(const NumericDiffJacobianBuilder &) = delete; + NumericDiffJacobianBuilder &operator=(const NumericDiffJacobianBuilder &) = delete; + + /** + * @brief Precomputes the (factor, position) -> (state batch, block index) + * mapping for one residual batch. + * + * Safe to call multiple times (e.g. if the problem structure changes); + * cheap, host-only work. `Compute` calls this automatically the first time + * it sees a given `residual_batch_index`, so most callers never need to + * call this directly. + * + * @param problem The owning problem (must have both the residual batch and + * all referenced state batches registered). + * @param residual_batch_index Index into `problem.GetResidualBatches()`. + */ + void PrepareResidualBatch(const Problem &problem, size_t residual_batch_index); + + /** + * @brief Computes the numeric-diff Jacobian for one residual batch. + * + * @param stream CUDA stream on which the result becomes valid once all + * work queued here (and on the internal stream pool) completes; safe to + * enqueue further work depending on `jacobian_out` on this same stream + * immediately after this call returns (no synchronize is performed here). + * @param problem The owning problem. + * @param residual_batch_index Index into `problem.GetResidualBatches()`. + * @param minimizer_state Current minimizer state (source of the state + * values being perturbed). + * @param baseline_residuals Device pointer to the already-computed RAW + * (pre-loss) residuals for this batch (`NumFactors() * ResidualsSize()` + * floats), as produced by `FactorBatch::Evaluate(residuals, nullptr, ...)`. + * Used directly for forward differencing; unused (but harmless if + * provided) for central differencing. + * @param jacobian_out Device pointer to this batch's dense per-factor + * Jacobian output region (same layout `FactorBatch::Evaluate` would have + * written). + * @param options Finite-difference method and step size. + */ + void Compute(cudaStream_t stream, const Problem &problem, size_t residual_batch_index, + const MinimizerState &minimizer_state, const float *baseline_residuals, + float *jacobian_out, const NumericDiffOptions &options); + + private: + /** @brief Per-residual-batch structural perturbation plan (host-only). */ + struct BatchPlan { + size_t num_factors = 0; + size_t num_positions = 0; + std::vector state_block_sizes; ///< Per position b. + std::vector col_offsets; ///< Per position b, prefix sum of state_block_sizes. + std::vector + owner_batch_index; ///< Per position b: index into problem.GetStateBatches(). + std::vector + block_idx; ///< Flattened [f * num_positions + b] -> block index within owner. + }; + + /** + * @brief Per-residual-batch cached slot layout, device scratch, and + * upload-validity bookkeeping. + * + * All of the host-built arrays this class uploads to the device each + * `Compute` call (`delta`, the replicated state-pointer table, and the + * differencing kernel's index/epsilon arrays) depend *only* on: (a) the + * static factor-graph structure captured in `BatchPlan`, (b) `options`, + * and (c) the *addresses* of the owning `StateBatch` buffers and of this + * cache's own `x_plus_delta_scratch` allocation -- never on the current + * state *values*, which are read directly on-device by `Plus`/`Evaluate`. + * For a given `Problem`/minimizer run those addresses are stable across + * `BuildSystem` calls (state buffers are value-copied in place, not + * reallocated, once the minimizer's state snapshot is created), so this + * cache lets every `Compute` call after the first skip all host-side + * rebuilding and H2D uploads entirely, re-validated cheaply (a handful of + * pointer comparisons) on every call in case that assumption ever doesn't + * hold (e.g. structure changed via a fresh `PrepareResidualBatch`, or the + * caller passed a `MinimizerState` backed by different buffers). + */ + struct ComputeCache { + bool uploaded = false; + bool central = false; + float step_size = 0.0f; + size_t S = 0, W = 0, F = 0, P = 0; + size_t residual_size = 0, total_cols = 0; + size_t delta_total = 0, xpd_total = 0; + + // Host-side slot layout, built once per (plan, options) pair. + std::vector slot_b, slot_k, slot_owner; + std::vector slot_sign; + std::vector delta_offset, xpd_offset; + std::vector col_idx_h, plus_slot_h, minus_slot_h; + std::vector eps_h; + + // Address fingerprint of the last successful upload, used to cheaply + // detect whether the cached device data is still valid. + std::vector last_owner_data_ptr; // size P. + const float *last_xpd_base = nullptr; + + // Per-residual-batch device scratch (never shared across residual + // batches, so caching is always safe even when a Problem has several + // numeric-diff residual batches evaluated back-to-back on one stream). + dvector delta_scratch; + dvector x_plus_delta_scratch; + dvector perturbed_residuals; + dvector state_pointer_scratch; + dvector col_idx_scratch; + dvector plus_slot_scratch; + dvector minus_slot_scratch; + dvector eps_scratch; + + // Small pinned host staging buffers for this cache's (rare -- only on + // rebuild) H2D uploads, reused/grown across calls; pageable + // std::vector-backed uploads showed up as ~35x slower cudaMemcpyAsync + // calls under nsys (internally staged through a driver bounce buffer). + // Deliberately per-cache, not shared across residual batches: a shared + // buffer could be overwritten by a second residual batch's rebuild + // before the first batch's async H2D copy had actually finished reading + // from it (the event/stream dependencies below only order *device* + // work, not host-side reuse of the pinned source buffer). + float *pinned_delta_host = nullptr; + size_t pinned_delta_capacity = 0; + const float **pinned_ptrs_host = nullptr; + size_t pinned_ptrs_capacity = 0; + int *pinned_int_host = nullptr; // Holds col_idx/plus_slot/minus_slot back-to-back. + size_t pinned_int_capacity = 0; + float *pinned_eps_host = nullptr; + size_t pinned_eps_capacity = 0; + + // Recorded on `stream` right after the last H2D copy that reads from + // the pinned buffers above; synchronized before those buffers are ever + // touched again (grown, overwritten, or freed) so a still-in-flight + // async upload from a prior rebuild can never race with the next one. + cudaEvent_t pinned_upload_done_event = nullptr; + + ComputeCache() = default; + ~ComputeCache(); + ComputeCache(ComputeCache &&) = default; + ComputeCache &operator=(ComputeCache &&) = default; + ComputeCache(const ComputeCache &) = delete; + ComputeCache &operator=(const ComputeCache &) = delete; + }; + + void EnsureStreamPool(size_t num_streams); + + std::unordered_map plans_; + std::unordered_map caches_; + + std::vector pool_streams_; + std::vector pool_events_; + cudaEvent_t delta_ready_event_ = nullptr; +}; + +} // namespace cunls diff --git a/cunls/minimizer/problem.cpp b/cunls/minimizer/problem.cpp index f15628f..3dc1292 100644 --- a/cunls/minimizer/problem.cpp +++ b/cunls/minimizer/problem.cpp @@ -30,10 +30,11 @@ namespace cunls { * @param factor_batch Pointer to the factor batch. * @param state_pointers Device pointers to state blocks. */ -void Problem::AddFactorBatch(FactorBatch *factor_batch, - const std::vector &state_pointers) { +void Problem::AddFactorBatch(FactorBatch *factor_batch, const std::vector &state_pointers, + std::optional jacobian_mode_override) { state_pointers_.emplace_back(state_pointers); residual_batches_.emplace_back(factor_batch, nullptr); + jacobian_mode_overrides_.emplace_back(jacobian_mode_override); } /** @@ -46,11 +47,12 @@ void Problem::AddFactorBatch(FactorBatch *factor_batch, * @param loss_function_batch Pointer to the loss function batch. * @param state_pointers Device pointers to state blocks. */ -void Problem::AddFactorBatch(FactorBatch *factor_batch, - LossFunctionBatch *loss_function_batch, - const std::vector &state_pointers) { +void Problem::AddFactorBatch(FactorBatch *factor_batch, LossFunctionBatch *loss_function_batch, + const std::vector &state_pointers, + std::optional jacobian_mode_override) { state_pointers_.emplace_back(state_pointers); residual_batches_.emplace_back(factor_batch, loss_function_batch); + jacobian_mode_overrides_.emplace_back(jacobian_mode_override); } /** @@ -58,9 +60,7 @@ void Problem::AddFactorBatch(FactorBatch *factor_batch, * * @param state_batch Pointer to the state batch. */ -void Problem::AddStateBatch(StateBatch *state_batch) { - state_batches_.push_back(state_batch); -} +void Problem::AddStateBatch(StateBatch *state_batch) { state_batches_.push_back(state_batch); } /** * @brief Validates that all inputs are non-null and sizes are consistent. @@ -181,18 +181,24 @@ bool Problem::CheckConsistency() const { } /** @brief Gets the residual batches. */ -const std::vector &Problem::GetResidualBatches() const { - return residual_batches_; -} +const std::vector &Problem::GetResidualBatches() const { return residual_batches_; } /** @brief Gets the state batches. */ -const std::vector &Problem::GetStateBatches() const { - return state_batches_; -} +const std::vector &Problem::GetStateBatches() const { return state_batches_; } /** @brief Gets the per-residual-batch state pointer arrays. */ const std::vector> &Problem::GetStatePointers() const { return state_pointers_; } -} // namespace cunls +/** @brief Resolves the effective Jacobian mode for a residual batch. */ +JacobianMode Problem::JacobianModeFor(size_t residual_batch_index, + JacobianMode global_default) const { + if (residual_batch_index < jacobian_mode_overrides_.size() && + jacobian_mode_overrides_[residual_batch_index].has_value()) { + return *jacobian_mode_overrides_[residual_batch_index]; + } + return global_default; +} + +} // namespace cunls diff --git a/cunls/minimizer/problem.h b/cunls/minimizer/problem.h index be33d2b..2bbb2b8 100644 --- a/cunls/minimizer/problem.h +++ b/cunls/minimizer/problem.h @@ -17,10 +17,12 @@ #pragma once +#include #include #include #include "cunls/factor/factor_batch.h" +#include "cunls/minimizer/jacobian_mode.h" #include "cunls/minimizer/residual_batch.h" #include "cunls/robustifier/loss_function_batch.h" #include "cunls/state/state_batch.h" @@ -40,7 +42,7 @@ namespace cunls { * instance to its corresponding state blocks on the GPU. */ class Problem { -public: + public: /** * @brief Adds a factor batch without a loss function. * @@ -52,9 +54,14 @@ class Problem { * @param state_pointers Host-side list of device pointers to state blocks for * each factor instance, flattened in row-major order: [cf0_state0, * cf0_state1, ..., cfN_stateM]. The problem stores a copy on the host. + * @param jacobian_mode_override Optional per-group override of the + * minimizer's global `MinimizerOptions::jacobian_mode`. When set, this + * factor batch always uses the given mode regardless of the minimizer's + * default; when `std::nullopt` (default), the minimizer's global default + * applies. See `JacobianModeFor`. */ - void AddFactorBatch(FactorBatch *factor_batch, - const std::vector &state_pointers); + void AddFactorBatch(FactorBatch *factor_batch, const std::vector &state_pointers, + std::optional jacobian_mode_override = std::nullopt); /** * @brief Adds a factor batch with a robust loss function. @@ -68,10 +75,13 @@ class Problem { * @param state_pointers Host-side list of device pointers to state blocks for * each factor instance, flattened in row-major order: [cf0_state0, * cf0_state1, ..., cfN_stateM]. The problem stores a copy on the host. + * @param jacobian_mode_override Optional per-group override of the + * minimizer's global `MinimizerOptions::jacobian_mode`; see the other + * `AddFactorBatch` overload. */ - void AddFactorBatch(FactorBatch *factor_batch, - LossFunctionBatch *loss_function_batch, - const std::vector &state_pointers); + void AddFactorBatch(FactorBatch *factor_batch, LossFunctionBatch *loss_function_batch, + const std::vector &state_pointers, + std::optional jacobian_mode_override = std::nullopt); /** * @brief Adds a state batch to the problem. @@ -121,7 +131,20 @@ class Problem { */ const std::vector> &GetStatePointers() const; -private: + /** + * @brief Resolves the effective Jacobian mode for a residual batch. + * + * Returns the per-group override registered via `AddFactorBatch` if one + * was given, otherwise `global_default` (typically + * `MinimizerOptions::jacobian_mode`). + * + * @param residual_batch_index Index into `GetResidualBatches()`. + * @param global_default Minimizer-wide default mode. + * @return The effective JacobianMode for this residual batch. + */ + JacobianMode JacobianModeFor(size_t residual_batch_index, JacobianMode global_default) const; + + private: /** * @brief Validates that all inputs are non-null and sizes are consistent. * @@ -139,13 +162,15 @@ class Problem { */ bool CheckGraphConnectivity() const; -private: - std::vector - residual_batches_; ///< Registered residual batches. - std::vector state_batches_; ///< Registered state batches. - std::vector> - state_pointers_; ///< Host copies of per-residual-batch state pointer - ///< lists. + private: + std::vector residual_batches_; ///< Registered residual batches. + std::vector state_batches_; ///< Registered state batches. + std::vector> state_pointers_; ///< Host copies of per-residual-batch state + ///< pointer lists. + std::vector> + jacobian_mode_overrides_; ///< Per-residual-batch JacobianMode + ///< override, index-aligned with + ///< residual_batches_. }; -} // namespace cunls +} // namespace cunls diff --git a/cunls/minimizer/residual_batch.cu b/cunls/minimizer/residual_batch.cu index 9666095..ba7974b 100644 --- a/cunls/minimizer/residual_batch.cu +++ b/cunls/minimizer/residual_batch.cu @@ -199,7 +199,6 @@ bool ResidualBatch::Evaluate(cudaStream_t stream, float *workspace, float *resid float const *const *state_pointers, float *cost, float *jacobians) const { int num_residuals = static_cast(factor_batch_->NumFactors()); - int residual_dim = static_cast(factor_batch_->ResidualsSize()); // Return before the preconditions below: an empty batch has nothing to // evaluate, its buffers are legitimately null (a zero-size DeviceVector has @@ -215,6 +214,21 @@ bool ResidualBatch::Evaluate(cudaStream_t stream, float *workspace, float *resid factor_batch_->Evaluate(residuals, jacobians, state_pointers, stream); + return ApplyLoss(stream, workspace, residuals, cost, jacobians); +} + +bool ResidualBatch::ApplyLoss(cudaStream_t stream, float *workspace, float *residuals, float *cost, + float *jacobians) const { + int num_residuals = static_cast(factor_batch_->NumFactors()); + int residual_dim = static_cast(factor_batch_->ResidualsSize()); + + if (num_residuals == 0) { + return true; + } + + assert(residuals != nullptr); + assert(workspace != nullptr); + float *sq_err_ptr = nullptr; float3 *rho_ptr = nullptr; MapRobustWorkspace(workspace, num_residuals, &sq_err_ptr, &rho_ptr); diff --git a/cunls/minimizer/residual_batch.h b/cunls/minimizer/residual_batch.h index 73ee510..09abf1c 100644 --- a/cunls/minimizer/residual_batch.h +++ b/cunls/minimizer/residual_batch.h @@ -17,10 +17,10 @@ #pragma once -#include - #include +#include + #include "cunls/factor/factor_batch.h" #include "cunls/robustifier/loss_function_batch.h" @@ -53,8 +53,7 @@ inline size_t ResidualBatchWorkspaceSizeBytes(size_t num_residuals) { * `buffer_`). */ inline size_t ResidualBatchWorkspaceNumFloats(size_t num_residuals) { - return (ResidualBatchWorkspaceSizeBytes(num_residuals) + sizeof(float) - 1u) / - sizeof(float); + return (ResidualBatchWorkspaceSizeBytes(num_residuals) + sizeof(float) - 1u) / sizeof(float); } /** @@ -66,7 +65,7 @@ inline size_t ResidualBatchWorkspaceNumFloats(size_t num_residuals) { * scale residuals and Jacobians, and optionally computes per-residual costs. */ class ResidualBatch { -public: + public: /** * @brief Constructs a residual batch from a factor batch and loss function. * @@ -109,8 +108,33 @@ class ResidualBatch { * @return True on success. */ bool Evaluate(cudaStream_t stream, float *workspace, float *residuals, - float const *const *state_pointers, float *cost, - float *jacobians) const; + float const *const *state_pointers, float *cost, float *jacobians) const; + + /** + * @brief Applies this batch's loss function to an already-computed raw + * (pre-loss) residual/Jacobian pair, in place. + * + * Factors out exactly the post-`FactorBatch::Evaluate` tail of `Evaluate` + * (loss evaluation, residual scaling, Jacobian scaling, cost extraction) so + * callers that compute raw residuals/Jacobians through a path other than + * this class's `Evaluate` (e.g. numeric-diff Jacobians, which call + * `FactorBatch::Evaluate` directly) can still apply the exact same loss + * handling `Evaluate` would have applied. No-op work beyond the trivial + * squared-error/cost bookkeeping when `GetLossFunction() == nullptr`. + * + * @param stream CUDA stream used for all kernels launched by this call. + * @param workspace Device scratch; same sizing contract as `Evaluate`'s + * `workspace` parameter. + * @param residuals Device array of raw residuals (`NumFactors() * + * ResidualsSize()` floats), scaled in place. + * @param cost Optional device array of length `NumFactors()`, filled if + * non-null. + * @param jacobians Optional raw Jacobian blocks (same layout as + * `FactorBatch::Evaluate`), scaled in place if non-null. + * @return True on success. + */ + bool ApplyLoss(cudaStream_t stream, float *workspace, float *residuals, float *cost, + float *jacobians) const; /** * @brief Gets the factor batch. @@ -126,9 +150,8 @@ class ResidualBatch { */ LossFunctionBatch *GetLossFunction() const { return loss_function_; } -private: - FactorBatch *factor_batch_ = nullptr; ///< Factor batch. - LossFunctionBatch *loss_function_ = - nullptr; ///< Optional loss function batch. + private: + FactorBatch *factor_batch_ = nullptr; ///< Factor batch. + LossFunctionBatch *loss_function_ = nullptr; ///< Optional loss function batch. }; -} // namespace cunls +} // namespace cunls diff --git a/docs/sphinx/api/factor.rst b/docs/sphinx/api/factor.rst index be08ca6..f91dd56 100644 --- a/docs/sphinx/api/factor.rst +++ b/docs/sphinx/api/factor.rst @@ -52,6 +52,15 @@ Abstract base (:code:`cunls/factor/factor_batch.h`). :returns: [out] Number of factors in the batch. +**Residual-only factors.** ``Evaluate`` must support ``jacobians == nullptr`` +(residual-only evaluation) — this is required for cost-only evaluation, and +it is also all that's needed to opt a factor into cuNLS's numeric +(finite-difference) Jacobians: a factor whose ``Evaluate`` never writes to +``jacobians`` at all still satisfies this interface, and can be solved by +registering it with ``JacobianMode::kNumeric`` (see +:doc:`../numeric_jacobians` and :ref:`minimizer-jacobian-mode-label`) instead +of implementing a Jacobian by hand. + SizedFactorBatch ---------------------------------------------------- @@ -1801,6 +1810,16 @@ and Jacobian computation that is not available as a built-in factor. Return ``True`` on success. The default implementation raises ``NotImplementedError``. +**Skipping the Jacobian entirely.** ``evaluate`` only has to write to +``jacobians_ptr`` when it is non-zero and you intend to supply an analytic +Jacobian. A custom factor that never writes to it — even when +``jacobians_ptr`` is non-zero — still satisfies the contract, and can be +registered with ``jacobian_mode_override=pycunls.JacobianMode.numeric`` in +:py:meth:`Problem.add_factor_batch` to have cuNLS differentiate it via +finite differences instead. See :doc:`../numeric_jacobians` for details and +:ref:`pycunls_tutorial:Custom Factor with a Numeric Jacobian` for a worked +Python example. + .. _py-warp-factor-batch: ``pycunls.warp.WarpFactorBatch`` diff --git a/docs/sphinx/api/minimizer.rst b/docs/sphinx/api/minimizer.rst index ab895c5..6936640 100644 --- a/docs/sphinx/api/minimizer.rst +++ b/docs/sphinx/api/minimizer.rst @@ -163,6 +163,55 @@ Used when constructing a :code:`GaussNewtonMinimizer`. validation), which can reduce per-iteration latency for small systems but may produce silently incorrect results for singular or ill-conditioned matrices. Default: ``true``. +- **jacobian_mode** [in]: Global default :code:`JacobianMode` used to + evaluate every factor batch's Jacobian, unless overridden per group via + :cpp:func:`Problem::AddFactorBatch`. See + :ref:`minimizer-jacobian-mode-label` below and :doc:`../numeric_jacobians` + for the full picture. Default: ``kAnalytic``. +- **numeric_diff_options** [in]: Tuning knobs (finite-difference scheme, step + size) used whenever a factor batch is evaluated with + ``JacobianMode::kNumeric``; see :code:`NumericDiffOptions` below. Ignored + for factor batches evaluated with ``kAnalytic``. + +.. _minimizer-jacobian-mode-label: + +-------------------------------------------------------------------------------- +:code:`JacobianMode` / :code:`NumericDiffOptions` +-------------------------------------------------------------------------------- + +Header: :code:`cunls/minimizer/jacobian_mode.h`. Selects, per factor batch, +whether its Jacobian comes from the factor's own hand-derived +:cpp:func:`FactorBatch::Evaluate` output, or from cuNLS differentiating the +factor's residual numerically. See :doc:`../numeric_jacobians` for a full +walkthrough (manifold-aware perturbation, accuracy/performance tradeoffs, +worked example). + +:code:`JacobianMode` (enum): + +- **kAnalytic**: Use the factor batch's own Jacobian output. Default. +- **kNumeric**: Ignore any Jacobian the factor batch would compute; instead + perturb each referenced state block along its manifold tangent space (via + :cpp:func:`StateBatch::Plus`) and finite-difference the residual. Requires + only that the factor batch support residual-only evaluation + (``jacobians == nullptr``), which every :cpp:class:`FactorBatch` must + already do. + +:code:`NumericDiffOptions` (struct, only consulted when a factor batch +resolves to ``kNumeric``): + +- **method** [in]: ``kForward`` (one-sided, :math:`(f(x+\epsilon)-f(x))/\epsilon`, + cheaper and less accurate) or ``kCentral`` (two-sided, + :math:`(f(x+\epsilon)-f(x-\epsilon))/(2\epsilon)`). Default: ``kCentral``. +- **relative_step_size** [in]: Per-tangent-coordinate perturbation step + :math:`\epsilon`. Default: 1e-4. + +Where the mode for a given factor batch comes from: + +.. cpp:function:: JacobianMode Problem::JacobianModeFor(size_t residual_batch_index, JacobianMode global_default) const + + :param ``residual_batch_index``: [in] Index into :cpp:func:`Problem::GetResidualBatches`. + :param ``global_default``: [in] Typically :code:`MinimizerOptions::jacobian_mode`. + :returns: [out] The per-group override passed to :cpp:func:`Problem::AddFactorBatch`, if one was given; otherwise ``global_default``. -------------------------------------------------------------------------------- :code:`LevenbergMarquardtMinimizerOptions` @@ -259,17 +308,19 @@ the problem and binds its factor instances to state block pointers. The ordering of :code:`state_pointers` must match the factor batch’s expected state layout (see :doc:`factor`). -.. cpp:function:: void AddFactorBatch(FactorBatch* factor_batch, const std::vector& state_pointers) +.. cpp:function:: void AddFactorBatch(FactorBatch* factor_batch, const std::vector& state_pointers, std::optional jacobian_mode_override = std::nullopt) :param ``factor_batch``: [in] Factor batch pointer (non-owning). :param ``state_pointers``: [in] Flattened device pointers: one per (factor index, state block), mapping factors to state. The problem stores a **host** copy of this list (each entry is still a device ``float*``); no device allocation is used for the table itself. + :param ``jacobian_mode_override``: [in] When set, this factor batch always uses the given :code:`JacobianMode` regardless of the minimizer's :code:`MinimizerOptions::jacobian_mode` default; see :ref:`minimizer-jacobian-mode-label`. Default: ``std::nullopt`` (use the minimizer's global default). :returns: [out] No return value. -.. cpp:function:: void AddFactorBatch(FactorBatch* factor_batch, LossFunctionBatch* loss_function_batch, const std::vector& state_pointers) +.. cpp:function:: void AddFactorBatch(FactorBatch* factor_batch, LossFunctionBatch* loss_function_batch, const std::vector& state_pointers, std::optional jacobian_mode_override = std::nullopt) :param ``factor_batch``: [in] Factor batch pointer (non-owning). :param ``loss_function_batch``: [in] Robust loss batch pointer (non-owning). :param ``state_pointers``: [in] Flattened state pointer mapping for all factors in the batch (stored on the host as above). + :param ``jacobian_mode_override``: [in] Same meaning as the other overload. :returns: [out] No return value. .. _problem-add-state-label: diff --git a/docs/sphinx/api/robustifier.rst b/docs/sphinx/api/robustifier.rst index cc56c88..ccd7287 100644 --- a/docs/sphinx/api/robustifier.rst +++ b/docs/sphinx/api/robustifier.rst @@ -324,9 +324,9 @@ gradient and Gauss-Newton system without recomputing :math:`\rho`. output :math:`(\rho(s), \rho'(s), \rho''(s))` is used to compute :math:`\alpha` and the scaling factors :math:`\sqrt{\rho'}` and :math:`(1-\alpha)^{-1}` applied to residuals and Jacobians in the solver. - For more detail, see the Ceres Solver documentation on - `LossFunction `_ - and the references therein (e.g. Triggs). + This is the standard "Triggs correction" used by robust nonlinear + least-squares solvers to keep a Gauss-Newton-style Jacobian approximation + valid under a robust loss. ================================================================================ Python API (``pycunls``) diff --git a/docs/sphinx/index.rst b/docs/sphinx/index.rst index 1f7a4ca..dac062a 100644 --- a/docs/sphinx/index.rst +++ b/docs/sphinx/index.rst @@ -25,6 +25,7 @@ User Guide installation tutorial quick_start + numeric_jacobians testing licensing diff --git a/docs/sphinx/numeric_jacobians.rst b/docs/sphinx/numeric_jacobians.rst new file mode 100644 index 0000000..f9ec85c --- /dev/null +++ b/docs/sphinx/numeric_jacobians.rst @@ -0,0 +1,156 @@ +############################################################################### +Numeric (finite-difference) Jacobians +############################################################################### + +Every shipped cuNLS factor computes its Jacobian analytically: a hand-derived +closed form, evaluated in the same fused CUDA kernel as the residual. That's +the fastest option, but deriving a correct closed-form Jacobian by hand isn't +always worth the effort — especially while prototyping a new factor, or for +a residual that's awkward to differentiate. + +cuNLS can compute the Jacobian for you instead, via finite differences. A +factor that implements **only** a residual (``Evaluate`` never has to write +to its ``jacobians`` argument) can be optimized exactly like any other +factor — you just tell the minimizer to differentiate it numerically. + +=============================================================================== +Enabling numeric Jacobians +=============================================================================== + +The switch is :cpp:enum:`JacobianMode` (``cunls/minimizer/jacobian_mode.h``), +with two values: ``kAnalytic`` (default) and ``kNumeric``. It can be set two +ways: + +- **Globally**, via :code:`MinimizerOptions::jacobian_mode` — applies to + every factor batch in the problem unless overridden. +- **Per factor group**, via the optional last argument of + :cpp:func:`Problem::AddFactorBatch` — overrides the global default for + just that one factor batch. This lets you mix modes in a single + ``Problem``: for example, keep cuNLS's shipped, analytically-differentiated + factors on the fast path while a new factor you're still prototyping uses + numeric differentiation. + +.. code-block:: cpp + + #include "cunls/cunls.h" + + // Option A: set the global default for every factor batch in the problem. + cunls::MinimizerOptions options; + options.jacobian_mode = cunls::JacobianMode::kNumeric; + + // Option B: override just one factor group, leaving everything else + // (including shipped factors) on the global default (kAnalytic here). + cunls::Problem problem; + problem.AddFactorBatch(&my_factor, state_pointers, cunls::JacobianMode::kNumeric); + +A factor doesn't need any special marker to be eligible for numeric +differentiation — every :cpp:class:`FactorBatch` must already support +residual-only evaluation (``jacobians == nullptr``, used for cost-only +evaluation), and that's the only requirement. See :ref:`factor-inputs` in +:doc:`api/factor` and the worked example below. + +=============================================================================== +How it works +=============================================================================== + +Numeric differentiation is manifold-aware: for each tangent-space +coordinate of each state block a factor references, cuNLS perturbs the +state via that state batch's own :cpp:func:`StateBatch::Plus` (the same +retraction the minimizer uses to apply solved steps), evaluates the +residual at the perturbed state, and differences against a second +evaluation (forward difference: the unperturbed baseline; central +difference: the same perturbation in the opposite direction). This means +numeric Jacobians are correct on SO2/SO3/SE2/SE3/Sim2/Sim3/SL4 states, not +just Euclidean ``Vector`` states — there's no need to reason about +exponential maps or local parameterizations yourself. + +:cpp:struct:`NumericDiffOptions` controls the scheme: + +- **method**: ``kCentral`` (default, two-sided, + :math:`(f(x+\epsilon)-f(x-\epsilon))/(2\epsilon)`, more accurate) or + ``kForward`` (one-sided, :math:`(f(x+\epsilon)-f(x))/\epsilon`, cheaper). +- **relative_step_size**: the per-tangent-coordinate perturbation + :math:`\epsilon`. Default: ``1e-4``. + +=============================================================================== +Accuracy and performance +=============================================================================== + +cuNLS is float32 throughout, so numeric Jacobians are finite-difference +*approximations*, not exact derivatives — expect agreement with an analytic +Jacobian to roughly 1e-2–1e-3 relative accuracy with the default central +difference, not machine precision. Loosen `MinimizerOptions::cost_tolerance` +slightly for problems solved entirely with numeric Jacobians if you see +convergence stall just short of an analytic run's final cost. + +Numeric differentiation costs more than an analytic Jacobian: computing it +requires evaluating the factor's residual multiple times per tangent +coordinate (twice per coordinate for central differences), whereas an +analytic factor computes residual and Jacobian together in one kernel. +Internal benchmarking across PGO/SBA/PnP-scale problems shows numeric-diff +Jacobian evaluation taking roughly 3-12x longer than the equivalent analytic +kernel, with the gap widening for factors that touch more state-block tangent +dimensions. In practice this cost is often small relative to the sparse +linear solve that dominates most iterations — but for factors that run at +scale and are worth the extra effort, prefer writing an analytic Jacobian. +Numeric differentiation is best suited to prototyping, one-off factors, or +residuals where a closed-form derivative genuinely isn't worth deriving. + +=============================================================================== +Worked example +=============================================================================== + +``examples/custom_factor/`` solves the same toy problem two ways: once with +a hand-derived analytic Jacobian, once with only a residual. The +residual-only factor: + +.. code-block:: cpp + + // Same residual as the analytic version: r_i = (x_{i+1} - x_i) - m_i. + // No Jacobian code path at all -- `jacobians` is simply never touched. + __global__ void ScalarDifferenceResidualOnlyKernel( + const float *measurements, float const *const *state_pointers, + float *residuals, size_t num_factors) { + const size_t idx = static_cast(blockIdx.x) * blockDim.x + threadIdx.x; + if (idx >= num_factors) return; + const float *left = state_pointers[idx * 2]; + const float *right = state_pointers[idx * 2 + 1]; + if (residuals != nullptr) { + residuals[idx] = (right[0] - left[0]) - measurements[idx]; + } + } + + class ScalarDifferenceResidualOnlyFactorBatch + : public cunls::SizedFactorBatch<1, 1, 1> { + public: + ScalarDifferenceResidualOnlyFactorBatch(const float *measurements, size_t num_factors) + : measurements_(measurements), num_factors_(num_factors) {} + + bool Evaluate(float *residuals, float * /*jacobians*/, + float const *const *state_pointers, cudaStream_t stream) const final { + constexpr int kBlockSize = 256; + const int grid_size = static_cast((num_factors_ + kBlockSize - 1) / kBlockSize); + ScalarDifferenceResidualOnlyKernel<<>>( + measurements_, state_pointers, residuals, num_factors_); + THROW_ON_CUDA_ERROR(cudaGetLastError()); + return true; + } + + size_t NumFactors() const final { return num_factors_; } + + private: + const float *measurements_; + size_t num_factors_; + }; + +Registering it with the per-group override (while an anchor +``PriorFactorBatch`` in the same problem stays analytic): + +.. code-block:: cpp + + problem.AddFactorBatch(&numeric_difference_factor, diff_state_pointers, + cunls::JacobianMode::kNumeric); + problem.AddFactorBatch(&anchor_factor, anchor_state_pointers); // stays analytic + +See ``examples/custom_factor/README.md`` for the full walkthrough and an +"analytic vs. numeric" comparison table. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1a80d22..b02b4c8 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -54,3 +54,8 @@ add_cunls_example( motion_prior_example "${CMAKE_CURRENT_SOURCE_DIR}/motion_prior/main.cpp" ) + +add_cunls_example( + pnp_example + "${CMAKE_CURRENT_SOURCE_DIR}/pnp/main.cpp" +) diff --git a/examples/custom_factor/README.md b/examples/custom_factor/README.md index 3500723..d37e471 100644 --- a/examples/custom_factor/README.md +++ b/examples/custom_factor/README.md @@ -1,26 +1,49 @@ # Custom Factor Example -This example shows how to implement a user-defined factor for `cuNLS`. +This example shows how to implement a user-defined factor for `cuNLS`, in +**two** ways, both solving the same 1D chain problem: -It defines: -- `ScalarDifferenceFactorBatch : SizedFactorBatch<1, 1, 1>` -- CUDA kernel `ScalarDifferenceKernel` +- **Part 1** (`ScalarDifferenceFactorBatch`): implements both the residual + and its analytic Jacobian by hand. +- **Part 2** (`ScalarDifferenceResidualOnlyFactorBatch`): implements only + the residual and relies on cuNLS's numeric (finite-difference) Jacobian + support to differentiate it. See + [Numeric (finite-difference) Jacobians](../../docs/sphinx/numeric_jacobians.rst) + for how this works in general (manifold-aware, mixing modes within one + `Problem`, accuracy/performance tradeoffs). -Residual model for each factor: +Residual model for each factor, shared by both parts: `r_i = (x_{i+1} - x_i) - m_i` -with Jacobians: +Part 1's analytic Jacobians: - `dr/dx_i = -1` - `dr/dx_{i+1} = +1` -The example also adds an anchor prior (`PriorFactorBatch>`, +Part 2's `Evaluate()` only ever writes `residuals` -- there is no Jacobian +code path at all. Its factor group is registered with +`JacobianMode::kNumeric` via `Problem::AddFactorBatch`'s per-group override, +so the minimizer differentiates it via `StateBatch::Plus` perturbations +instead. + +### When to use which + +| | Analytic (Part 1) | Numeric (Part 2) | +|---|---|---| +| Effort to write | Derive + hand-code the Jacobian | Residual only | +| Jacobian evaluation cost | Fastest | Several times slower (roughly 3-12x per Jacobian evaluation in internal benchmarks, problem-dependent) | +| Best for | Factors that ship / run at scale | Prototyping, one-off factors, or residuals that are awkward to differentiate by hand | + +Both parts also add an anchor prior (`PriorFactorBatch>`, the manifold-generic facade specialized to `R^1`) on the first state to -remove global shift ambiguity. +remove global shift ambiguity; the anchor prior is always evaluated +analytically (it's a shipped factor), which in Part 2 also demonstrates +mixing Jacobian modes within a single `Problem`. ## Files -- `main.cu`: custom factor class, kernel, and optimization pipeline. +- `main.cu`: both custom factor classes, kernels, and the shared + optimization pipeline (`RunChainExample`, called once per part). - `../utils/`: shared host-side utilities (validation metrics). - Built by the shared `examples/CMakeLists.txt`. - Exported by the shared `examples/build_in_docker.sh`. @@ -32,8 +55,9 @@ remove global shift ambiguity. 3. Disturb all states to create an initial estimate. 4. Build `VectorStateBatch<1>` for all states. 5. Add: - - custom difference factor batch for all edges - - anchor prior factor for the first node + - the difference factor batch for all edges (analytic in Part 1, + numeric-diff in Part 2) + - anchor prior factor for the first node (always analytic) 6. Solve with `LevenbergMarquardtMinimizer` (the minimizer allocates GPU workspace during initialization). 7. Compare initial vs final MSE to validate improvement. @@ -44,7 +68,8 @@ For this custom factor: - residual size = 1 - state block sizes = [1, 1] - jacobian per factor is therefore `1 x 2` and written as: - `[dres_dleft, dres_dright]` + `[dres_dleft, dres_dright]` (Part 1 only -- Part 2 never writes to the + Jacobian buffer) State pointer layout for factor `i`: - `state_pointers[2*i] -> x_i` diff --git a/examples/custom_factor/main.cu b/examples/custom_factor/main.cu index 95532a5..5f350ba 100644 --- a/examples/custom_factor/main.cu +++ b/examples/custom_factor/main.cu @@ -26,6 +26,7 @@ #include "cunls/common/types.h" #include "cunls/factor/prior/prior_factor_batch.h" #include "cunls/factor/sized_factor_batch.h" +#include "cunls/minimizer/jacobian_mode.h" #include "cunls/minimizer/levenberg_marquardt_minimizer.h" #include "cunls/minimizer/problem.h" #include "cunls/state/vector_state_batch.h" @@ -104,120 +105,223 @@ class ScalarDifferenceFactorBatch : public cunls::SizedFactorBatch<1, 1, 1> { size_t num_factors_; }; +// --------------------------------------------------------------------------- +// Part 2: the same factor, but with only a residual implemented. +// --------------------------------------------------------------------------- +// Deriving a closed-form Jacobian by hand isn't always worth it -- for +// prototyping, or for factors whose residual is awkward to differentiate, +// cuNLS can compute the Jacobian for you via finite differences on the +// manifold tangent space of each referenced state block (see +// `cunls/minimizer/jacobian_mode.h`). All a factor has to do is support +// residual-only evaluation (`jacobians == nullptr`), which every FactorBatch +// must already do for cost-only evaluation. +// +// This kernel is a copy of ScalarDifferenceKernel with the Jacobian branch +// deleted entirely -- there is nothing else to write. +__global__ void ScalarDifferenceResidualOnlyKernel(const float *measurements, + float const *const *state_pointers, + float *residuals, size_t num_factors) { + const size_t idx = static_cast(blockIdx.x) * blockDim.x + threadIdx.x; + if (idx >= num_factors) { + return; + } + + const float *left = state_pointers[idx * 2]; + const float *right = state_pointers[idx * 2 + 1]; + if (residuals != nullptr) { + residuals[idx] = (right[0] - left[0]) - measurements[idx]; + } +} + +// Same SizedFactorBatch<1, 1, 1> shape as Part 1, but Evaluate() only ever +// writes residuals -- it doesn't even look at the `jacobians` argument. +// Registering this factor group with JacobianMode::kNumeric (see main() +// below) tells the minimizer to fill in the Jacobian itself by perturbing +// x_i/x_{i+1} with StateBatch::Plus and differencing the residual, instead +// of calling into a Jacobian code path that doesn't exist here. +class ScalarDifferenceResidualOnlyFactorBatch : public cunls::SizedFactorBatch<1, 1, 1> { + public: + ScalarDifferenceResidualOnlyFactorBatch(const float *measurements, size_t num_factors) + : measurements_(measurements), num_factors_(num_factors) {} + + bool Evaluate(float *residuals, float * /*jacobians*/, float const *const *state_pointers, + cudaStream_t stream) const final { + constexpr int kBlockSize = 256; + const int grid_size = static_cast((num_factors_ + kBlockSize - 1) / kBlockSize); + ScalarDifferenceResidualOnlyKernel<<>>( + measurements_, state_pointers, residuals, num_factors_); + THROW_ON_CUDA_ERROR(cudaGetLastError()); + return true; + } + + size_t NumFactors() const final { return num_factors_; } + + private: + const float *measurements_; + size_t num_factors_; +}; + +} // namespace + +namespace { + +// Shared setup: a 1D chain x_0..x_{N-1} with noisy differences, solved via +// a custom "difference" factor plus an anchor prior. Part 1 uses the +// analytic-Jacobian factor; Part 2 uses the residual-only one and asks the +// minimizer to numerically differentiate it. `use_numeric_jacobian` controls +// which factor class is registered and how. +int RunChainExample(const char *title, bool use_numeric_jacobian) { + // We model a chain of scalar states: + // x_0 -- x_1 -- ... -- x_{N-1} + // + // For N states we have N-1 custom "difference" factors. + const size_t num_states = 256; + const size_t num_diff_factors = num_states - 1; + + // Ground truth states, noisy initialization, and measured differences. + std::vector> gt_states(num_states); + std::vector> initial_states(num_states); + std::vector measurements(num_diff_factors); + + std::mt19937 rng(121314); + std::uniform_real_distribution step_dist(0.2f, 0.6f); + std::uniform_real_distribution noise_dist(-0.35f, 0.35f); + + // Create a monotonic synthetic trajectory. + gt_states[0][0] = 0.5f; + for (size_t i = 1; i < num_states; ++i) { + gt_states[i][0] = gt_states[i - 1][0] + step_dist(rng); + } + + // Disturb all states to create a non-trivial initial estimate. + for (size_t i = 0; i < num_states; ++i) { + initial_states[i][0] = gt_states[i][0] + noise_dist(rng); + } + + // Measurements come from ground truth consecutive differences. + for (size_t i = 0; i < num_diff_factors; ++i) { + measurements[i] = gt_states[i + 1][0] - gt_states[i][0]; + } + + // Copy initial data to device. + dvector> states_device(initial_states); + dvector measurements_device(measurements); + + // Anchor x_0 to remove gauge freedom: + // without this prior, adding a constant offset to all states leaves every + // difference residual unchanged, so the system is rank-deficient. + std::vector> anchor_observation(1); + anchor_observation[0][0] = gt_states[0][0]; + dvector> anchor_observation_device(anchor_observation); + + // Build a single state batch containing all scalar states. + const float *states_ptr = reinterpret_cast(states_device.data()); + cunls::VectorStateBatch<1> state_batch(states_ptr, num_states); + + // Build the anchor prior (always analytic -- it's a shipped factor). + cunls::PriorFactorBatch> anchor_factor( + anchor_observation_device.data(), 1); + + // Build one of the two difference factors depending on which part of the + // example we're running. Only one of these is actually constructed. + ScalarDifferenceFactorBatch analytic_difference_factor(measurements_device.data(), + num_diff_factors); + ScalarDifferenceResidualOnlyFactorBatch numeric_difference_factor(measurements_device.data(), + num_diff_factors); + + // Create state pointer map for all custom factors. + std::vector diff_state_pointers; + diff_state_pointers.reserve(2 * num_diff_factors); + for (size_t i = 0; i < num_diff_factors; ++i) { + diff_state_pointers.push_back(state_batch.StateBlockDevicePtr(i)); + diff_state_pointers.push_back(state_batch.StateBlockDevicePtr(i + 1)); + } + + // State pointer map for the anchor factor: just x_0. + std::vector anchor_state_pointers = {state_batch.StateBlockDevicePtr(0)}; + + // Assemble the optimization problem graph. + cunls::Problem problem; + problem.AddStateBatch(&state_batch); + if (use_numeric_jacobian) { + // Force this factor group to numeric differentiation via the per-group + // override, regardless of the minimizer's global default -- the anchor + // prior above still uses its own analytic Jacobian either way, so this + // also demonstrates mixing modes within a single Problem. + problem.AddFactorBatch(&numeric_difference_factor, diff_state_pointers, + cunls::JacobianMode::kNumeric); + } else { + problem.AddFactorBatch(&analytic_difference_factor, diff_state_pointers); + } + problem.AddFactorBatch(&anchor_factor, anchor_state_pointers); + if (!problem.CheckConsistency()) { + std::cerr << "Problem consistency check failed\n"; + return 1; + } + + // Levenberg-Marquardt options: fairly strict tolerances for this small + // dense-in-logic but sparse-in-structure toy problem. + cunls::MinimizerOptions options; + options.max_num_iterations = 50; + options.state_tolerance = 1e-8f; + options.cost_tolerance = 1e-8f; + + cunls::LevenbergMarquardtMinimizerOptions lm_options; + lm_options.base_options = options; + lm_options.initial_lambda = 1e-3f; + cunls::LevenbergMarquardtMinimizer minimizer(lm_options); + + // Solve on CUDA stream, then synchronize before reading back outputs. + cunls::CudaStream stream; + const auto summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + // Copy optimized states back to host and evaluate reconstruction quality. + std::vector> optimized_states(num_states); + states_device.CopyToHost(optimized_states.data(), num_states); + + const float initial_mse = examples::ComputeVectorMSE(initial_states, gt_states); + const float final_mse = examples::ComputeVectorMSE(optimized_states, gt_states); + + std::cout << title << "\n"; + std::cout << " Initial cost: " << summary.initial_cost << "\n"; + std::cout << " Final cost: " << summary.final_cost << "\n"; + std::cout << " Iterations: " << summary.num_iterations << "\n"; + std::cout << " State MSE: " << initial_mse << " -> " << final_mse << "\n"; + + // Numeric-diff Jacobians are finite-difference approximations (float32, + // central difference by default -- see NumericDiffOptions), so Part 2 + // converges to the same optimum but needs a slightly looser cost + // tolerance than Part 1's exact analytic Jacobian. + const float cost_tolerance = use_numeric_jacobian ? 5e-4f : 1e-5f; + if (summary.final_cost > cost_tolerance || final_mse > initial_mse * 0.02f) { + std::cerr << "Optimization quality check failed.\n"; + return 2; + } + return 0; +} + } // namespace int main() { try { - // We model a chain of scalar states: - // x_0 -- x_1 -- ... -- x_{N-1} - // - // For N states we have N-1 custom "difference" factors. - const size_t num_states = 256; - const size_t num_diff_factors = num_states - 1; - - // Ground truth states, noisy initialization, and measured differences. - std::vector> gt_states(num_states); - std::vector> initial_states(num_states); - std::vector measurements(num_diff_factors); - - std::mt19937 rng(121314); - std::uniform_real_distribution step_dist(0.2f, 0.6f); - std::uniform_real_distribution noise_dist(-0.35f, 0.35f); - - // Create a monotonic synthetic trajectory. - gt_states[0][0] = 0.5f; - for (size_t i = 1; i < num_states; ++i) { - gt_states[i][0] = gt_states[i - 1][0] + step_dist(rng); - } - - // Disturb all states to create a non-trivial initial estimate. - for (size_t i = 0; i < num_states; ++i) { - initial_states[i][0] = gt_states[i][0] + noise_dist(rng); - } - - // Measurements come from ground truth consecutive differences. - for (size_t i = 0; i < num_diff_factors; ++i) { - measurements[i] = gt_states[i + 1][0] - gt_states[i][0]; - } - - // Copy initial data to device. - dvector> states_device(initial_states); - dvector measurements_device(measurements); - - // Anchor x_0 to remove gauge freedom: - // without this prior, adding a constant offset to all states leaves every - // difference residual unchanged, so the system is rank-deficient. - std::vector> anchor_observation(1); - anchor_observation[0][0] = gt_states[0][0]; - dvector> anchor_observation_device(anchor_observation); - - // Build a single state batch containing all scalar states. - const float *states_ptr = reinterpret_cast(states_device.data()); - cunls::VectorStateBatch<1> state_batch(states_ptr, num_states); - - // Build: - // - custom difference factors over edges (x_i, x_{i+1}) - // - one prior factor anchoring x_0 - ScalarDifferenceFactorBatch difference_factor(measurements_device.data(), num_diff_factors); - cunls::PriorFactorBatch> anchor_factor( - anchor_observation_device.data(), 1); - - // Create state pointer map for all custom factors. - std::vector diff_state_pointers; - diff_state_pointers.reserve(2 * num_diff_factors); - for (size_t i = 0; i < num_diff_factors; ++i) { - diff_state_pointers.push_back(state_batch.StateBlockDevicePtr(i)); - diff_state_pointers.push_back(state_batch.StateBlockDevicePtr(i + 1)); - } - - // State pointer map for the anchor factor: just x_0. - std::vector anchor_state_pointers = {state_batch.StateBlockDevicePtr(0)}; - - // Assemble the optimization problem graph. - cunls::Problem problem; - problem.AddStateBatch(&state_batch); - problem.AddFactorBatch(&difference_factor, diff_state_pointers); - problem.AddFactorBatch(&anchor_factor, anchor_state_pointers); - if (!problem.CheckConsistency()) { - std::cerr << "Problem consistency check failed\n"; - return 1; - } - - // Levenberg-Marquardt options: fairly strict tolerances for this small - // dense-in-logic but sparse-in-structure toy problem. - cunls::MinimizerOptions options; - options.max_num_iterations = 50; - options.state_tolerance = 1e-8f; - options.cost_tolerance = 1e-8f; - - cunls::LevenbergMarquardtMinimizerOptions lm_options; - lm_options.base_options = options; - lm_options.initial_lambda = 1e-3f; - cunls::LevenbergMarquardtMinimizer minimizer(lm_options); - - // Solve on CUDA stream, then synchronize before reading back outputs. - cunls::CudaStream stream; - const auto summary = minimizer.Minimize(stream.GetStream(), problem); - THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); - - // Copy optimized states back to host and evaluate reconstruction quality. - std::vector> optimized_states(num_states); - states_device.CopyToHost(optimized_states.data(), num_states); - - const float initial_mse = examples::ComputeVectorMSE(initial_states, gt_states); - const float final_mse = examples::ComputeVectorMSE(optimized_states, gt_states); - - std::cout << "Custom Factor Example\n"; - std::cout << " Initial cost: " << summary.initial_cost << "\n"; - std::cout << " Final cost: " << summary.final_cost << "\n"; - std::cout << " Iterations: " << summary.num_iterations << "\n"; - std::cout << " State MSE: " << initial_mse << " -> " << final_mse << "\n"; - - if (summary.final_cost > 1e-5f || final_mse > initial_mse * 0.02f) { - std::cerr << "Optimization quality check failed.\n"; - return 2; - } - return 0; + // Part 1: a factor that implements both the residual and its analytic + // Jacobian by hand -- the fastest option, worth the extra derivation + // effort for factors that ship at scale. + const int part1_status = RunChainExample("Part 1: analytic Jacobian", false); + + std::cout << "\n"; + + // Part 2: the same factor family, but only the residual is implemented. + // cuNLS fills in the Jacobian via finite differences -- the fastest way + // to get a new factor working, at the cost of extra Jacobian-evaluation + // time (several times slower than the analytic kernel above; see the + // "Numeric (finite-difference) Jacobians" page in the docs for + // benchmarks). Good for prototyping, or factors where a closed-form + // derivative isn't worth deriving. + const int part2_status = RunChainExample("Part 2: residual-only, numeric Jacobian", true); + + return (part1_status != 0) ? part1_status : part2_status; } catch (const std::exception &e) { std::cerr << "Exception: " << e.what() << "\n"; return 3; diff --git a/examples/pnp/README.md b/examples/pnp/README.md new file mode 100644 index 0000000..4291c48 --- /dev/null +++ b/examples/pnp/README.md @@ -0,0 +1,65 @@ +# PnP Example + +This example shows how to use the shipped `PnPFactorBatch` +(`cunls/factor/pnp_factor_batch.h`) to solve a Perspective-n-Point problem: +recover a single camera pose from known 3D world points and their noisy 2D +normalized observations, with the 3D structure held fixed (unlike +`sparse_bundle_adjustment`, which also optimizes the points). + +Residual per correspondence (see `PnPFactorBatch`'s doc comment): + +``` +P_cam = T_cam_from_world * P_world[i] +r_i = [P_cam.x/P_cam.z - obs_i.x, P_cam.y/P_cam.z - obs_i.y] +``` + +Jacobian: `2x6`, pose tangent only (no point derivatives, since the points +are fixed). + +## Jacobian modes + +The example demonstrates **both** `JacobianMode::kAnalytic` (the factor's +own hand-derived 2x6 Jacobian) and `JacobianMode::kNumeric` (finite +differences on the SE(3) tangent space, via `NumericDiffJacobianBuilder`) on +the same synthetic problem, and prints the final cost / pose error for each +so they can be compared directly. + +```bash +./pnp_example # runs both modes, default 2000 points +./pnp_example --num-points 20000 # scale up the correspondence count +./pnp_example --jacobian-mode analytic # only the analytic path +./pnp_example --jacobian-mode numeric # only the numeric-diff path +``` + +## Files + +- `main.cpp`: synthetic PnP dataset generation and the analytic-vs-numeric + optimization pipeline. +- `../utils/`: shared host-side utilities (`camera_utils.h` for + projection/depth, `se3_utils.h` for pose composition and random SE(3) + sampling, `validation.h` for MSE metrics). +- Built by the shared `examples/CMakeLists.txt`. + +## Walkthrough + +1. Generate a ground-truth camera pose (`T_cam_from_world`) and `--num-points` + random 3D world points visible from it. +2. Project each point to a normalized 2D observation and add pixel noise. +3. Perturb the pose to create a non-trivial initial estimate. +4. Build a single `SE3StateBatch` (one pose) and a `PnPFactorBatch` with one + factor per correspondence, all referencing the same pose state block. +5. Solve with `LevenbergMarquardtMinimizer`, once per requested Jacobian + mode, each on a fresh device copy of the perturbed pose so the two runs + are directly comparable. +6. Compare initial vs. final cost and pose MSE for each mode. + +## Build locally (all examples) + +```bash +cmake -S examples -B build/examples/all \ + -DCMAKE_BUILD_TYPE=Release \ + -DCUNLS_INSTALL_DIR=/path/to/cunls_install +cmake --build build/examples/all -j +``` + +Output binary: `pnp_example`. diff --git a/examples/pnp/main.cpp b/examples/pnp/main.cpp new file mode 100644 index 0000000..d75381a --- /dev/null +++ b/examples/pnp/main.cpp @@ -0,0 +1,255 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "cunls/common/cublas_helper.h" +#include "cunls/common/helper.h" +#include "cunls/common/types.h" +#include "cunls/factor/pnp_factor_batch.h" +#include "cunls/minimizer/jacobian_mode.h" +#include "cunls/minimizer/levenberg_marquardt_minimizer.h" +#include "cunls/minimizer/problem.h" +#include "cunls/state/se3_state_batch.h" +#include "utils/camera_utils.h" +#include "utils/se3_utils.h" +#include "utils/validation.h" + +using cunls::dvector; +using cunls::LogError; +using cunls::SE3Transform; +using cunls::Vector; + +namespace { + +// Visibility threshold used when generating synthetic points. +constexpr float kMinDepth = 1.0f; +// Reprojection factor guard threshold to avoid unstable divisions near z=0. +constexpr float kZThreshold = 1e-3f; + +// A single known camera pose (world -> camera), N known 3D world points, and +// their noisy normalized 2D observations. +struct PnPDataset { + SE3Transform gt_pose; + SE3Transform initial_pose; + std::vector> points_world; + std::vector> observations; +}; + +PnPDataset GenerateDataset(size_t num_points, std::mt19937 &rng) { + PnPDataset data; + + // Ground-truth pose: a small twist away from identity, translated forward + // so the world origin (and nearby points) stay in front of the camera. + std::uniform_real_distribution rot_dist(-0.3f, 0.3f); + std::uniform_real_distribution trans_dist(-1.0f, 1.0f); + Vector<6> gt_twist; + gt_twist[0] = rot_dist(rng); + gt_twist[1] = rot_dist(rng); + gt_twist[2] = rot_dist(rng); + gt_twist[3] = trans_dist(rng); + gt_twist[4] = trans_dist(rng); + gt_twist[5] = 8.0f + trans_dist(rng); + std::vector gt_pose_vec; + examples::TwistsToSE3({gt_twist}, gt_pose_vec); + data.gt_pose = gt_pose_vec[0]; + + // Sample world points visible from the ground-truth pose. + std::uniform_real_distribution point_dist(-3.0f, 3.0f); + std::normal_distribution pixel_noise(0.0f, 3e-3f); + data.points_world.resize(num_points); + data.observations.resize(num_points); + for (size_t i = 0; i < num_points; ++i) { + Vector<3> p; + do { + p[0] = point_dist(rng); + p[1] = point_dist(rng); + p[2] = point_dist(rng); + } while (examples::ComputeDepth(data.gt_pose, p) < kMinDepth); + data.points_world[i] = p; + + Vector<2> obs = examples::ProjectNormalized(data.gt_pose, p); + obs[0] += pixel_noise(rng); + obs[1] += pixel_noise(rng); + data.observations[i] = obs; + } + + // Perturb the pose to create a non-trivial initial estimate. Kept small + // enough that every ground-truth-visible point stays visible. + std::vector perturbation; + examples::GenerateRandomSE3(1, rng, perturbation, 0.05f, 0.2f); + data.initial_pose = examples::ComposeSE3(perturbation[0], data.gt_pose); + + return data; +} + +// Runs LM on a fresh device copy of `data.initial_pose`, using the requested +// Jacobian mode, and returns the optimization summary plus final pose error. +struct RunResult { + cunls::MinimizerSummary summary; + float initial_pose_mse; + float final_pose_mse; +}; + +RunResult RunPnP(const PnPDataset &data, cunls::JacobianMode jacobian_mode) { + const size_t num_points = data.points_world.size(); + + dvector> points_device(data.points_world); + dvector> observations_device(data.observations); + std::vector pose_host = {data.initial_pose}; + dvector pose_device(pose_host); + + cunls::cuBLASHandle cublas_handle; + cunls::SE3StateBatch pose_states(cublas_handle, + reinterpret_cast(pose_device.data()), 1); + + cunls::PnPFactorBatch pnp_factor(observations_device.data(), points_device.data(), num_points, + kZThreshold); + + std::vector state_pointers(num_points, pose_states.StateBlockDevicePtr(0)); + + cunls::Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddFactorBatch(&pnp_factor, state_pointers, jacobian_mode); + if (!problem.CheckConsistency()) { + throw std::runtime_error("PnP problem consistency check failed"); + } + + cunls::MinimizerOptions options; + options.max_num_iterations = 60; + options.state_tolerance = 1e-9f; + options.cost_tolerance = 1e-9f; + options.jacobian_mode = jacobian_mode; + + cunls::LevenbergMarquardtMinimizerOptions lm_options; + lm_options.base_options = options; + lm_options.initial_lambda = 1e-2f; + cunls::LevenbergMarquardtMinimizer minimizer(lm_options); + + cunls::CudaStream stream; + RunResult result; + result.summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + std::vector optimized_pose(1); + pose_device.CopyToHost(optimized_pose.data(), 1); + + result.initial_pose_mse = examples::ComputePoseMSE({data.initial_pose}, {data.gt_pose}); + result.final_pose_mse = examples::ComputePoseMSE(optimized_pose, {data.gt_pose}); + return result; +} + +const char *JacobianModeName(cunls::JacobianMode mode) { + return mode == cunls::JacobianMode::kAnalytic ? "analytic" : "numeric"; +} + +void PrintResult(const char *label, const RunResult &r) { + std::cout << " [" << label << "]\n"; + std::cout << " Initial cost: " << r.summary.initial_cost << "\n"; + std::cout << " Final cost: " << r.summary.final_cost << "\n"; + std::cout << " Iterations: " << r.summary.num_iterations << "\n"; + std::cout << " Pose MSE: " << r.initial_pose_mse << " -> " << r.final_pose_mse << "\n"; +} + +} // namespace + +int main(int argc, char **argv) { + try { + // CLI: --num-points N (default 2000), --jacobian-mode {analytic,numeric,both} + const std::string usage = std::string("Usage: ") + argv[0] + + " [--num-points N] [--jacobian-mode analytic|numeric|both]\n"; + size_t num_points = 2000; + std::string mode_arg = "both"; + for (int i = 1; i < argc; ++i) { + if (std::strcmp(argv[i], "--num-points") == 0) { + if (i + 1 >= argc) { + std::cerr << "Missing value for --num-points\n" << usage; + return 1; + } + const char *value = argv[++i]; + char *end = nullptr; + const long long parsed = std::strtoll(value, &end, 10); + if (end == value || *end != '\0' || parsed <= 0) { + std::cerr << "Invalid --num-points value '" << value + << "' (expected a positive integer)\n" + << usage; + return 1; + } + num_points = static_cast(parsed); + } else if (std::strcmp(argv[i], "--jacobian-mode") == 0) { + if (i + 1 >= argc) { + std::cerr << "Missing value for --jacobian-mode\n" << usage; + return 1; + } + mode_arg = argv[++i]; + } else if (std::strcmp(argv[i], "--help") == 0) { + std::cout << usage; + return 0; + } else { + std::cerr << "Unknown argument '" << argv[i] << "'\n" << usage; + return 1; + } + } + + std::mt19937 rng(13579); + PnPDataset data = GenerateDataset(num_points, rng); + + std::cout << "PnP Example\n"; + std::cout << " Num correspondences: " << num_points << "\n"; + + bool run_analytic = (mode_arg == "analytic" || mode_arg == "both"); + bool run_numeric = (mode_arg == "numeric" || mode_arg == "both"); + if (!run_analytic && !run_numeric) { + std::cerr << "Unknown --jacobian-mode '" << mode_arg + << "' (expected analytic|numeric|both)\n"; + return 1; + } + + // Observations carry pixel noise, so the residual cost has a noise floor + // and will not reach ~0; judge convergence by pose accuracy instead + // (final pose MSE should collapse relative to the initial perturbation). + bool ok = true; + if (run_analytic) { + RunResult analytic = RunPnP(data, cunls::JacobianMode::kAnalytic); + PrintResult("JacobianMode::kAnalytic", analytic); + ok = ok && analytic.summary.final_cost <= analytic.summary.initial_cost && + analytic.final_pose_mse < analytic.initial_pose_mse * 0.05f; + } + if (run_numeric) { + RunResult numeric = RunPnP(data, cunls::JacobianMode::kNumeric); + PrintResult("JacobianMode::kNumeric", numeric); + ok = ok && numeric.summary.final_cost <= numeric.summary.initial_cost && + numeric.final_pose_mse < numeric.initial_pose_mse * 0.05f; + } + + if (!ok) { + std::cerr << "Optimization quality check failed.\n"; + return 2; + } + return 0; + } catch (const std::exception &e) { + std::cerr << "Exception: " << e.what() << "\n"; + return 3; + } +} diff --git a/python/examples/custom_warp_factor.py b/python/examples/custom_warp_factor.py index 411b47f..63fd7a9 100644 --- a/python/examples/custom_warp_factor.py +++ b/python/examples/custom_warp_factor.py @@ -15,13 +15,22 @@ """Custom factor defined with NVIDIA Warp, solved via pycunls. -This is a Python port of ``examples/custom_factor/main.cu``. It builds a -chain of scalar states connected by "difference" constraints: +This is a Python port of ``examples/custom_factor/main.cu``. Like that C++ +example, it solves the same chain problem two ways: + +* **Part 1** (``ScalarDiffFactor``): the Warp kernel computes both the + residual and its (constant) analytic Jacobian. +* **Part 2** (``ScalarDiffResidualOnlyFactor``): the Warp kernel computes + only the residual and is registered with + ``jacobian_mode_override=pycunls.JacobianMode.numeric`` so pycunls + differentiates it via finite differences instead. See + :doc:`../numeric_jacobians` for how this works. + +Both parts solve: residual_i = (x_{i+1} - x_i) - measurement_i -The Warp kernel computes residuals and Jacobians, and pycunls runs -Levenberg-Marquardt to recover the ground truth. +with Levenberg-Marquardt. Pointer-gathering strategy -------------------------- @@ -147,9 +156,69 @@ def evaluate(self, residuals_ptr, jacobians_ptr, state_pointers_ptr, stream_hand return True +# ── Part 2: residual-only Warp kernel, numeric Jacobian ──────────────────── +# Same residual as scalar_diff_kernel, but no Jacobian code path at all. + +@wp.kernel +def scalar_diff_residual_only_kernel( + measurements: wp.array(dtype=wp.float32), + left_vals: wp.array(dtype=wp.float32), + right_vals: wp.array(dtype=wp.float32), + residuals: wp.array(dtype=wp.float32), + num_factors: int, +): + i = wp.tid() + if i >= num_factors: + return + residuals[i] = (right_vals[i] - left_vals[i]) - measurements[i] + + +class ScalarDiffResidualOnlyFactor(WarpFactorBatch): + """Same factor as ScalarDiffFactor, but only implements the residual. + + Register this factor group with + ``jacobian_mode_override=pycunls.JacobianMode.numeric`` (see main() + below) and pycunls differentiates it via finite differences on the + manifold tangent space of each referenced state block -- there is + nothing else to write. + """ + + def __init__(self, measurements_wp: wp.array, num_factors: int): + super().__init__( + residual_size=1, + state_block_sizes=[1, 1], + num_factors=num_factors, + ) + self.measurements = measurements_wp + self._num_factors = num_factors + + def evaluate(self, residuals_ptr, jacobians_ptr, state_pointers_ptr, stream_handle): + n = self._num_factors + + all_vals = _gather_state_values(state_pointers_ptr, n * 2) + left_vals = all_vals[0::2].copy() + right_vals = all_vals[1::2].copy() + + left_wp = wp.array(ptr=int(left_vals.data.ptr), dtype=wp.float32, + shape=(n,), device=self._device, copy=False) + right_wp = wp.array(ptr=int(right_vals.data.ptr), dtype=wp.float32, + shape=(n,), device=self._device, copy=False) + + res = self.wrap_array(residuals_ptr, wp.float32, n) + + stream = self.make_warp_stream(stream_handle) + wp.launch( + scalar_diff_residual_only_kernel, + dim=n, + inputs=[self.measurements, left_wp, right_wp, res, n], + stream=stream, + ) + return True + + # ── Main ──────────────────────────────────────────────────────────────────── -def main(): +def run_chain_example(title: str, use_numeric_jacobian: bool): num_states = 256 num_diff = num_states - 1 @@ -178,7 +247,6 @@ def main(): stream = pycunls.CudaStream() state_batch = pycunls.VectorStateBatch1(states_gpu, num_states) - diff_factor = ScalarDiffFactor(measurements_wp, num_diff) prior_factor = pycunls.PriorVectorFactorBatch1(prior_obs_gpu, 1) diff_ptrs = [] @@ -190,7 +258,18 @@ def main(): problem = pycunls.Problem() problem.add_state_batch(state_batch) - problem.add_factor_batch(diff_factor, diff_ptrs) + if use_numeric_jacobian: + # Force this factor group to numeric differentiation via the + # per-group override; the anchor prior below still uses its own + # analytic Jacobian either way (it's a built-in factor), so this + # also demonstrates mixing modes within a single Problem. + diff_factor = ScalarDiffResidualOnlyFactor(measurements_wp, num_diff) + problem.add_factor_batch( + diff_factor, diff_ptrs, + jacobian_mode_override=pycunls.JacobianMode.numeric) + else: + diff_factor = ScalarDiffFactor(measurements_wp, num_diff) + problem.add_factor_batch(diff_factor, diff_ptrs) problem.add_factor_batch(prior_factor, prior_ptrs) assert problem.check_consistency(), "Problem consistency check failed" @@ -214,12 +293,22 @@ def main(): mse_before = float(np.mean((initial - gt) ** 2)) mse_after = float(np.mean((optimized - gt) ** 2)) - print("Custom Warp Factor Example (pycunls)") + print(title) print(f" Initial cost : {summary.initial_cost:.6f}") print(f" Final cost : {summary.final_cost:.6f}") print(f" Iterations : {summary.num_iterations}") print(f" State MSE : {mse_before:.6f} -> {mse_after:.6f}") +def main(): + # Part 1: analytic Jacobian, computed by the Warp kernel itself. + run_chain_example("Part 1: analytic Jacobian (Warp)", use_numeric_jacobian=False) + print() + # Part 2: residual-only Warp kernel; pycunls supplies the Jacobian via + # finite differences. + run_chain_example("Part 2: residual-only, numeric Jacobian (Warp)", + use_numeric_jacobian=True) + + if __name__ == "__main__": main() diff --git a/python/pycunls/__init__.py b/python/pycunls/__init__.py index fac09d7..401031c 100644 --- a/python/pycunls/__init__.py +++ b/python/pycunls/__init__.py @@ -57,8 +57,11 @@ # --- Enumerations --- SparseLinearSolverType, ColumnScaling, + JacobianMode, + NumericDiffMethod, # --- Minimizer options & summary --- MinimizerOptions, + NumericDiffOptions, MinimizerSummary, LevenbergMarquardtMinimizerOptions, # --- Minimizers --- @@ -129,7 +132,10 @@ "CublasHandle", "SparseLinearSolverType", "ColumnScaling", + "JacobianMode", + "NumericDiffMethod", "MinimizerOptions", + "NumericDiffOptions", "MinimizerSummary", "LevenbergMarquardtMinimizerOptions", "GaussNewtonMinimizer", diff --git a/python/pycunls/_pycunls_core.pyi b/python/pycunls/_pycunls_core.pyi index 0e85191..d4bd60f 100644 --- a/python/pycunls/_pycunls_core.pyi +++ b/python/pycunls/_pycunls_core.pyi @@ -61,10 +61,33 @@ class ColumnScaling(enum.IntEnum): none = ... hessian_diagonal = ... +class JacobianMode(enum.IntEnum): + """Selects how a factor batch's Jacobian is obtained: the factor's own + analytic Evaluate() output, or finite differences on the manifold + tangent space of each referenced state block.""" + + analytic = ... + numeric = ... + +class NumericDiffMethod(enum.IntEnum): + """Finite-difference scheme used when a factor batch resolves to + JacobianMode.numeric.""" + + forward = ... + central = ... + # =================================================================== # Options and summary # =================================================================== +class NumericDiffOptions: + """Tuning knobs for numeric (finite-difference) Jacobian computation.""" + + method: NumericDiffMethod + relative_step_size: float + + def __init__(self) -> None: ... + class MinimizerOptions: """Options for Gauss-Newton and Levenberg-Marquardt minimizers.""" @@ -74,6 +97,8 @@ class MinimizerOptions: max_consecutive_rejected_steps: int sparse_linear_solver_type: SparseLinearSolverType column_scaling: ColumnScaling + jacobian_mode: JacobianMode + numeric_diff_options: NumericDiffOptions disable_safety_checks: bool def __init__(self) -> None: ... @@ -848,8 +873,14 @@ class Problem: self, factor_batch: FactorBatch, state_pointers: Sequence[int], + jacobian_mode_override: JacobianMode | None = None, ) -> None: - """Add a factor batch with its state pointer connectivity.""" + """Add a factor batch with its state pointer connectivity. + + jacobian_mode_override, when set, forces this factor batch to always + use the given JacobianMode regardless of the minimizer's + MinimizerOptions.jacobian_mode default. + """ ... @overload def add_factor_batch( @@ -857,8 +888,14 @@ class Problem: factor_batch: FactorBatch, loss_function: LossFunctionBatch, state_pointers: Sequence[int], + jacobian_mode_override: JacobianMode | None = None, ) -> None: - """Add a factor batch with a loss function and state pointer connectivity.""" + """Add a factor batch with a loss function and state pointer connectivity. + + jacobian_mode_override, when set, forces this factor batch to always + use the given JacobianMode regardless of the minimizer's + MinimizerOptions.jacobian_mode default. + """ ... def check_consistency(self) -> bool: """Validate that all state batches and factor batches are consistent.""" diff --git a/python/src/bind_problem.cpp b/python/src/bind_problem.cpp index 367e24a..d5e52a7 100644 --- a/python/src/bind_problem.cpp +++ b/python/src/bind_problem.cpp @@ -30,12 +30,13 @@ // cast to float* because nanobind cannot automatically convert a Python // list[int] to std::vector. -#include "bindings.h" - +#include #include #include +#include "bindings.h" #include "cunls/common/device_vector.h" +#include "cunls/minimizer/jacobian_mode.h" #include "cunls/minimizer/problem.h" // cunls::Problem contains DeviceVector members which are move-only, but the @@ -43,7 +44,8 @@ // nanobind it is not, so it does not try to synthesise a copy constructor. NAMESPACE_BEGIN(NB_NAMESPACE) NAMESPACE_BEGIN(detail) -template <> struct is_copy_constructible : std::false_type {}; +template <> +struct is_copy_constructible : std::false_type {}; NAMESPACE_END(detail) NAMESPACE_END(NB_NAMESPACE) @@ -51,41 +53,44 @@ void bind_problem(nb::module_ &m) { nb::class_(m, "Problem", "Defines a nonlinear least-squares problem from " "state and factor batches.") - .def("__init__", - [](cunls::Problem *self) { new (self) cunls::Problem(); }) - .def("add_state_batch", &cunls::Problem::AddStateBatch, - nb::arg("state_batch"), nb::keep_alive<1, 2>(), - "Register a state batch with the problem.") + .def("__init__", [](cunls::Problem *self) { new (self) cunls::Problem(); }) + .def("add_state_batch", &cunls::Problem::AddStateBatch, nb::arg("state_batch"), + nb::keep_alive<1, 2>(), "Register a state batch with the problem.") // Overload without a loss function (defaults to trivial/identity loss). .def( "add_factor_batch", [](cunls::Problem &self, cunls::FactorBatch *factor_batch, - const std::vector &state_ptrs) { + const std::vector &state_ptrs, + std::optional jacobian_mode_override) { std::vector ptrs(state_ptrs.size()); for (size_t i = 0; i < state_ptrs.size(); ++i) ptrs[i] = reinterpret_cast(state_ptrs[i]); - self.AddFactorBatch(factor_batch, ptrs); + self.AddFactorBatch(factor_batch, ptrs, jacobian_mode_override); }, nb::arg("factor_batch"), nb::arg("state_pointers"), - nb::keep_alive<1, 2>(), - "Add a factor batch with its state pointer connectivity.") + nb::arg("jacobian_mode_override") = std::nullopt, nb::keep_alive<1, 2>(), + "Add a factor batch with its state pointer connectivity. " + "jacobian_mode_override, when set, forces this factor batch to " + "always use the given JacobianMode regardless of the minimizer's " + "MinimizerOptions.jacobian_mode default.") // Overload with an explicit robust loss function. .def( "add_factor_batch", - [](cunls::Problem &self, cunls::FactorBatch *factor_batch, - cunls::LossFunctionBatch *loss, - const std::vector &state_ptrs) { + [](cunls::Problem &self, cunls::FactorBatch *factor_batch, cunls::LossFunctionBatch *loss, + const std::vector &state_ptrs, + std::optional jacobian_mode_override) { std::vector ptrs(state_ptrs.size()); for (size_t i = 0; i < state_ptrs.size(); ++i) ptrs[i] = reinterpret_cast(state_ptrs[i]); - self.AddFactorBatch(factor_batch, loss, ptrs); + self.AddFactorBatch(factor_batch, loss, ptrs, jacobian_mode_override); }, - nb::arg("factor_batch"), nb::arg("loss_function"), - nb::arg("state_pointers"), nb::keep_alive<1, 2>(), + nb::arg("factor_batch"), nb::arg("loss_function"), nb::arg("state_pointers"), + nb::arg("jacobian_mode_override") = std::nullopt, nb::keep_alive<1, 2>(), nb::keep_alive<1, 3>(), "Add a factor batch with a loss function and state pointer " - "connectivity.") - .def( - "check_consistency", &cunls::Problem::CheckConsistency, - "Validate that all state batches and factor batches are consistent."); + "connectivity. jacobian_mode_override, when set, forces this " + "factor batch to always use the given JacobianMode regardless of " + "the minimizer's MinimizerOptions.jacobian_mode default.") + .def("check_consistency", &cunls::Problem::CheckConsistency, + "Validate that all state batches and factor batches are consistent."); } diff --git a/python/src/bind_types.cpp b/python/src/bind_types.cpp index 56f9fef..62d4eea 100644 --- a/python/src/bind_types.cpp +++ b/python/src/bind_types.cpp @@ -30,6 +30,7 @@ #include "cunls/common/cuda_stream.h" #include "cunls/linear_solver/sparse_linear_solver.h" #include "cunls/minimizer/gauss_newton_minimizer.h" +#include "cunls/minimizer/jacobian_mode.h" #include "cunls/minimizer/levenberg_marquardt_minimizer.h" // Convert a Python object to a raw device pointer (uintptr_t). @@ -72,6 +73,32 @@ void bind_types(nb::module_ &m) { .value("none", cunls::ColumnScaling::None) .value("hessian_diagonal", cunls::ColumnScaling::HessianDiagonal); + nb::enum_( + m, "JacobianMode", + "Selects how a factor batch's Jacobian is obtained: analytic (the " + "factor's own hand-derived Evaluate() output) or numeric (finite " + "differences on the manifold tangent space of each referenced state " + "block, via Problem.add_factor_batch's jacobian_mode_override or " + "MinimizerOptions.jacobian_mode).") + .value("analytic", cunls::JacobianMode::kAnalytic) + .value("numeric", cunls::JacobianMode::kNumeric); + + nb::enum_( + m, "NumericDiffMethod", + "Finite-difference scheme used when a factor batch resolves to " + "JacobianMode.numeric.") + .value("forward", cunls::NumericDiffOptions::Method::kForward, + "One-sided: (f(x+eps) - f(x)) / eps. Cheaper, less accurate.") + .value("central", cunls::NumericDiffOptions::Method::kCentral, + "Two-sided: (f(x+eps) - f(x-eps)) / (2*eps). Default."); + + nb::class_( + m, "NumericDiffOptions", "Tuning knobs for numeric (finite-difference) Jacobian computation.") + .def(nb::init<>()) + .def_rw("method", &cunls::NumericDiffOptions::method) + .def_rw("relative_step_size", &cunls::NumericDiffOptions::relative_step_size, + "Per-tangent-coordinate perturbation step size. Default: 1e-4."); + // --- Minimizer configuration structs --- // All fields are read/write so users can tune convergence behaviour // from Python before passing the options to a minimizer constructor. @@ -86,6 +113,13 @@ void bind_types(nb::module_ &m) { &cunls::MinimizerOptions::max_consecutive_rejected_steps) .def_rw("sparse_linear_solver_type", &cunls::MinimizerOptions::sparse_linear_solver_type) .def_rw("column_scaling", &cunls::MinimizerOptions::column_scaling) + .def_rw("jacobian_mode", &cunls::MinimizerOptions::jacobian_mode, + "Global default JacobianMode for every factor batch, unless " + "overridden per group via Problem.add_factor_batch's " + "jacobian_mode_override. Default: JacobianMode.analytic.") + .def_rw("numeric_diff_options", &cunls::MinimizerOptions::numeric_diff_options, + "Tuning knobs used whenever a factor batch is evaluated with " + "JacobianMode.numeric; ignored otherwise.") .def_rw("disable_safety_checks", &cunls::MinimizerOptions::disable_safety_checks, "When False, the minimizer enables all optional runtime " "validation. Currently this covers post-factorization " diff --git a/python/tests/test_minimizer.py b/python/tests/test_minimizer.py index edcf529..7246d90 100644 --- a/python/tests/test_minimizer.py +++ b/python/tests/test_minimizer.py @@ -168,16 +168,69 @@ def test_defaults(self): assert (opts.sparse_linear_solver_type == pycunls.SparseLinearSolverType.BlockSparsePCG) assert opts.column_scaling == pycunls.ColumnScaling.none + assert opts.jacobian_mode == pycunls.JacobianMode.analytic + assert opts.numeric_diff_options.method == pycunls.NumericDiffMethod.central + assert opts.numeric_diff_options.relative_step_size == pytest.approx(1e-4) assert opts.disable_safety_checks is True def test_modification(self): opts = pycunls.MinimizerOptions() opts.max_num_iterations = 100 opts.state_tolerance = 1e-9 + opts.jacobian_mode = pycunls.JacobianMode.numeric assert opts.max_num_iterations == 100 assert opts.state_tolerance == pytest.approx(1e-9) + assert opts.jacobian_mode == pycunls.JacobianMode.numeric def test_lm_defaults(self): lm = pycunls.LevenbergMarquardtMinimizerOptions() assert lm.initial_lambda == pytest.approx(1e-3) assert lm.lambda_upscale == pytest.approx(2.0) + + +class TestJacobianMode: + """Numeric (finite-difference) Jacobians via JacobianMode.numeric.""" + def test_global_default_converges(self, stream): + problem, states_gpu, target = _make_prior_problem() + + opts = pycunls.MinimizerOptions() + opts.max_num_iterations = 20 + opts.jacobian_mode = pycunls.JacobianMode.numeric + minimizer = pycunls.GaussNewtonMinimizer(opts) + summary = minimizer.minimize(stream, problem) + + cp.cuda.runtime.streamSynchronize(stream.get_stream()) + + assert summary.final_cost < 1e-3 + result = cp.asnumpy(states_gpu) + np.testing.assert_allclose(result, target, atol=1e-2) + + def test_per_group_override_converges(self, stream): + # Same problem, but forced to numeric mode via the per-group + # override on add_factor_batch instead of the global default. + target = np.array([1.0, 2.0, 3.0], dtype=np.float32) + initial = np.array([0.0, 0.0, 0.0], dtype=np.float32) + + states_gpu = cp.asarray(initial) + obs_gpu = cp.asarray(target) + + sb = pycunls.VectorStateBatch3(states_gpu, 1) + fb = pycunls.PriorVectorFactorBatch3(obs_gpu, 1) + + problem = pycunls.Problem() + problem.add_state_batch(sb) + problem.add_factor_batch( + fb, [sb.state_block_device_ptr(0)], + jacobian_mode_override=pycunls.JacobianMode.numeric) + assert problem.check_consistency() + + opts = pycunls.MinimizerOptions() + opts.max_num_iterations = 20 + minimizer = pycunls.GaussNewtonMinimizer(opts) + summary = minimizer.minimize(stream, problem) + + cp.cuda.runtime.streamSynchronize(stream.get_stream()) + + assert summary.final_cost < 1e-3 + result = cp.asnumpy(states_gpu) + np.testing.assert_allclose(result, target, atol=1e-2) diff --git a/tests/numeric_diff_e2e_perf_test.cpp b/tests/numeric_diff_e2e_perf_test.cpp new file mode 100644 index 0000000..a9c512c --- /dev/null +++ b/tests/numeric_diff_e2e_perf_test.cpp @@ -0,0 +1,548 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file numeric_diff_e2e_perf_test.cpp + * @brief Wall-clock comparison of JacobianMode::kAnalytic vs kNumeric across + * problem types (PGO / SBA / PnP) and named problem sizes, this time timing + * full `LevenbergMarquardtMinimizer::Minimize()` calls rather than just + * `GaussNewtonMinimizer::BuildSystem()` (tests/numeric_diff_perf_test.cpp). + * + * The synthetic problem generators/sizes here are copy-identical to + * numeric_diff_perf_test.cpp so the two benchmarks are directly comparable + * in problem definition -- only what's timed differs. `Minimize()` also + * folds in the sparse linear solve, which is independent of JacobianMode + * and (per the profiling note in commit e5c4612) dominates total GPU time; + * this file exists to show the realistic end-to-end overhead of picking + * numeric diff for a real solve, complementing (not replacing) the + * BuildSystem-only isolation benchmark. + * + * Modeled on tests/motion_prior_perf_test.cpp's structure (gtest + * TestWithParam, NVTX instrumentation) and reuses LevenbergMarquardtMinimizer + * since that's what examples/pose_graph_optimization, examples/pnp and + * examples/sparse_bundle_adjustment all default to. + * + * See the size-selection comment in numeric_diff_perf_test.cpp for how the + * PGO / SBA / PnP problem sizes (and, for SBA, the sparse visibility + * pattern) were chosen; the same reasoning/config tables are duplicated + * here verbatim. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cunls/common/cublas_helper.h" +#include "cunls/common/cuda_stream.h" +#include "cunls/common/device_vector.h" +#include "cunls/common/helper.h" +#include "cunls/common/profiler.h" +#include "cunls/common/types.h" +#include "cunls/factor/between/se3_between_factor_batch.h" +#include "cunls/factor/pnp_factor_batch.h" +#include "cunls/factor/reprojection_factor_batch.h" +#include "cunls/math/so_se_lie_math.h" +#include "cunls/minimizer/gauss_newton_minimizer.h" +#include "cunls/minimizer/jacobian_mode.h" +#include "cunls/minimizer/levenberg_marquardt_minimizer.h" +#include "cunls/minimizer/problem.h" +#include "cunls/state/se3_state_batch.h" +#include "cunls/state/vector_state_batch.h" +#include "tests/utils.h" + +namespace cunls { +namespace { + +enum class ProblemType { kPGO, kSBA, kPnP }; + +const char *ToString(ProblemType t) { + switch (t) { + case ProblemType::kPGO: + return "PGO"; + case ProblemType::kSBA: + return "SBA"; + case ProblemType::kPnP: + return "PnP"; + } + return "?"; +} + +const char *ToString(JacobianMode m) { + return m == JacobianMode::kAnalytic ? "analytic" : "numeric"; +} + +// --------------------------------------------------------------------------- +// Synthetic dataset generators -- copy-identical to numeric_diff_perf_test.cpp +// so the two benchmarks time the exact same problems. +// --------------------------------------------------------------------------- + +SE3Transform ComposeSE3Host(const SE3Transform &a, const SE3Transform &b) { + SE3Transform c{}; + for (int r = 0; r < 4; ++r) { + for (int cix = 0; cix < 4; ++cix) { + float s = 0.f; + for (int k = 0; k < 4; ++k) s += a[r * 4 + k] * b[k * 4 + cix]; + c[r * 4 + cix] = s; + } + } + return c; +} + +std::vector RandomPoses(size_t n, std::mt19937 &rng) { + std::uniform_real_distribution rot(-0.3f, 0.3f); + std::uniform_real_distribution trans(-1.0f, 1.0f); + std::vector> twists(n); + for (size_t i = 0; i < n; ++i) { + twists[i] = {rot(rng), rot(rng), rot(rng), trans(rng), trans(rng), 8.0f + trans(rng)}; + } + CudaStream stream; + dvector> d_twists(twists); + dvector d_poses(n); + ComputeExpSE3(stream.GetStream(), reinterpret_cast(d_twists.data()), 6, 4, 16, n, + reinterpret_cast(d_poses.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + std::vector poses(n); + d_poses.CopyToHost(poses.data(), n); + return poses; +} + +// Perturbs each pose in `gt_poses` by a small random right-multiplied twist +// (poses[skip_first_n:] only, so an anchor pose used as a fixed/constant +// state block stays exactly at ground truth). Used to build an initial +// state that is *not* already at the ground-truth optimum: the +// BuildSystem-only benchmark (numeric_diff_perf_test.cpp) doesn't care +// about this since it never runs the solve loop, but a full end-to-end +// Minimize() benchmark does -- an exact-ground-truth start converges in 0 +// iterations (Minimize's `initial_cost < cost_tolerance` early exit) and +// never exercises the linear solve this benchmark exists to include. +std::vector PerturbPoses(const std::vector >_poses, std::mt19937 &rng, + size_t skip_first_n = 0) { + const size_t n = gt_poses.size(); + std::uniform_real_distribution rot(-0.05f, 0.05f); + std::uniform_real_distribution trans(-0.1f, 0.1f); + std::vector> twists(n, Vector<6>{0, 0, 0, 0, 0, 0}); + for (size_t i = skip_first_n; i < n; ++i) { + twists[i] = {rot(rng), rot(rng), rot(rng), trans(rng), trans(rng), trans(rng)}; + } + CudaStream stream; + dvector> d_twists(twists); + dvector d_disturb(n); + ComputeExpSE3(stream.GetStream(), reinterpret_cast(d_twists.data()), 6, 4, 16, n, + reinterpret_cast(d_disturb.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + std::vector disturb(n); + d_disturb.CopyToHost(disturb.data(), n); + + std::vector init_poses(n); + for (size_t i = 0; i < n; ++i) init_poses[i] = ComposeSE3Host(gt_poses[i], disturb[i]); + return init_poses; +} + +Vector<3> PerturbPoint(const Vector<3> &p, std::mt19937 &rng) { + std::uniform_real_distribution trans(-0.1f, 0.1f); + return {p[0] + trans(rng), p[1] + trans(rng), p[2] + trans(rng)}; +} + +// --------------------------------------------------------------------------- +// Named problem sizes -- see numeric_diff_perf_test.cpp for full rationale. +// --------------------------------------------------------------------------- + +struct PGOConfig { + size_t num_poses; + const char *size_label; +}; +constexpr std::array kPGOConfigs = {{ + {10000, "10k_poses"}, + {100000, "100k_poses"}, + {1000000, "1M_poses"}, +}}; + +struct SBAConfig { + int n_poses; + int n_points; + int obs_per_landmark; + const char *size_label; +}; +constexpr std::array kSBAConfigs = {{ + {1000, 5000, 3, "1kposes_5klandmarks"}, + {5000, 25000, 3, "5kposes_25klandmarks"}, + {10000, 100000, 3, "10kposes_100klandmarks"}, +}}; + +struct PnPConfig { + size_t n; + const char *size_label; +}; +constexpr std::array kPnPConfigs = {{ + {1000, "1k_correspondences"}, + {100000, "100k_correspondences"}, + {1000000, "1M_correspondences"}, +}}; + +std::string SizeLabel(ProblemType pt, int size_index) { + switch (pt) { + case ProblemType::kPGO: + return kPGOConfigs[size_index].size_label; + case ProblemType::kSBA: + return kSBAConfigs[size_index].size_label; + case ProblemType::kPnP: + return kPnPConfigs[size_index].size_label; + } + return "?"; +} + +struct PerfParams { + ProblemType problem_type; + int size_index; // 0, 1, 2 -- indexes into the per-problem-type config table above. + JacobianMode mode; +}; + +std::string ParamLabel(const PerfParams &p) { + return std::string(ToString(p.problem_type)) + "_" + SizeLabel(p.problem_type, p.size_index) + + "_" + ToString(p.mode); +} + +std::ostream &operator<<(std::ostream &os, const PerfParams &p) { return os << ParamLabel(p); } + +class NumericDiffE2EPerfTest : public ::testing::TestWithParam { + protected: + // Same iteration counts as numeric_diff_perf_test.cpp, for consistency + // between the two benchmarks. + static constexpr int kWarmupIters = 3; + static constexpr int kTimedIters = 10; + + static void AppendCsvRow(const std::string &problem_type, const std::string &size_label, + int size_index, size_t n_primary, size_t n_secondary, + const std::string &mode, double mean_ms, size_t num_iterations) { + std::ofstream f(CsvPath(), std::ios::app); + f << problem_type << "," << size_label << "," << size_index << "," << n_primary << "," + << n_secondary << "," << mode << "," << mean_ms << "," << num_iterations << "\n"; + } + + static std::string CsvPath() { return "/tmp/cunls_numeric_diff_perf/e2e_results.csv"; } + + static void SetUpTestSuite() { + ::mkdir("/tmp/cunls_numeric_diff_perf", 0755); + std::ofstream f(CsvPath(), std::ios::trunc); + f << "problem_type,size_label,size_index,n_primary,n_secondary,jacobian_mode," + "mean_ms_per_minimize,num_iterations_to_convergence\n"; + } + + /** + * @brief Runs kWarmupIters untimed + kTimedIters CUDA-event-timed full + * `Minimize()` calls (a fresh minimizer instance per call), wrapped in a + * per-iteration NVTX range named after (problem_type, size, mode). + * Records the mean elapsed ms and the last run's iteration count to the + * CSV. + * + * `Minimize()` writes the converged state back into the state batches' + * backing device buffers (GaussNewtonMinimizer::Minimize's final `Copy( + * stream, current_state_, problem)`), so without `reset_state` every call + * after the first would start from the previous call's converged (or + * near-converged) state and trivially finish in ~1 iteration. `reset_state` + * re-uploads the original (unconverged) initial values before every + * warmup and timed call so each run solves the exact same problem. + */ + double TimeMinimize(const LevenbergMarquardtMinimizerOptions &lm_options, Problem &problem, + const PerfParams &p, size_t n_primary, size_t n_secondary, + const std::function &reset_state, size_t &num_iterations) { + CudaStream stream; + + for (int i = 0; i < kWarmupIters; ++i) { + reset_state(); + LevenbergMarquardtMinimizer minimizer(lm_options); + minimizer.Minimize(stream.GetStream(), problem); + } + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + cudaEvent_t start, stop; + THROW_ON_CUDA_ERROR(cudaEventCreate(&start)); + THROW_ON_CUDA_ERROR(cudaEventCreate(&stop)); + + profiler::Domain domain("NumericDiffE2EPerfTest"); + double total_ms = 0.0; + MinimizerSummary summary; + for (int i = 0; i < kTimedIters; ++i) { + reset_state(); + LevenbergMarquardtMinimizer minimizer(lm_options); + auto range = domain.CreateDomainRange(ParamLabel(p) + "/Minimize"); + THROW_ON_CUDA_ERROR(cudaEventRecord(start, stream.GetStream())); + summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaEventRecord(stop, stream.GetStream())); + THROW_ON_CUDA_ERROR(cudaEventSynchronize(stop)); + float ms = 0.f; + THROW_ON_CUDA_ERROR(cudaEventElapsedTime(&ms, start, stop)); + total_ms += ms; + } + + THROW_ON_CUDA_ERROR(cudaEventDestroy(start)); + THROW_ON_CUDA_ERROR(cudaEventDestroy(stop)); + + double mean_ms = total_ms / kTimedIters; + num_iterations = summary.num_iterations; + AppendCsvRow(ToString(p.problem_type), SizeLabel(p.problem_type, p.size_index), p.size_index, + n_primary, n_secondary, ToString(p.mode), mean_ms, num_iterations); + return mean_ms; + } + + LevenbergMarquardtMinimizerOptions MakeOptions(JacobianMode mode) { + MinimizerOptions options; + options.jacobian_mode = mode; + options.disable_safety_checks = true; + options.sparse_linear_solver_type = test_utils::SolverTypeFromEnv(); + + LevenbergMarquardtMinimizerOptions lm_options; + lm_options.base_options = options; + return lm_options; + } + + cuBLASHandle cublas_handle_; +}; + +TEST_P(NumericDiffE2EPerfTest, MinimizeTiming) { + const PerfParams p = GetParam(); + SCOPED_TRACE(ParamLabel(p)); + + double mean_ms = 0.0; + size_t num_iterations = 0; + + switch (p.problem_type) { + case ProblemType::kPGO: { + const size_t num_poses = kPGOConfigs[p.size_index].num_poses; + const size_t num_factors = num_poses - 1; + std::mt19937 rng(1000); + std::vector gt_poses = RandomPoses(num_poses, rng); + + std::vector deltas(num_factors); + { + CudaStream stream; + dvector d_poses(gt_poses); + dvector d_inv(num_poses); + ComputeInverseSE3(stream.GetStream(), reinterpret_cast(d_poses.data()), 4, + 16, 4, 16, num_poses, reinterpret_cast(d_inv.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + std::vector inv(num_poses); + d_inv.CopyToHost(inv.data(), num_poses); + for (size_t i = 0; i < num_factors; ++i) { + deltas[i] = ComposeSE3Host(inv[i], gt_poses[i + 1]); + } + } + + // Initial state (what the state batch is constructed from) is + // perturbed off ground truth -- pose 0 is the anchor/constant state, + // so it's left exact -- so Minimize() actually has to iterate/solve + // instead of hitting the initial_cost < cost_tolerance early exit. + std::vector init_poses = PerturbPoses(gt_poses, rng, /*skip_first_n=*/1); + + dvector poses_device(init_poses); + dvector deltas_device(deltas); + std::vector const_ids = {0}; + dvector const_ids_device(const_ids); + + SE3StateBatch pose_states(cublas_handle_, + reinterpret_cast(poses_device.data()), num_poses, + const_ids_device.data(), 1); + SE3BetweenFactorBatch between_factor(deltas_device.data(), num_factors); + + std::vector state_pointers; + state_pointers.reserve(2 * num_factors); + for (size_t i = 0; i < num_factors; ++i) { + state_pointers.push_back(pose_states.StateBlockDevicePtr(i)); + state_pointers.push_back(pose_states.StateBlockDevicePtr(i + 1)); + } + + Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddFactorBatch(&between_factor, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + auto reset_state = [&]() { poses_device.CopyFromHost(init_poses.data(), num_poses); }; + mean_ms = + TimeMinimize(MakeOptions(p.mode), problem, p, num_poses, 0, reset_state, num_iterations); + break; + } + case ProblemType::kSBA: { + const SBAConfig &cfg = kSBAConfigs[p.size_index]; + const int n_poses = cfg.n_poses; + const int n_points = cfg.n_points; + const int obs_per_landmark = cfg.obs_per_landmark; + std::mt19937 rng(2000); + std::vector gt_poses = RandomPoses(n_poses, rng); + + std::uniform_real_distribution xy(-8.f, 8.f); + std::uniform_real_distribution zz(3.f, 15.f); + std::vector> points(n_points); + for (int i = 0; i < n_points; ++i) points[i] = {xy(rng), xy(rng), zz(rng)}; + + // Sparse visibility -- see numeric_diff_perf_test.cpp / the SBAConfig + // comment there for the full rationale (dense pose x landmark grid is + // infeasible at these sizes; this stays well under the ~1.3M + // factors/FactorBatch GPU-safety limit while guaranteeing every pose + // and landmark is observed). + std::uniform_int_distribution pose_pick(0, n_poses - 1); + + // State batches are initialized from perturbed poses/points (pose 0 is + // the anchor/constant state, left exact); observations below are + // still computed from the exact ground truth, so the problem is + // well-posed and Minimize() has real work to do instead of hitting + // the initial_cost < cost_tolerance early exit. + std::vector init_poses = PerturbPoses(gt_poses, rng, /*skip_first_n=*/1); + std::vector> init_points(n_points); + for (int i = 0; i < n_points; ++i) init_points[i] = PerturbPoint(points[i], rng); + + dvector poses_device(init_poses); + dvector> points_device(init_points); + std::vector const_pose_ids = {0}; + dvector const_pose_ids_device(const_pose_ids); + + SE3StateBatch pose_states(cublas_handle_, + reinterpret_cast(poses_device.data()), n_poses, + const_pose_ids_device.data(), 1); + VectorStateBatch<3> point_states(reinterpret_cast(points_device.data()), + n_points); + + std::vector> observations; + std::vector state_pointers; + const size_t expected_factors = static_cast(n_points) * obs_per_landmark; + observations.reserve(expected_factors); + state_pointers.reserve(2 * expected_factors); + + std::vector chosen_poses; + chosen_poses.reserve(obs_per_landmark); + for (int pt_idx = 0; pt_idx < n_points; ++pt_idx) { + chosen_poses.clear(); + chosen_poses.push_back(pt_idx % n_poses); + while (static_cast(chosen_poses.size()) < obs_per_landmark && + static_cast(chosen_poses.size()) < n_poses) { + int candidate = pose_pick(rng); + if (std::find(chosen_poses.begin(), chosen_poses.end(), candidate) == + chosen_poses.end()) { + chosen_poses.push_back(candidate); + } + } + + const Vector<3> &pt = points[pt_idx]; + for (int pose_idx : chosen_poses) { + const SE3Transform &T = gt_poses[pose_idx]; + float pc[3]; + pc[0] = T[3] + T[0] * pt[0] + T[1] * pt[1] + T[2] * pt[2]; + pc[1] = T[7] + T[4] * pt[0] + T[5] * pt[1] + T[6] * pt[2]; + pc[2] = T[11] + T[8] * pt[0] + T[9] * pt[1] + T[10] * pt[2]; + Vector<2> obs{pc[0] / pc[2], pc[1] / pc[2]}; + observations.push_back(obs); + state_pointers.push_back(pose_states.StateBlockDevicePtr(pose_idx)); + state_pointers.push_back(point_states.StateBlockDevicePtr(pt_idx)); + } + } + + dvector> observations_device(observations); + ReprojectionFactorBatch reproj(observations_device.data(), observations.size(), 1e-3f); + + Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddStateBatch(&point_states); + problem.AddFactorBatch(&reproj, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + auto reset_state = [&]() { + poses_device.CopyFromHost(init_poses.data(), n_poses); + points_device.CopyFromHost(init_points.data(), n_points); + }; + mean_ms = TimeMinimize(MakeOptions(p.mode), problem, p, n_poses, n_points, reset_state, + num_iterations); + break; + } + case ProblemType::kPnP: { + const size_t n = kPnPConfigs[p.size_index].n; + std::mt19937 rng(3000); + std::vector gt_pose_vec = RandomPoses(1, rng); + const SE3Transform &T = gt_pose_vec[0]; + + std::uniform_real_distribution xy(-4.f, 4.f); + std::uniform_real_distribution zz(3.f, 12.f); + std::vector> points(n); + std::vector> observations(n); + for (size_t i = 0; i < n; ++i) { + Vector<3> p{xy(rng), xy(rng), zz(rng)}; + float pc[3]; + pc[0] = T[3] + T[0] * p[0] + T[1] * p[1] + T[2] * p[2]; + pc[1] = T[7] + T[4] * p[0] + T[5] * p[1] + T[6] * p[2]; + pc[2] = T[11] + T[8] * p[0] + T[9] * p[1] + T[10] * p[2]; + points[i] = p; + observations[i] = {pc[0] / pc[2], pc[1] / pc[2]}; + } + + // Pose is fully optimizable (no constant ids here), so perturb it off + // ground truth -- otherwise Minimize() starts exactly at the optimum + // (zero residual) and hits the initial_cost < cost_tolerance early + // exit without ever running the linear solve. + std::vector init_pose_vec = PerturbPoses(gt_pose_vec, rng); + + dvector> points_device(points); + dvector> observations_device(observations); + dvector pose_device(init_pose_vec); + + SE3StateBatch pose_states(cublas_handle_, reinterpret_cast(pose_device.data()), + 1); + PnPFactorBatch pnp(observations_device.data(), points_device.data(), n, 1e-3f); + + std::vector state_pointers(n, pose_states.StateBlockDevicePtr(0)); + + Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddFactorBatch(&pnp, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + auto reset_state = [&]() { pose_device.CopyFromHost(init_pose_vec.data(), 1); }; + mean_ms = TimeMinimize(MakeOptions(p.mode), problem, p, n, 0, reset_state, num_iterations); + break; + } + } + + EXPECT_GT(mean_ms, 0.0); + std::cout << "[NumericDiffE2EPerfTest] " << ParamLabel(p) << ": " << mean_ms << " ms/Minimize (" + << num_iterations << " iterations)\n"; +} + +std::vector AllParams() { + std::vector out; + for (ProblemType pt : {ProblemType::kPGO, ProblemType::kSBA, ProblemType::kPnP}) { + for (int size_index = 0; size_index < 3; ++size_index) { + for (JacobianMode m : {JacobianMode::kAnalytic, JacobianMode::kNumeric}) { + out.push_back({pt, size_index, m}); + } + } + } + return out; +} + +INSTANTIATE_TEST_SUITE_P(Sweep, NumericDiffE2EPerfTest, ::testing::ValuesIn(AllParams()), + [](const ::testing::TestParamInfo &info) { + return ParamLabel(info.param); + }); + +} // namespace +} // namespace cunls diff --git a/tests/numeric_diff_jacobian_test.cpp b/tests/numeric_diff_jacobian_test.cpp new file mode 100644 index 0000000..46c945d --- /dev/null +++ b/tests/numeric_diff_jacobian_test.cpp @@ -0,0 +1,385 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file numeric_diff_jacobian_test.cpp + * @brief Compares NumericDiffJacobianBuilder's finite-difference Jacobians + * against each factor's analytic Jacobian, for factors spanning Euclidean, + * SO(3), and SE(3) manifolds. + */ + +#include "cunls/minimizer/numeric_diff_jacobian.h" + +#include + +#include +#include +#include + +#include "cunls/common/cublas_helper.h" +#include "cunls/common/cuda_stream.h" +#include "cunls/common/device_vector.h" +#include "cunls/common/helper.h" +#include "cunls/common/types.h" +#include "cunls/factor/between/se3_between_factor_batch.h" +#include "cunls/factor/between/so3_between_factor_batch.h" +#include "cunls/factor/between/vector_between_factor_batch.h" +#include "cunls/math/so_se_lie_math.h" +#include "cunls/minimizer/jacobian_mode.h" +#include "cunls/minimizer/minimizer_state.h" +#include "cunls/minimizer/problem.h" +#include "cunls/state/se3_state_batch.h" +#include "cunls/state/so3_state_batch.h" +#include "cunls/state/vector_state_batch.h" + +namespace cunls { +namespace { + +constexpr uint32_t kSeed = 12345u; + +/** + * @brief Compares two dense per-factor Jacobian buffers element-wise. + * + * Uses a combined relative/absolute tolerance since numeric-diff Jacobians + * are computed in float32 and central differencing amplifies rounding error + * relative to the analytic values. + */ +void ExpectJacobiansClose(const std::vector &analytic, const std::vector &numeric, + float rel_tol, float abs_tol) { + ASSERT_EQ(analytic.size(), numeric.size()); + for (size_t i = 0; i < analytic.size(); i++) { + float a = analytic[i]; + float n = numeric[i]; + float tol = abs_tol + rel_tol * std::max(std::fabs(a), std::fabs(n)); + EXPECT_NEAR(a, n, tol) << "Mismatch at flattened index " << i << " (analytic=" << a + << ", numeric=" << n << ")"; + } +} + +/** @brief Central-diff tolerance used by every test in this file. */ +constexpr float kRelTol = 5e-2f; +constexpr float kAbsTol = 5e-3f; + +TEST(NumericDiffJacobianTest, VectorBetweenMatchesAnalytic) { + constexpr int kDim = 3; + constexpr size_t kNumStates = 6; + constexpr size_t kNumFactors = kNumStates - 1; + + std::mt19937 rng(kSeed); + std::uniform_real_distribution dist(-2.0f, 2.0f); + + std::vector> states_host(kNumStates); + for (auto &v : states_host) { + for (int i = 0; i < kDim; i++) v[i] = dist(rng); + } + std::vector> deltas_host(kNumFactors); + for (auto &v : deltas_host) { + for (int i = 0; i < kDim; i++) v[i] = dist(rng); + } + + DeviceVector> states_device(states_host); + DeviceVector> deltas_device(deltas_host); + + VectorStateBatch state_batch(reinterpret_cast(states_device.data()), + kNumStates); + VectorBetweenFactorBatch factor_batch(deltas_device.data(), kNumFactors); + + std::vector state_pointers(kNumFactors * 2); + for (size_t f = 0; f < kNumFactors; f++) { + state_pointers[2 * f + 0] = state_batch.StateBlockDevicePtr(f); + state_pointers[2 * f + 1] = state_batch.StateBlockDevicePtr(f + 1); + } + + Problem problem; + problem.AddStateBatch(&state_batch); + problem.AddFactorBatch(&factor_batch, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + CudaStream stream; + MinimizerState ms(stream.GetStream(), problem); + + const size_t residual_size = factor_batch.ResidualsSize(); + const size_t total_cols = kDim + kDim; + const size_t jac_floats = kNumFactors * residual_size * total_cols; + + dvector residuals_analytic(kNumFactors * residual_size); + dvector jacobian_analytic(jac_floats); + auto ptrs = ms.GetStatePointers()[0].data(); + factor_batch.Evaluate(residuals_analytic.data(), jacobian_analytic.data(), ptrs, + stream.GetStream()); + + dvector residuals_baseline(kNumFactors * residual_size); + factor_batch.Evaluate(residuals_baseline.data(), nullptr, ptrs, stream.GetStream()); + + dvector jacobian_numeric(jac_floats); + NumericDiffJacobianBuilder builder; + NumericDiffOptions options; + builder.Compute(stream.GetStream(), problem, 0, ms, residuals_baseline.data(), + jacobian_numeric.data(), options); + + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + std::vector jac_an_host(jac_floats), jac_num_host(jac_floats); + jacobian_analytic.CopyToHost(jac_an_host.data(), jac_floats); + jacobian_numeric.CopyToHost(jac_num_host.data(), jac_floats); + + ExpectJacobiansClose(jac_an_host, jac_num_host, kRelTol, kAbsTol); +} + +// SO3BetweenFactorBatch: residual = Log(R_left^T * R_right * Delta^T). Uses +// non-identity deltas (identity deltas make the left/right analytic-vs-Ad +// ordering bug invisible, since Ad(I) = I). This regression-tests the fix to +// so3_between_fused_jacobians_kernel (see so3_between_factor_batch.cu): +// previously the left block was `-Ad(Delta) * J_l^{-1}(r)` and the right +// block was `J_r^{-1}(r)` (no Delta factor at all), which does not match the +// residual's actual dependence on the SO3StateBatch::Plus right-multiplicative +// retraction. Fixed to left = `-J_l^{-1}(r)`, right = `J_r^{-1}(r) * Delta`. +TEST(NumericDiffJacobianTest, SO3BetweenMatchesAnalytic) { + constexpr size_t kNumFactors = 5; + constexpr size_t kNumStates = kNumFactors + 1; + + std::mt19937 rng(kSeed + 1); + std::uniform_real_distribution rot_dist(-0.5f, 0.5f); + + CudaStream stream; + cuBLASHandle cublas; + + hvector> state_twists(kNumStates); + for (auto &t : state_twists) { + for (int i = 0; i < 3; i++) t[i] = rot_dist(rng); + } + dvector> state_twists_d(state_twists); + dvector states_d(kNumStates); + ComputeExpSO3(stream.GetStream(), reinterpret_cast(state_twists_d.data()), 3, 3, 9, + kNumStates, reinterpret_cast(states_d.data())); + + hvector> delta_twists(kNumFactors); + for (auto &t : delta_twists) { + for (int i = 0; i < 3; i++) t[i] = rot_dist(rng); + } + dvector> delta_twists_d(delta_twists); + dvector deltas_d(kNumFactors); + ComputeExpSO3(stream.GetStream(), reinterpret_cast(delta_twists_d.data()), 3, 3, 9, + kNumFactors, reinterpret_cast(deltas_d.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + SO3StateBatch state_batch(cublas, reinterpret_cast(states_d.data()), kNumStates); + SO3BetweenFactorBatch factor_batch(deltas_d.data(), kNumFactors); + + std::vector state_pointers(kNumFactors * 2); + for (size_t f = 0; f < kNumFactors; f++) { + state_pointers[2 * f + 0] = state_batch.StateBlockDevicePtr(f); + state_pointers[2 * f + 1] = state_batch.StateBlockDevicePtr(f + 1); + } + + Problem problem; + problem.AddStateBatch(&state_batch); + problem.AddFactorBatch(&factor_batch, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + MinimizerState ms(stream.GetStream(), problem); + + const size_t residual_size = factor_batch.ResidualsSize(); + const size_t total_cols = 3 + 3; + const size_t jac_floats = kNumFactors * residual_size * total_cols; + + dvector residuals_analytic(kNumFactors * residual_size); + dvector jacobian_analytic(jac_floats); + auto ptrs = ms.GetStatePointers()[0].data(); + factor_batch.Evaluate(residuals_analytic.data(), jacobian_analytic.data(), ptrs, + stream.GetStream()); + + dvector residuals_baseline(kNumFactors * residual_size); + factor_batch.Evaluate(residuals_baseline.data(), nullptr, ptrs, stream.GetStream()); + + dvector jacobian_numeric(jac_floats); + NumericDiffJacobianBuilder builder; + NumericDiffOptions options; + builder.Compute(stream.GetStream(), problem, 0, ms, residuals_baseline.data(), + jacobian_numeric.data(), options); + + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + std::vector jac_an_host(jac_floats), jac_num_host(jac_floats); + jacobian_analytic.CopyToHost(jac_an_host.data(), jac_floats); + jacobian_numeric.CopyToHost(jac_num_host.data(), jac_floats); + + ExpectJacobiansClose(jac_an_host, jac_num_host, kRelTol, kAbsTol); +} + +// SE3BetweenFactorBatch: residual = Log(Delta * T_left^{-1} * T_right). Uses +// non-identity deltas (see the comment on SO3BetweenMatchesAnalytic above for +// why identity deltas would hide the bug). This regression-tests the fix to +// se3_between_fused_jacobians_kernel (see se3_between_factor_batch.cu): +// previously the left block multiplied Ad(Delta) and J_l^{-1}(twist) in the +// wrong order (`-Ad(Delta) * J_l^{-1}(twist)`) instead of the correct +// `-J_l^{-1}(twist) * Ad(Delta)`. +TEST(NumericDiffJacobianTest, SE3BetweenMatchesAnalytic) { + constexpr size_t kNumFactors = 5; + constexpr size_t kNumStates = kNumFactors + 1; + + std::mt19937 rng(kSeed + 2); + std::uniform_real_distribution rot_dist(-0.3f, 0.3f); + std::uniform_real_distribution trans_dist(-2.0f, 2.0f); + + CudaStream stream; + cuBLASHandle cublas; + + auto make_twists = [&](size_t n) { + hvector> twists(n); + for (auto &t : twists) { + t[0] = rot_dist(rng); + t[1] = rot_dist(rng); + t[2] = rot_dist(rng); + t[3] = trans_dist(rng); + t[4] = trans_dist(rng); + t[5] = trans_dist(rng); + } + return twists; + }; + + hvector> state_twists = make_twists(kNumStates); + dvector> state_twists_d(state_twists); + dvector states_d(kNumStates); + ComputeExpSE3(stream.GetStream(), reinterpret_cast(state_twists_d.data()), 6, 4, + 16, kNumStates, reinterpret_cast(states_d.data())); + + hvector> delta_twists = make_twists(kNumFactors); + dvector> delta_twists_d(delta_twists); + dvector deltas_d(kNumFactors); + ComputeExpSE3(stream.GetStream(), reinterpret_cast(delta_twists_d.data()), 6, 4, + 16, kNumFactors, reinterpret_cast(deltas_d.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + SE3StateBatch state_batch(cublas, reinterpret_cast(states_d.data()), kNumStates); + SE3BetweenFactorBatch factor_batch(deltas_d.data(), kNumFactors); + + std::vector state_pointers(kNumFactors * 2); + for (size_t f = 0; f < kNumFactors; f++) { + state_pointers[2 * f + 0] = state_batch.StateBlockDevicePtr(f); + state_pointers[2 * f + 1] = state_batch.StateBlockDevicePtr(f + 1); + } + + Problem problem; + problem.AddStateBatch(&state_batch); + problem.AddFactorBatch(&factor_batch, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + MinimizerState ms(stream.GetStream(), problem); + + const size_t residual_size = factor_batch.ResidualsSize(); + const size_t total_cols = 6 + 6; + const size_t jac_floats = kNumFactors * residual_size * total_cols; + + dvector residuals_analytic(kNumFactors * residual_size); + dvector jacobian_analytic(jac_floats); + auto ptrs = ms.GetStatePointers()[0].data(); + factor_batch.Evaluate(residuals_analytic.data(), jacobian_analytic.data(), ptrs, + stream.GetStream()); + + dvector residuals_baseline(kNumFactors * residual_size); + factor_batch.Evaluate(residuals_baseline.data(), nullptr, ptrs, stream.GetStream()); + + dvector jacobian_numeric(jac_floats); + NumericDiffJacobianBuilder builder; + NumericDiffOptions options; + builder.Compute(stream.GetStream(), problem, 0, ms, residuals_baseline.data(), + jacobian_numeric.data(), options); + + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + std::vector jac_an_host(jac_floats), jac_num_host(jac_floats); + jacobian_analytic.CopyToHost(jac_an_host.data(), jac_floats); + jacobian_numeric.CopyToHost(jac_num_host.data(), jac_floats); + + ExpectJacobiansClose(jac_an_host, jac_num_host, kRelTol, kAbsTol); +} + +TEST(NumericDiffJacobianTest, ForwardDiffMatchesAnalytic) { + // Sanity check for the forward-difference path (kForward), on the same + // Euclidean between-factor setup as VectorBetweenMatchesAnalytic, with a + // looser tolerance since forward differencing is first-order accurate. + constexpr int kDim = 3; + constexpr size_t kNumStates = 6; + constexpr size_t kNumFactors = kNumStates - 1; + + std::mt19937 rng(kSeed + 3); + std::uniform_real_distribution dist(-2.0f, 2.0f); + + std::vector> states_host(kNumStates); + for (auto &v : states_host) { + for (int i = 0; i < kDim; i++) v[i] = dist(rng); + } + std::vector> deltas_host(kNumFactors); + for (auto &v : deltas_host) { + for (int i = 0; i < kDim; i++) v[i] = dist(rng); + } + + DeviceVector> states_device(states_host); + DeviceVector> deltas_device(deltas_host); + + VectorStateBatch state_batch(reinterpret_cast(states_device.data()), + kNumStates); + VectorBetweenFactorBatch factor_batch(deltas_device.data(), kNumFactors); + + std::vector state_pointers(kNumFactors * 2); + for (size_t f = 0; f < kNumFactors; f++) { + state_pointers[2 * f + 0] = state_batch.StateBlockDevicePtr(f); + state_pointers[2 * f + 1] = state_batch.StateBlockDevicePtr(f + 1); + } + + Problem problem; + problem.AddStateBatch(&state_batch); + problem.AddFactorBatch(&factor_batch, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + CudaStream stream; + MinimizerState ms(stream.GetStream(), problem); + + const size_t residual_size = factor_batch.ResidualsSize(); + const size_t total_cols = kDim + kDim; + const size_t jac_floats = kNumFactors * residual_size * total_cols; + + dvector jacobian_analytic(jac_floats); + dvector residuals_analytic(kNumFactors * residual_size); + auto ptrs = ms.GetStatePointers()[0].data(); + factor_batch.Evaluate(residuals_analytic.data(), jacobian_analytic.data(), ptrs, + stream.GetStream()); + + dvector residuals_baseline(kNumFactors * residual_size); + factor_batch.Evaluate(residuals_baseline.data(), nullptr, ptrs, stream.GetStream()); + + dvector jacobian_numeric(jac_floats); + NumericDiffJacobianBuilder builder; + NumericDiffOptions options; + options.method = NumericDiffOptions::Method::kForward; + builder.Compute(stream.GetStream(), problem, 0, ms, residuals_baseline.data(), + jacobian_numeric.data(), options); + + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + std::vector jac_an_host(jac_floats), jac_num_host(jac_floats); + jacobian_analytic.CopyToHost(jac_an_host.data(), jac_floats); + jacobian_numeric.CopyToHost(jac_num_host.data(), jac_floats); + + // Forward diff (linear factors here are exact regardless, since the + // between-factor residual is affine in the tangent perturbation). + ExpectJacobiansClose(jac_an_host, jac_num_host, kRelTol, kAbsTol); +} + +} // namespace +} // namespace cunls diff --git a/tests/numeric_diff_minimizer_test.cpp b/tests/numeric_diff_minimizer_test.cpp new file mode 100644 index 0000000..5725e4e --- /dev/null +++ b/tests/numeric_diff_minimizer_test.cpp @@ -0,0 +1,267 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file numeric_diff_minimizer_test.cpp + * @brief End-to-end GaussNewtonMinimizer / LevenbergMarquardtMinimizer + * convergence tests in JacobianMode::kNumeric on a small synthetic SE3 pose + * graph, plus a mixed-mode (one group numeric, one analytic) test. Adapted + * from the fixture pattern in synthetic_pgo_test.cpp, but with a much + * smaller pose count since this exercises correctness, not performance. + */ + +#include + +#include +#include + +#include "cunls/common/cublas_helper.h" +#include "cunls/common/cuda_stream.h" +#include "cunls/common/device_vector.h" +#include "cunls/common/helper.h" +#include "cunls/common/types.h" +#include "cunls/factor/between/se3_between_factor_batch.h" +#include "cunls/math/so_se_lie_math.h" +#include "cunls/minimizer/gauss_newton_minimizer.h" +#include "cunls/minimizer/jacobian_mode.h" +#include "cunls/minimizer/levenberg_marquardt_minimizer.h" +#include "cunls/minimizer/problem.h" +#include "cunls/state/se3_state_batch.h" +#include "tests/utils.h" + +namespace cunls { +namespace { + +constexpr size_t kNumPoses = 24; +constexpr uint32_t kFixedSeed = 7; + +std::vector GenerateRandomPoses( + size_t num_poses, std::mt19937 &rng, std::uniform_real_distribution &rotation_dist, + std::uniform_real_distribution &translation_dist) { + hvector> twists(num_poses); + for (size_t i = 0; i < num_poses; i++) { + Vector<6> &twist = twists[i]; + twist[0] = rotation_dist(rng); + twist[1] = rotation_dist(rng); + twist[2] = rotation_dist(rng); + twist[3] = translation_dist(rng); + twist[4] = translation_dist(rng); + twist[5] = translation_dist(rng); + } + + CudaStream stream; + dvector> twists_device(twists); + dvector poses_device(num_poses); + ComputeExpSE3(stream.GetStream(), reinterpret_cast(twists_device.data()), 6, 4, 16, + num_poses, reinterpret_cast(poses_device.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + std::vector poses(num_poses); + hvector poses_host(num_poses); + poses_device.CopyToHost(poses_host.data(), num_poses); + for (size_t i = 0; i < num_poses; i++) poses[i] = poses_host[i]; + return poses; +} + +/** + * @brief Owns a small synthetic PGO problem: two SE3 pose sets connected by + * one-to-one between constraints (pose_set1[i] <-> pose_set2[i]), matching + * the pattern in `SyntheticPGOTest.OptimizeConsecutiveBetweenConstraints`. + */ +struct SyntheticPGOProblem { + explicit SyntheticPGOProblem(uint32_t seed) { + std::mt19937 rng(seed); + std::uniform_real_distribution rotation_dist(-0.5f, 0.5f); + std::uniform_real_distribution translation_dist(-2.0f, 2.0f); + poses_set1 = GenerateRandomPoses(kNumPoses, rng, rotation_dist, translation_dist); + poses_set2 = GenerateRandomPoses(kNumPoses, rng, rotation_dist, translation_dist); + pose_deltas = GenerateRandomPoses(kNumPoses, rng, rotation_dist, translation_dist); + + poses_set1_device = dvector(poses_set1); + poses_set2_device = dvector(poses_set2); + pose_deltas_device = dvector(pose_deltas); + + state_batch_set1 = std::make_unique( + cublas_handle, reinterpret_cast(poses_set1_device.data()), kNumPoses); + state_batch_set2 = std::make_unique( + cublas_handle, reinterpret_cast(poses_set2_device.data()), kNumPoses); + } + + /** + * @brief Registers the full constraint set as a single factor group + * (optionally with a JacobianMode override) on `problem`. + */ + void AddSingleGroup(Problem &problem, std::optional override_mode) { + between_factor_batches.push_back( + std::make_unique(pose_deltas_device.data(), kNumPoses)); + std::vector state_pointers; + for (size_t i = 0; i < kNumPoses; i++) { + state_pointers.push_back(state_batch_set1->StateBlockDevicePtr(i)); + state_pointers.push_back(state_batch_set2->StateBlockDevicePtr(i)); + } + problem.AddStateBatch(state_batch_set1.get()); + problem.AddStateBatch(state_batch_set2.get()); + problem.AddFactorBatch(between_factor_batches.back().get(), state_pointers, override_mode); + } + + /** + * @brief Registers the constraint set split into two disjoint factor + * groups (first half / second half of the pose indices), each with its + * own JacobianMode. + */ + void AddSplitGroups(Problem &problem, JacobianMode first_half_mode, + JacobianMode second_half_mode) { + const size_t half = kNumPoses / 2; + + dvector deltas_first( + std::vector(pose_deltas.begin(), pose_deltas.begin() + half)); + dvector deltas_second( + std::vector(pose_deltas.begin() + half, pose_deltas.end())); + // Keep the underlying device storage alive for the lifetime of this + // object (factor batches only store the raw pointer). + extra_owned_deltas.push_back(std::move(deltas_first)); + extra_owned_deltas.push_back(std::move(deltas_second)); + + between_factor_batches.push_back( + std::make_unique(extra_owned_deltas[0].data(), half)); + between_factor_batches.push_back( + std::make_unique(extra_owned_deltas[1].data(), kNumPoses - half)); + + std::vector ptrs_first, ptrs_second; + for (size_t i = 0; i < half; i++) { + ptrs_first.push_back(state_batch_set1->StateBlockDevicePtr(i)); + ptrs_first.push_back(state_batch_set2->StateBlockDevicePtr(i)); + } + for (size_t i = half; i < kNumPoses; i++) { + ptrs_second.push_back(state_batch_set1->StateBlockDevicePtr(i)); + ptrs_second.push_back(state_batch_set2->StateBlockDevicePtr(i)); + } + + problem.AddStateBatch(state_batch_set1.get()); + problem.AddStateBatch(state_batch_set2.get()); + problem.AddFactorBatch(between_factor_batches[0].get(), ptrs_first, first_half_mode); + problem.AddFactorBatch(between_factor_batches[1].get(), ptrs_second, second_half_mode); + } + + cuBLASHandle cublas_handle; + std::vector poses_set1, poses_set2, pose_deltas; + dvector poses_set1_device, poses_set2_device, pose_deltas_device; + std::vector> extra_owned_deltas; + std::unique_ptr state_batch_set1, state_batch_set2; + std::vector> between_factor_batches; +}; + +MinimizerOptions MakeBaseOptions() { + MinimizerOptions options; + options.max_num_iterations = 50; + options.state_tolerance = 1e-6f; + options.cost_tolerance = 1e-6f; + options.disable_safety_checks = false; + options.sparse_linear_solver_type = test_utils::SolverTypeFromEnv(); + options.sparse_linear_solver_config.block_sparse_pcg_options.block_size = + test_utils::PCGBlockSizeFromEnv(6); + options.sparse_linear_solver_config.block_sparse_pcg_options.max_iterations = + test_utils::PCGMaxIterFromEnv(400); + options.sparse_linear_solver_config.block_sparse_pcg_options.relative_tolerance = + test_utils::PCGTolFromEnv(1e-4f); + return options; +} + +constexpr float kConvergedCostThreshold = 5e-2f; + +TEST(NumericDiffMinimizerTest, GaussNewtonAnalyticBaseline) { + SyntheticPGOProblem data(kFixedSeed); + Problem problem; + data.AddSingleGroup(problem, std::nullopt); + ASSERT_TRUE(problem.CheckConsistency()); + + MinimizerOptions options = MakeBaseOptions(); + options.jacobian_mode = JacobianMode::kAnalytic; + GaussNewtonMinimizer minimizer(options); + + CudaStream stream; + MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + ASSERT_LT(summary.final_cost, kConvergedCostThreshold); +} + +TEST(NumericDiffMinimizerTest, GaussNewtonNumericMatchesAnalytic) { + SyntheticPGOProblem data(kFixedSeed); + Problem problem; + data.AddSingleGroup(problem, std::nullopt); + ASSERT_TRUE(problem.CheckConsistency()); + + MinimizerOptions options = MakeBaseOptions(); + options.jacobian_mode = JacobianMode::kNumeric; + GaussNewtonMinimizer minimizer(options); + + CudaStream stream; + MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + ASSERT_GT(summary.num_iterations, 0u); + ASSERT_LT(summary.final_cost, kConvergedCostThreshold); +} + +TEST(NumericDiffMinimizerTest, LevenbergMarquardtNumericMatchesAnalytic) { + SyntheticPGOProblem data(kFixedSeed); + Problem problem; + data.AddSingleGroup(problem, std::nullopt); + ASSERT_TRUE(problem.CheckConsistency()); + + LevenbergMarquardtMinimizerOptions lm_options; + lm_options.base_options = MakeBaseOptions(); + lm_options.base_options.jacobian_mode = JacobianMode::kNumeric; + lm_options.initial_lambda = 1e-3f; + LevenbergMarquardtMinimizer minimizer(lm_options); + + CudaStream stream; + MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + ASSERT_GT(summary.num_iterations, 0u); + ASSERT_LT(summary.final_cost, kConvergedCostThreshold); +} + +/** + * @brief One factor group forced to kNumeric via the per-group override, + * the other left at the minimizer's global default (kAnalytic), inside the + * same Problem. Both halves should still converge together. + */ +TEST(NumericDiffMinimizerTest, MixedModeSingleProblem) { + SyntheticPGOProblem data(kFixedSeed); + Problem problem; + data.AddSplitGroups(problem, JacobianMode::kAnalytic, JacobianMode::kNumeric); + ASSERT_TRUE(problem.CheckConsistency()); + + LevenbergMarquardtMinimizerOptions lm_options; + lm_options.base_options = MakeBaseOptions(); + lm_options.base_options.jacobian_mode = JacobianMode::kAnalytic; // global default + lm_options.initial_lambda = 1e-3f; + LevenbergMarquardtMinimizer minimizer(lm_options); + + CudaStream stream; + MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + ASSERT_GT(summary.num_iterations, 0u); + ASSERT_LT(summary.final_cost, kConvergedCostThreshold); +} + +} // namespace +} // namespace cunls diff --git a/tests/numeric_diff_perf_test.cpp b/tests/numeric_diff_perf_test.cpp new file mode 100644 index 0000000..210380e --- /dev/null +++ b/tests/numeric_diff_perf_test.cpp @@ -0,0 +1,508 @@ +/* + * SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/** + * @file numeric_diff_perf_test.cpp + * @brief Wall-clock comparison of JacobianMode::kAnalytic vs kNumeric, across + * problem types (PGO / SBA / PnP) and named problem sizes. + * + * Times repeated `GaussNewtonMinimizer::BuildSystem` calls (not full + * `Minimize()` runs) via CUDA events: `BuildSystem` is exactly the call that + * computes residuals + Jacobians and assembles the normal equations, so it + * isolates the cost the two Jacobian modes actually differ on. A full + * `Minimize()` would also fold in the sparse linear solve, whose cost is + * independent of `JacobianMode` and (per the profiling note in commit + * e5c4612) dominates total GPU time -- mixing it in would wash out the very + * difference this benchmark exists to measure. + * + * Modeled directly on tests/motion_prior_perf_test.cpp's structure (gtest + * TestWithParam, NVTX instrumentation gated by ENABLE_PROFILING) and on the + * `SystemBuilder` BuildSystem-exposing pattern from + * tests/block_hessian_assembler_test.cpp. Meant to be run under `nsys + * profile`; also appends CUDA-event timings to a CSV for offline plotting. + * + * Problem sizes (each problem type sweeps 3 named sizes, small->large): + * - PGO: 10k / 100k / 1M poses, an SE3 chain (SE3BetweenFactorBatch). + * - SBA: (1k poses, 5k landmarks) / (5k poses, 25k landmarks) / + * (10k poses, 100k landmarks), via ReprojectionFactorBatch, with a + * *sparse* visibility pattern (each landmark observed by a small fixed + * number of poses -- see SBAConfig::obs_per_landmark below) rather than a + * dense pose x landmark grid. A dense grid at these sizes would produce + * an infeasible number of factors (e.g. 10k poses x 100k landmarks would + * be 1e9 factors); this codebase's GPU-safety guidance caps any single + * FactorBatch at roughly 1.3M factors (much beyond ~6M states in one + * batch has been observed to crash the GPU process), so sparse + * visibility is required to stay in a safe, realistic regime. + * - PnP: 1k / 100k / 1M correspondences (landmarks observed by the single + * pose being estimated), via PnPFactorBatch. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "cunls/common/cublas_helper.h" +#include "cunls/common/cuda_stream.h" +#include "cunls/common/device_vector.h" +#include "cunls/common/helper.h" +#include "cunls/common/profiler.h" +#include "cunls/common/types.h" +#include "cunls/factor/between/se3_between_factor_batch.h" +#include "cunls/factor/pnp_factor_batch.h" +#include "cunls/factor/reprojection_factor_batch.h" +#include "cunls/math/so_se_lie_math.h" +#include "cunls/minimizer/gauss_newton_minimizer.h" +#include "cunls/minimizer/jacobian_mode.h" +#include "cunls/minimizer/problem.h" +#include "cunls/state/se3_state_batch.h" +#include "cunls/state/vector_state_batch.h" +#include "tests/utils.h" + +namespace cunls { +namespace { + +enum class ProblemType { kPGO, kSBA, kPnP }; + +const char *ToString(ProblemType t) { + switch (t) { + case ProblemType::kPGO: + return "PGO"; + case ProblemType::kSBA: + return "SBA"; + case ProblemType::kPnP: + return "PnP"; + } + return "?"; +} + +const char *ToString(JacobianMode m) { + return m == JacobianMode::kAnalytic ? "analytic" : "numeric"; +} + +/** + * @brief Exposes GaussNewtonMinimizer::Initialize/BuildSystem (both + * protected). Same rationale/pattern as SystemBuilder in + * tests/block_hessian_assembler_test.cpp: least invasive way to time + * assembly in isolation from the rest of Minimize(). + */ +class SystemBuilder : public GaussNewtonMinimizer { + public: + explicit SystemBuilder(const MinimizerOptions &options) : GaussNewtonMinimizer(options) {} + + void Prepare(cudaStream_t stream, Problem &problem) { + Initialize(stream, problem); + current_state_.Recreate(stream, problem); + } + + void Build(cudaStream_t stream, const Problem &problem) { + BuildSystem(stream, problem, current_state_); + } +}; + +// --------------------------------------------------------------------------- +// Synthetic dataset generators (kept minimal; correctness of these factor +// types is already covered by synthetic_pgo_test.cpp / synthetic_sba_test.cpp +// / pnp_factor_batch_test.cpp -- this file only needs *some* well-posed +// problem of the right scale to time BuildSystem on). +// --------------------------------------------------------------------------- + +SE3Transform ComposeSE3Host(const SE3Transform &a, const SE3Transform &b) { + SE3Transform c{}; + for (int r = 0; r < 4; ++r) { + for (int cix = 0; cix < 4; ++cix) { + float s = 0.f; + for (int k = 0; k < 4; ++k) s += a[r * 4 + k] * b[k * 4 + cix]; + c[r * 4 + cix] = s; + } + } + return c; +} + +std::vector RandomPoses(size_t n, std::mt19937 &rng) { + std::uniform_real_distribution rot(-0.3f, 0.3f); + std::uniform_real_distribution trans(-1.0f, 1.0f); + std::vector> twists(n); + for (size_t i = 0; i < n; ++i) { + twists[i] = {rot(rng), rot(rng), rot(rng), trans(rng), trans(rng), 8.0f + trans(rng)}; + } + CudaStream stream; + dvector> d_twists(twists); + dvector d_poses(n); + ComputeExpSE3(stream.GetStream(), reinterpret_cast(d_twists.data()), 6, 4, 16, n, + reinterpret_cast(d_poses.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + std::vector poses(n); + d_poses.CopyToHost(poses.data(), n); + return poses; +} + +// --------------------------------------------------------------------------- +// Named problem sizes. +// --------------------------------------------------------------------------- + +struct PGOConfig { + size_t num_poses; + const char *size_label; +}; +constexpr std::array kPGOConfigs = {{ + {10000, "10k_poses"}, + {100000, "100k_poses"}, + {1000000, "1M_poses"}, +}}; + +// Sparse SBA visibility: each landmark is observed by `obs_per_landmark` +// poses (one deterministic "primary" pose -- landmark_idx % n_poses, which +// guarantees every pose is observed at least once since n_points >= +// n_poses in every config below -- plus obs_per_landmark - 1 further +// distinct poses chosen pseudo-randomly). This keeps total factor counts +// (n_points * obs_per_landmark) far below the ~1.3M/FactorBatch safety +// limit for all three sizes while giving every landmark >1 observation +// (needed to constrain its 3D position) and every pose several +// observations, unlike a dense pose x landmark grid which would be +// infeasible at these sizes. +struct SBAConfig { + int n_poses; + int n_points; + int obs_per_landmark; + const char *size_label; +}; +constexpr std::array kSBAConfigs = {{ + {1000, 5000, 3, "1kposes_5klandmarks"}, + {5000, 25000, 3, "5kposes_25klandmarks"}, + {10000, 100000, 3, "10kposes_100klandmarks"}, +}}; + +struct PnPConfig { + size_t n; + const char *size_label; +}; +constexpr std::array kPnPConfigs = {{ + {1000, "1k_correspondences"}, + {100000, "100k_correspondences"}, + {1000000, "1M_correspondences"}, +}}; + +std::string SizeLabel(ProblemType pt, int size_index) { + switch (pt) { + case ProblemType::kPGO: + return kPGOConfigs[size_index].size_label; + case ProblemType::kSBA: + return kSBAConfigs[size_index].size_label; + case ProblemType::kPnP: + return kPnPConfigs[size_index].size_label; + } + return "?"; +} + +struct PerfParams { + ProblemType problem_type; + int size_index; // 0, 1, 2 -- indexes into the per-problem-type config table above. + JacobianMode mode; +}; + +std::string ParamLabel(const PerfParams &p) { + return std::string(ToString(p.problem_type)) + "_" + SizeLabel(p.problem_type, p.size_index) + + "_" + ToString(p.mode); +} + +std::ostream &operator<<(std::ostream &os, const PerfParams &p) { return os << ParamLabel(p); } + +class NumericDiffPerfTest : public ::testing::TestWithParam { + protected: + static constexpr int kWarmupIters = 3; + static constexpr int kTimedIters = 10; + + // Appends one CSV row; writes the header once (file created fresh at + // SetUpTestSuite time). + static void AppendCsvRow(const std::string &problem_type, const std::string &size_label, + int size_index, size_t n_primary, size_t n_secondary, + const std::string &mode, double mean_ms) { + std::ofstream f(CsvPath(), std::ios::app); + f << problem_type << "," << size_label << "," << size_index << "," << n_primary << "," + << n_secondary << "," << mode << "," << mean_ms << "\n"; + } + + static std::string CsvPath() { return "/tmp/cunls_numeric_diff_perf/results.csv"; } + + static void SetUpTestSuite() { + ::mkdir("/tmp/cunls_numeric_diff_perf", 0755); + std::ofstream f(CsvPath(), std::ios::trunc); + f << "problem_type,size_label,size_index,n_primary,n_secondary,jacobian_mode," + "mean_ms_per_build_system\n"; + } + + /** + * @brief Runs kWarmupIters untimed + kTimedIters CUDA-event-timed + * BuildSystem calls, wrapped in a per-iteration NVTX range named after + * (problem_type, size, mode) so it is identifiable in an nsys timeline. + * Records the mean elapsed ms to the CSV and returns it. + */ + double TimeBuildSystem(SystemBuilder &builder, Problem &problem, const PerfParams &p, + size_t n_primary, size_t n_secondary) { + CudaStream stream; + builder.Prepare(stream.GetStream(), problem); + + for (int i = 0; i < kWarmupIters; ++i) { + builder.Build(stream.GetStream(), problem); + } + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + + cudaEvent_t start, stop; + THROW_ON_CUDA_ERROR(cudaEventCreate(&start)); + THROW_ON_CUDA_ERROR(cudaEventCreate(&stop)); + + profiler::Domain domain("NumericDiffPerfTest"); + double total_ms = 0.0; + for (int i = 0; i < kTimedIters; ++i) { + auto range = domain.CreateDomainRange(ParamLabel(p) + "/BuildSystem"); + THROW_ON_CUDA_ERROR(cudaEventRecord(start, stream.GetStream())); + builder.Build(stream.GetStream(), problem); + THROW_ON_CUDA_ERROR(cudaEventRecord(stop, stream.GetStream())); + THROW_ON_CUDA_ERROR(cudaEventSynchronize(stop)); + float ms = 0.f; + THROW_ON_CUDA_ERROR(cudaEventElapsedTime(&ms, start, stop)); + total_ms += ms; + } + + THROW_ON_CUDA_ERROR(cudaEventDestroy(start)); + THROW_ON_CUDA_ERROR(cudaEventDestroy(stop)); + + double mean_ms = total_ms / kTimedIters; + AppendCsvRow(ToString(p.problem_type), SizeLabel(p.problem_type, p.size_index), p.size_index, + n_primary, n_secondary, ToString(p.mode), mean_ms); + return mean_ms; + } + + MinimizerOptions MakeOptions(JacobianMode mode) { + MinimizerOptions options; + options.jacobian_mode = mode; + options.disable_safety_checks = true; + options.sparse_linear_solver_type = test_utils::SolverTypeFromEnv(); + return options; + } + + cuBLASHandle cublas_handle_; +}; + +TEST_P(NumericDiffPerfTest, BuildSystemTiming) { + const PerfParams p = GetParam(); + SCOPED_TRACE(ParamLabel(p)); + + double mean_ms = 0.0; + + switch (p.problem_type) { + case ProblemType::kPGO: { + const size_t num_poses = kPGOConfigs[p.size_index].num_poses; + const size_t num_factors = num_poses - 1; + std::mt19937 rng(1000); + std::vector gt_poses = RandomPoses(num_poses, rng); + + // Deltas satisfying delta_i = T_i^{-1} * T_{i+1} exactly, so the + // problem is well-posed (not required for a timing-only benchmark, but + // cheap and keeps BuildSystem's cost path realistic). + std::vector deltas(num_factors); + { + // delta = T_i^{-1} * T_{i+1}; computed via device inverse, matching + // other tests' conventions. + CudaStream stream; + dvector d_poses(gt_poses); + dvector d_inv(num_poses); + ComputeInverseSE3(stream.GetStream(), reinterpret_cast(d_poses.data()), 4, + 16, 4, 16, num_poses, reinterpret_cast(d_inv.data())); + THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream())); + std::vector inv(num_poses); + d_inv.CopyToHost(inv.data(), num_poses); + for (size_t i = 0; i < num_factors; ++i) { + deltas[i] = ComposeSE3Host(inv[i], gt_poses[i + 1]); + } + } + + dvector poses_device(gt_poses); + dvector deltas_device(deltas); + std::vector const_ids = {0}; + dvector const_ids_device(const_ids); + + SE3StateBatch pose_states(cublas_handle_, + reinterpret_cast(poses_device.data()), num_poses, + const_ids_device.data(), 1); + SE3BetweenFactorBatch between_factor(deltas_device.data(), num_factors); + + std::vector state_pointers; + state_pointers.reserve(2 * num_factors); + for (size_t i = 0; i < num_factors; ++i) { + state_pointers.push_back(pose_states.StateBlockDevicePtr(i)); + state_pointers.push_back(pose_states.StateBlockDevicePtr(i + 1)); + } + + Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddFactorBatch(&between_factor, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + SystemBuilder builder(MakeOptions(p.mode)); + mean_ms = TimeBuildSystem(builder, problem, p, num_poses, 0); + break; + } + case ProblemType::kSBA: { + const SBAConfig &cfg = kSBAConfigs[p.size_index]; + const int n_poses = cfg.n_poses; + const int n_points = cfg.n_points; + const int obs_per_landmark = cfg.obs_per_landmark; + std::mt19937 rng(2000); + std::vector gt_poses = RandomPoses(n_poses, rng); + + std::uniform_real_distribution xy(-8.f, 8.f); + std::uniform_real_distribution zz(3.f, 15.f); + std::vector> points(n_points); + for (int i = 0; i < n_points; ++i) points[i] = {xy(rng), xy(rng), zz(rng)}; + + // Sparse visibility: each landmark observed by `obs_per_landmark` + // poses -- one deterministic primary pose (landmark_idx % n_poses, + // guaranteeing full pose coverage since n_points >= n_poses here) plus + // (obs_per_landmark - 1) further distinct random poses. See the + // SBAConfig comment above for the full rationale. + std::uniform_int_distribution pose_pick(0, n_poses - 1); + std::vector> observations; + std::vector state_pointers; + const size_t expected_factors = static_cast(n_points) * obs_per_landmark; + observations.reserve(expected_factors); + state_pointers.reserve(2 * expected_factors); + + dvector poses_device(gt_poses); + dvector> points_device(points); + std::vector const_pose_ids = {0}; + dvector const_pose_ids_device(const_pose_ids); + + SE3StateBatch pose_states(cublas_handle_, + reinterpret_cast(poses_device.data()), n_poses, + const_pose_ids_device.data(), 1); + VectorStateBatch<3> point_states(reinterpret_cast(points_device.data()), + n_points); + + std::vector chosen_poses; + chosen_poses.reserve(obs_per_landmark); + for (int pt_idx = 0; pt_idx < n_points; ++pt_idx) { + chosen_poses.clear(); + chosen_poses.push_back(pt_idx % n_poses); + while (static_cast(chosen_poses.size()) < obs_per_landmark && + static_cast(chosen_poses.size()) < n_poses) { + int candidate = pose_pick(rng); + if (std::find(chosen_poses.begin(), chosen_poses.end(), candidate) == + chosen_poses.end()) { + chosen_poses.push_back(candidate); + } + } + + const Vector<3> &pt = points[pt_idx]; + for (int pose_idx : chosen_poses) { + const SE3Transform &T = gt_poses[pose_idx]; + float pc[3]; + pc[0] = T[3] + T[0] * pt[0] + T[1] * pt[1] + T[2] * pt[2]; + pc[1] = T[7] + T[4] * pt[0] + T[5] * pt[1] + T[6] * pt[2]; + pc[2] = T[11] + T[8] * pt[0] + T[9] * pt[1] + T[10] * pt[2]; + Vector<2> obs{pc[0] / pc[2], pc[1] / pc[2]}; + observations.push_back(obs); + state_pointers.push_back(pose_states.StateBlockDevicePtr(pose_idx)); + state_pointers.push_back(point_states.StateBlockDevicePtr(pt_idx)); + } + } + + dvector> observations_device(observations); + ReprojectionFactorBatch reproj(observations_device.data(), observations.size(), 1e-3f); + + Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddStateBatch(&point_states); + problem.AddFactorBatch(&reproj, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + SystemBuilder builder(MakeOptions(p.mode)); + mean_ms = TimeBuildSystem(builder, problem, p, n_poses, n_points); + break; + } + case ProblemType::kPnP: { + const size_t n = kPnPConfigs[p.size_index].n; + std::mt19937 rng(3000); + std::vector gt_pose_vec = RandomPoses(1, rng); + const SE3Transform &T = gt_pose_vec[0]; + + std::uniform_real_distribution xy(-4.f, 4.f); + std::uniform_real_distribution zz(3.f, 12.f); + std::vector> points(n); + std::vector> observations(n); + for (size_t i = 0; i < n; ++i) { + Vector<3> p{xy(rng), xy(rng), zz(rng)}; + float pc[3]; + pc[0] = T[3] + T[0] * p[0] + T[1] * p[1] + T[2] * p[2]; + pc[1] = T[7] + T[4] * p[0] + T[5] * p[1] + T[6] * p[2]; + pc[2] = T[11] + T[8] * p[0] + T[9] * p[1] + T[10] * p[2]; + points[i] = p; + observations[i] = {pc[0] / pc[2], pc[1] / pc[2]}; + } + + dvector> points_device(points); + dvector> observations_device(observations); + dvector pose_device(gt_pose_vec); + + SE3StateBatch pose_states(cublas_handle_, reinterpret_cast(pose_device.data()), + 1); + PnPFactorBatch pnp(observations_device.data(), points_device.data(), n, 1e-3f); + + std::vector state_pointers(n, pose_states.StateBlockDevicePtr(0)); + + Problem problem; + problem.AddStateBatch(&pose_states); + problem.AddFactorBatch(&pnp, state_pointers); + ASSERT_TRUE(problem.CheckConsistency()); + + SystemBuilder builder(MakeOptions(p.mode)); + mean_ms = TimeBuildSystem(builder, problem, p, n, 0); + break; + } + } + + EXPECT_GT(mean_ms, 0.0); + std::cout << "[NumericDiffPerfTest] " << ParamLabel(p) << ": " << mean_ms << " ms/BuildSystem\n"; +} + +std::vector AllParams() { + std::vector out; + for (ProblemType pt : {ProblemType::kPGO, ProblemType::kSBA, ProblemType::kPnP}) { + for (int size_index = 0; size_index < 3; ++size_index) { + for (JacobianMode m : {JacobianMode::kAnalytic, JacobianMode::kNumeric}) { + out.push_back({pt, size_index, m}); + } + } + } + return out; +} + +INSTANTIATE_TEST_SUITE_P(Sweep, NumericDiffPerfTest, ::testing::ValuesIn(AllParams()), + [](const ::testing::TestParamInfo &info) { + return ParamLabel(info.param); + }); + +} // namespace +} // namespace cunls diff --git a/tests/residual_batch_test.cpp b/tests/residual_batch_test.cpp index 9fd5f56..59943eb 100644 --- a/tests/residual_batch_test.cpp +++ b/tests/residual_batch_test.cpp @@ -278,8 +278,8 @@ TYPED_TEST(ResidualBatchTest, Jacobians) { std::vector> gt_jacobian(this->num_vectors_); { - // Calculate the ground truth robustified jacobian, - // Refer to http://ceres-solver.org/nnls_modeling.html#lossfunction + // Calculate the ground truth robustified jacobian using the standard + // Triggs-correction rescaling for a robust loss function rho(s). float jac_value = -sqrt_rho1 * alpha * residual * residual; for (auto &x : gt_jacobian) {