C++ Tutorial#

This tutorial covers the cuNLS C++ API. For the Python version (pycunls), see Tutorial.

Important

Capacity vs. active count. Factor and state batches are constructed with their capacity (how many factors / states their buffers hold) and start with zero active entries: call SetNumActiveFactors(n) / SetNumActiveStates(n) before solving, and again whenever the problem size changes. See Capacity and active count.

Overview#

This tutorial walks through three complete C++ cuNLS examples, each demonstrating a different optimization pattern. Every example follows the same high-level flow described in the Introduction:

  1. Allocate state data on the GPU.

  2. Wrap state memory in one or more StateBatch objects (see State API).

  3. Build one or more FactorBatch objects from observations (see Factor API).

  4. Add state batches and factor batches to a Problem (see Minimizer API).

  5. Run a minimizer and inspect MinimizerSummary.

The examples increase in complexity:

  • Sparse Bundle Adjustment — uses built-in ReprojectionFactorBatch to jointly optimize camera poses and 3D landmarks from multi-view observations.

  • Pose Graph Optimization — uses BetweenFactorBatch (the manifold-generic facade, deduced to SE(3) here) to recover a chain of SE(3) poses from consecutive relative-transform measurements.

  • Custom Factor — shows how to implement a user-defined CUDA factor kernel by subclassing SizedFactorBatch.

Full source code for all examples lives in the examples/ directory and is built by the shared examples/CMakeLists.txt.

Common build commands#

Build all examples locally:

cmake -S examples -B build/examples/all \
  -DCMAKE_BUILD_TYPE=Release \
  -DCUNLS_INSTALL_DIR=/path/to/cunls_install
cmake --build build/examples/all -j

Build all examples in Docker and export binaries:

./examples/build_in_docker.sh Release ./artifacts/examples

Sparse Bundle Adjustment#

  • Source: examples/sparse_bundle_adjustment/main.cpp

Problem statement#

Bundle adjustment is the problem of jointly refining 3D structure and camera parameters to minimize reprojection error — the difference between where a 3D point actually projects into an image and where it was observed.

Given \(M\) cameras with poses \(T_1, \ldots, T_M \in \mathrm{SE}(3)\) (each camera’s pose in the world, world_from_camera; see pose-convention) and \(N\) 3D landmarks \(\mathbf{p}_1, \ldots, \mathbf{p}_N \in \mathbb{R}^3\), we form one residual per observation. The projection model transforms a world point \(\mathbf{p}\) into camera \(i\)’s frame and divides by depth to obtain normalized image coordinates:

\[\begin{split}\mathbf{p}_{\mathrm{cam}} = T_i^{-1} \, \mathbf{p}, \qquad \hat{\mathbf{z}} = \begin{bmatrix} p_{\mathrm{cam},x} / p_{\mathrm{cam},z} \\ p_{\mathrm{cam},y} / p_{\mathrm{cam},z} \end{bmatrix}\end{split}\]

The reprojection residual for observation \(k\), which pairs camera \(i\) with point \(j\), is:

\[r_k = \hat{\mathbf{z}}_k - \mathbf{z}_k = \pi(T_i,\, \mathbf{p}_j) - \mathbf{z}_k\]

where \(\mathbf{z}_k\) is the measured 2D observation and \(\pi\) is the normalized projection function above.

The full bundle adjustment objective minimizes the sum of squared reprojection errors across all \(K\) observations:

\[\min_{T_1,\ldots,T_M,\; \mathbf{p}_1,\ldots,\mathbf{p}_N} \frac{1}{2} \sum_{k=1}^{K} \left\| \pi(T_{i_k}, \mathbf{p}_{j_k}) - \mathbf{z}_k \right\|^2\]

Because applying a rigid transform to every pose and point, or scaling the whole scene, leaves all reprojection residuals unchanged, the system has a 7-DOF gauge freedom (6 rigid + 1 scale). Fixing two camera poses as gauge anchors removes it. In this example, the first two poses \(T_0, T_1\) are held constant while the remaining poses and all 3D points are jointly optimized — the classic full bundle adjustment problem.

BA factor graph#

The problem has a bipartite factor graph: camera-pose variable nodes on one side, 3D-point variable nodes on the other, and reprojection factor nodes connecting them.

Bundle adjustment factor graph Bundle adjustment factor graph

Factor graph for sparse bundle adjustment. The blue circle is a fixed anchor pose \(T_0\)(the example also fixes \(T_1\) for the scale), green circles are optimized camera poses (SE3StateBatch) and 3D point variables (VectorStateBatch<3>), and orange squares are reprojection factors (ReprojectionFactorBatch). Each factor connects one camera and one point.

BA API used#

Class

Role

SE3StateBatch (State API)

Stores camera poses on the SE(3) manifold. The first two poses are marked constant via device_constant_state_ids; the rest are optimized.

VectorStateBatch<3> (State API)

Stores 3D landmark coordinates in \(\mathbb{R}^3\). All points are optimization variables.

ReprojectionFactorBatch (Factor API)

Computes normalized reprojection residuals and Jacobians for each (pose, point) pair.

Problem (Minimizer API)

Assembles the factor graph: connects factors to states via device pointers.

LevenbergMarquardtMinimizer (Minimizer API)

Iteratively solves the nonlinear least-squares problem with adaptive damping.

BA code walkthrough#

Step 1 — Generate synthetic data. MakeBundleAdjustmentScene returns ground-truth SE(3) poses and 3D points (every point in front of every camera), a perturbed initial guess (poses \(T_0\) and \(T_1\) stay exact), and the normalized observation of every point in every camera. The scene comes from examples/utils/datasets.h; the generators are ordinary host code and are not part of the lesson.

// 1. Synthetic data: ground truth, perturbed initial guess, observations.
const examples::BundleAdjustmentScene scene =
    examples::MakeBundleAdjustmentScene(num_poses, num_points);

Step 2 — Upload to the GPU. dvector owns device memory. The initial guess is uploaded into the buffers the solver will update in place; constant_pose_ids lists the gauge anchors.

// 2. Upload the initial guess and the observations to the GPU.
dvector<SE3Transform> poses(scene.initial_poses);
dvector<Vector<3>> points(scene.initial_points);
dvector<Vector<2>> observations(scene.observations);
// Gauge anchors: camera 0 fixes the frame, camera 1 the scale (unobservable from
// reprojections alone).
dvector<int> constant_pose_ids(std::vector<int>{0, 1});

Step 3 — Wrap the device memory in state batches. A state batch wraps device memory without copying it. Poses use SE3StateBatch with states 0 and 1 marked constant (the gauge anchors); points use VectorStateBatch<3> (see State API).

Capacity and active count. The count passed to a batch constructor is its capacity: how many states (state batches) or factors (factor batches) the bound device buffers hold, fixed for the batch’s lifetime. Right after construction nothing is active. SetNumActiveStates / SetNumActiveFactors set the active count: how many of the first states or factors the next solve uses. They are host-only (no allocation) and may be called again between solves with any count up to the capacity, so one set of batches serves problems of changing size. In this example every slot is used, so active = capacity.

// 3. State batches wrap the device memory: SE(3) poses (two constant) and 3D points.
//    Capacity vs. active count. A batch is constructed with its capacity: how many states (or
//    factors) its bound device buffers hold. The capacity is fixed for the batch's lifetime;
//    size it once for the largest problem you expect. Right after construction nothing is
//    active: SetNumActiveStates / SetNumActiveFactors set the active count, how many of the
//    first slots the next solve uses (a solve without it throws). The setter is host-only (no
//    allocation, no device work) and may change the count between solves up to the capacity,
//    which is what lets a real-time application allocate once and reuse the same buffers every
//    frame while the problem size changes. This example solves every slot once, so each active
//    count equals its capacity.
const size_t poses_capacity = num_poses;  // every slot solved: active = capacity
const size_t points_capacity = num_points;
const size_t const_poses_capacity = 2;  // entries of constant_pose_ids
cunls::SE3StateBatch pose_states(reinterpret_cast<const float *>(poses.data()), poses_capacity,
                                 constant_pose_ids.data(), const_poses_capacity);
cunls::VectorStateBatch<3> point_states(reinterpret_cast<const float *>(points.data()),
                                        points_capacity);
const size_t num_const_poses = 2;  // active constant ids: the gauge anchors
pose_states.SetNumActiveStates(num_poses, num_const_poses);  // active counts
point_states.SetNumActiveStates(num_points);

Step 4 — Build the reprojection factor batch and its state pointers. Each reprojection factor reads two states, [pose, point]. The state-pointer list is flattened in factor order, two pointers per factor, and tells cuNLS which states every factor reads. z_threshold guards points almost behind a camera (see Factor API). Like state batches, a factor batch is constructed with its capacity and starts with 0 active factors: SetNumActiveFactors activates them.

// 4. One reprojection factor per observation, reading [pose, point].
//    Capacity (fixed, sizes the buffers) vs. active count (set per solve): see step 3.
const size_t observations_capacity = num_observations;  // every slot solved
cunls::ReprojectionFactorBatch reprojection(observations.data(), observations_capacity,
                                            /*z_threshold=*/1e-3f);
reprojection.SetNumActiveFactors(num_observations);  // active count
std::vector<float *> state_pointers;
for (size_t c = 0; c < num_poses; ++c) {
  for (size_t j = 0; j < num_points; ++j) {
    state_pointers.push_back(pose_states.StateDevicePtr(c));
    state_pointers.push_back(point_states.StateDevicePtr(j));
  }
}

Step 5 — Assemble the problem. Problem connects state batches and factor batches. CheckConsistency verifies that every state pointer lies inside a registered state batch.

// 5. The problem: states plus factors with their state pointers.
cunls::Problem problem;
problem.AddStateBatch(&pose_states);
problem.AddStateBatch(&point_states);
problem.AddFactorBatch(&reprojection, state_pointers);
if (!problem.CheckConsistency()) {
  std::cerr << "Problem consistency check failed\n";
  return 1;
}

Step 6 — Solve with Levenberg-Marquardt. LevenbergMarquardtMinimizer (see Minimizer API) solves the damped normal equations each iteration and adapts \(\lambda\) from the step quality. Minimize writes the solution back into the state batches’ memory.

// 6. Solve with Levenberg-Marquardt; the result is written into poses / points.
cunls::LevenbergMarquardtMinimizerOptions options;
options.base_options.max_num_iterations = 80;
options.base_options.state_tolerance = 1e-8f;
options.base_options.cost_tolerance = 1e-8f;
options.initial_lambda = 1e-3f;
cunls::LevenbergMarquardtMinimizer minimizer(options);
cunls::CudaStream stream;
const cunls::MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem);
THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream()));

Step 7 — Read back and validate. Copy the optimized poses and points back to the host, compare them with the ground truth, print the summary, and turn the quality check into the exit code.

// 7. Read back and compare with the ground truth.
std::vector<SE3Transform> final_poses(num_poses);
std::vector<Vector<3>> final_points(num_points);
poses.CopyToHost(final_poses.data(), num_poses);
points.CopyToHost(final_points.data(), num_points);
const float point_mse_before =
    examples::ComputeVectorMSE(scene.initial_points, scene.gt_points);
const float point_mse_after = examples::ComputeVectorMSE(final_points, scene.gt_points);

examples::PrintTitle("Sparse Bundle Adjustment Example");
examples::PrintSummary(summary);
examples::PrintChange("Point MSE", point_mse_before, point_mse_after);
examples::PrintChange("Pose MSE", examples::ComputePoseMSE(scene.initial_poses, scene.gt_poses),
                      examples::ComputePoseMSE(final_poses, scene.gt_poses));
return examples::QualityExitCode(summary.final_cost <= 1e-3f &&
                                 point_mse_after <= point_mse_before * 0.05f);

Pose Graph Optimization#

  • Source: examples/pose_graph_optimization/main.cpp

PGO problem statement#

Pose graph optimization (PGO) is a key building block in Simultaneous Localization and Mapping (SLAM). Given a set of poses and pairwise relative-transform measurements between them, the goal is to find the configuration of poses that best satisfies all measurements.

This example models a pose chain: \(N\) poses \(T_0, T_1, \ldots, T_{N-1} \in \mathrm{SE}(3)\) connected by \(N{-}1\) consecutive between constraints. Each constraint \(i\) connects pose \(T_i\) to pose \(T_{i+1}\) and carries a measured relative transform \(\Delta_i\). The relative-transform residual is defined on the SE(3) Lie algebra:

\[r_i = \mathrm{Log}\!\left( \Delta_i \, T_i^{-1} \, T_{i+1} \right) \in \mathbb{R}^6\]

The residual \(r_i\) is the 6-DOF twist that measures how far the observed relative transform deviates from the measurement. When the constraint is exactly satisfied, the argument of \(\mathrm{Log}\) is the identity and \(r_i = 0\).

Because adding a rigid transform to every pose leaves all relative residuals unchanged, the system is rank-deficient without further constraints. Fixing the first pose \(T_0\) as a gauge anchor removes this freedom.

The optimization objective minimizes the sum of squared residuals over all non-fixed poses:

\[\min_{T_1,\ldots,T_{N-1}} \frac{1}{2} \sum_{i=0}^{N-2} \left\| \mathrm{Log}\!\left( \Delta_i \, T_i^{-1} \, T_{i+1} \right) \right\|^2\]

For a thorough introduction to graph-based SLAM see Grisetti et al., A Tutorial on Graph-Based SLAM, IEEE Intelligent Transportation Systems Magazine, 2010.

PGO factor graph#

The factor graph is a chain: each pose node connects to its neighbors through between factors, and the first pose \(T_0\) is held fixed.

Pose graph optimization factor graph Pose graph optimization factor graph

Factor graph for pose graph optimization. The blue circle is the fixed anchor pose \(T_0\), green circles are optimized poses (SE3StateBatch), and orange squares are between factors (BetweenFactorBatch, deduced to SE(3)). Each factor encodes a measured relative transform \(\Delta_i\).

PGO API used#

Class

Role

SE3StateBatch (State API)

A single instance stores the full pose chain. The first pose is marked constant via device_constant_state_ids; the rest are optimized.

BetweenFactorBatch<Manifold> (Factor API)

Manifold-generic facade; deduced here to SE(3) via CTAD from the deltas pointer’s type. Computes the relative-transform residual \(\mathrm{Log}(\Delta \, T_i^{-1} \, T_{i+1})\) and its Jacobians w.r.t. both poses.

Problem (Minimizer API)

Assembles the factor graph.

LevenbergMarquardtMinimizer (Minimizer API)

Solves the nonlinear system.

PGO code walkthrough#

Step 1 — Generate the pose chain and its measurements. MakePoseChainScene returns a ground-truth chain of SE(3) poses, the relative transform (delta) between each consecutive pair, and a disturbed initial guess. The scene comes from examples/utils/datasets.h; the generators are ordinary host code and are not part of the lesson.

// 1. Synthetic data: ground-truth chain, disturbed initial guess, and the
//    relative transforms (deltas) between consecutive poses.
const examples::PoseChainScene scene = examples::MakePoseChainScene(num_poses);

Step 2 — Upload to the GPU. Upload the initial guess (updated in place by the solver) and the measurements. Pose \(T_0\) is the gauge anchor.

// 2. Upload the initial guess and the measurements to the GPU.
dvector<SE3Transform> poses(scene.initial_poses);
dvector<SE3Transform> deltas(scene.deltas);
dvector<int> constant_pose_ids(std::vector<int>{0});  // T_0 is the gauge anchor

Step 3 — Wrap the chain in one state batch. All poses live in a single SE3StateBatch; only state 0 is constant. SetNumActiveStates activates all poses and the constant state (batches start with 0 active entries).

// 3. One state batch for the whole chain.
//    Capacity vs. active count. A batch is constructed with its capacity: how many states (or
//    factors) its bound device buffers hold. The capacity is fixed for the batch's lifetime;
//    size it once for the largest problem you expect. Right after construction nothing is
//    active: SetNumActiveStates / SetNumActiveFactors set the active count, how many of the
//    first slots the next solve uses (a solve without it throws). The setter is host-only (no
//    allocation, no device work) and may change the count between solves up to the capacity,
//    which is what lets a real-time application allocate once and reuse the same buffers every
//    frame while the problem size changes. This example solves every slot once, so each active
//    count equals its capacity.
const size_t poses_capacity = num_poses;  // every slot solved: active = capacity
const size_t const_poses_capacity = 1;    // entries of constant_pose_ids
cunls::SE3StateBatch pose_states(reinterpret_cast<const float *>(poses.data()), poses_capacity,
                                 constant_pose_ids.data(), const_poses_capacity);
const size_t num_const_poses = 1;  // active constant ids: the gauge anchor
pose_states.SetNumActiveStates(num_poses, num_const_poses);  // active counts

Step 4 — Build the between factors and their state pointers. BetweenFactorBatch deduces its manifold (SE(3)) from the type of the measurements. Factor \(i\) reads [T_i, T_{i+1}]. SetNumActiveFactors activates all of them.

// 4. Between factors; the manifold (SE(3)) is deduced from the deltas' type.
//    Factor i reads [T_i, T_{i+1}]. Capacity (fixed, sizes the buffers) vs.
//    active count (set per solve): see step 3.
const size_t constraints_capacity = num_constraints;  // every slot solved
cunls::BetweenFactorBatch between(deltas.data(), constraints_capacity);
between.SetNumActiveFactors(num_constraints);  // active count
std::vector<float *> state_pointers;
for (size_t i = 0; i < num_constraints; ++i) {
  state_pointers.push_back(pose_states.StateDevicePtr(i));
  state_pointers.push_back(pose_states.StateDevicePtr(i + 1));
}

Step 5 — Assemble the problem. Register the state batch and the factor batch with their state pointers.

// 5. The problem.
cunls::Problem problem;
problem.AddStateBatch(&pose_states);
problem.AddFactorBatch(&between, state_pointers);
if (!problem.CheckConsistency()) {
  std::cerr << "Problem consistency check failed\n";
  return 1;
}

Step 6 — Solve with Levenberg-Marquardt. Same solver as in the bundle adjustment example.

// 6. Solve with Levenberg-Marquardt; the result is written into poses.
cunls::LevenbergMarquardtMinimizerOptions options;
options.base_options.max_num_iterations = 60;
options.base_options.state_tolerance = 1e-8f;
options.base_options.cost_tolerance = 1e-8f;
options.initial_lambda = 1e-3f;
cunls::LevenbergMarquardtMinimizer minimizer(options);
cunls::CudaStream stream;
const cunls::MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem);
THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream()));

Step 7 — Read back and validate. Copy the chain back and measure how well it satisfies its relative constraints before and after the solve.

// 7. Read back and measure how well the chain satisfies its constraints.
std::vector<SE3Transform> final_poses(num_poses);
poses.CopyToHost(final_poses.data(), num_poses);
const float error_before = examples::ChainConstraintError(scene.initial_poses, scene.deltas);
const float error_after = examples::ChainConstraintError(final_poses, scene.deltas);

examples::PrintTitle("Pose Graph Optimization Example (Chain)");
examples::PrintValue("Num poses", num_poses);
examples::PrintValue("Num constraints", num_constraints);
examples::PrintSummary(summary);
examples::PrintChange("Constraint MSE", error_before, error_after);
return examples::QualityExitCode(summary.final_cost <= 1e-2f &&
                                 error_after <= error_before * 0.05f);

Custom Factor#

  • Source: examples/custom_factor/main.cu

Note

This walkthrough shows the mechanics on a small example. The complete contract of Evaluate (items, factor_ids, num_factor_ids) and Plus (num_replicas), which every custom type must honor to work with the RANSAC minimizers, is explained in Custom Factors and States (Python and C++).

Custom factor problem statement#

This example shows how to implement a user-defined factor by subclassing SizedFactorBatch (see Factor API) and writing a CUDA kernel that computes residuals and Jacobians.

We model a 1-D chain of \(N\) scalar states \(x_0, x_1, \ldots, x_{N-1}\) connected by \(N{-}1\) difference constraints. Each constraint carries a measurement \(m_i\) of the expected difference between consecutive states:

\[r_i = (x_{i+1} - x_i) - m_i, \qquad i = 0, \ldots, N{-}2\]

The Jacobians are trivially constant:

\[\frac{\partial r_i}{\partial x_i} = -1, \qquad \frac{\partial r_i}{\partial x_{i+1}} = +1\]

Because adding a constant to every state leaves all difference residuals unchanged, the system is rank-deficient without further constraints. A prior factor (anchor) on the first state removes this gauge freedom:

\[r_{\mathrm{prior}} = x_0 - x_0^{\mathrm{obs}}\]

The full objective is:

\[\min_{x_0, \ldots, x_{N-1}} \frac{1}{2} \left\| x_0 - x_0^{\mathrm{obs}} \right\|^2 + \frac{1}{2} \sum_{i=0}^{N-2} \left\| (x_{i+1} - x_i) - m_i \right\|^2\]

This is a simple linear-in-state problem, but it demonstrates the full workflow for authoring custom factors.

Custom factor graph#

The factor graph is a chain: each variable node connects to its neighbors through difference factors, and a prior factor anchors \(x_0\).

Custom factor (1D chain) factor graph Custom factor (1D chain) factor graph

Factor graph for the custom factor example. Green circles are scalar state variables (VectorStateBatch<1>), orange squares are custom difference factors (ScalarDifferenceFactorBatch), and the purple square is the anchor prior (PriorFactorBatch<manifold::Vector<1>>) on \(x_0\).

Custom factor API used#

Class

Role

VectorStateBatch<1> (State API)

Stores all \(N\) scalar states in \(\mathbb{R}^1\).

SizedFactorBatch<1, 1, 1> (Factor API)

Compile-time base for the custom factor (residual dim = 1, two states of tangent dim 1 each).

PriorFactorBatch<manifold::Vector<1>> (Factor API)

Manifold-generic facade over the built-in prior factor that pulls \(x_0\) toward the observed anchor value.

Problem (Minimizer API)

Assembles the factor graph.

LevenbergMarquardtMinimizer (Minimizer API)

Solves the nonlinear system.

Custom factor code walkthrough#

All steps of the solve live in RunChainExample, which main calls twice: Part 1 with the analytic factor, Part 2 with the residual-only factor and numeric Jacobians.

Step 1 — Implement the CUDA kernel. The kernel is launched with one thread per item: one factor evaluated against its own set of states. Item idx reads the measurement of its factor (factor_ids[idx], or idx % num_factors when factor_ids is null), its two state pointers state_pointers[2 * idx ..], and writes row idx of the outputs. The regular minimizers pass factor_ids == nullptr and one item per factor; the RANSAC minimizers evaluate many items per factor (see Custom Factors and States (Python and C++)).

__global__ void ScalarDifferenceKernel(const float *measurements, const int *factor_ids,
                                       size_t num_factors, float const *const *state_pointers,
                                       float *residuals, float *jacobians, size_t num_items) {
  const size_t idx = static_cast<size_t>(blockIdx.x) * blockDim.x + threadIdx.x;
  if (idx >= num_items) {
    return;
  }

  const size_t factor = factor_ids != nullptr ? factor_ids[idx] : idx % num_factors;
  const float *left = state_pointers[idx * 2];
  const float *right = state_pointers[idx * 2 + 1];
  const float residual = (right[0] - left[0]) - measurements[factor];

  if (residuals != nullptr) {
    residuals[idx] = residual;
  }
  if (jacobians != nullptr) {
    jacobians[idx * 2] = -1.0f;
    jacobians[idx * 2 + 1] = 1.0f;
  }
}

Step 2 — Subclass SizedFactorBatch<1, 1, 1>. The template arguments encode the residual dimension (1) and the tangent dimensions of the two states (1, 1). The constructor passes the capacity (the number of measurements the buffer holds) to the base class, which keeps the active count NumActiveFactors(): 0 until SetNumActiveFactors is called. The class stores a device pointer to the measurements and launches the kernel in Evaluate over the active factors.

class ScalarDifferenceFactorBatch : public cunls::SizedFactorBatch<1, 1, 1> {
 public:
  ScalarDifferenceFactorBatch(const float *measurements, size_t capacity)
      : SizedFactorBatch(capacity), measurements_(measurements) {}

  bool Evaluate(float *residuals, float *jacobians, float const *const *state_pointers,
                cudaStream_t stream, const int *factor_ids = nullptr,
                size_t num_factor_ids = 0) const final {
    const size_t num_factors = NumActiveFactors();
    const size_t num_items = num_factor_ids == 0 ? num_factors : num_factor_ids;
    if (num_items == 0 || num_factors == 0) {
      return true;
    }
    constexpr int kBlockSize = 256;
    const int grid_size = static_cast<int>((num_items + kBlockSize - 1) / kBlockSize);
    ScalarDifferenceKernel<<<grid_size, kBlockSize, 0, stream>>>(
        measurements_, factor_ids, num_factors, state_pointers, residuals, jacobians, num_items);
    THROW_ON_CUDA_ERROR(cudaGetLastError());
    return true;
  }

 private:
  const float *measurements_;
};

Step 3 — Generate synthetic data. MakeScalarChainScene returns a monotonic ground-truth chain, a noisy initial guess, and the exact differences of consecutive states. The scene comes from examples/utils/datasets.h; the generators are ordinary host code and are not part of the lesson.

// 1. Synthetic data: ground truth, a noisy initial guess, exact differences.
const examples::ScalarChainScene scene = examples::MakeScalarChainScene(num_states);

Step 4 — Upload to the GPU. Upload the states and the differences. The prior’s target anchors \(x_0\): without it, adding a constant to every state would leave all differences unchanged.

// 2. Upload to the GPU. Without the anchor, adding a constant to every
//    state leaves all differences unchanged (rank-deficient system).
dvector<Vector<1>> states(scene.initial_states);
dvector<float> differences(scene.differences);
dvector<Vector<1>> anchor(std::vector<Vector<1>>{scene.gt_states[0]});

Step 5 — Build the state batch. All scalar states share one VectorStateBatch<1>, activated with SetNumActiveStates.

// 3. One state batch with all scalar states.
//    Capacity vs. active count. A batch is constructed with its capacity: how many states
//    (or factors) its bound device buffers hold. The capacity is fixed for the batch's lifetime;
//    size it once for the largest problem you expect. Right after construction nothing is active:
//    SetNumActiveStates / SetNumActiveFactors set the active count, how many of the first slots
//    the next solve uses (a solve without it throws). The setter is host-only (no allocation, no
//    device work) and may change the count between solves up to the capacity, which is what lets
//    a real-time application allocate once and reuse the same buffers every frame while the
//    problem size changes. This example solves every slot once, so each active count equals its
//    capacity.
const size_t states_capacity = num_states;  // every slot solved: active = capacity
cunls::VectorStateBatch<1> state_batch(reinterpret_cast<const float *>(states.data()),
                                       states_capacity);
state_batch.SetNumActiveStates(num_states);  // active count

Step 6 — Build the factor batches and their state pointers. Difference factors read [x_i, x_{i+1}]; the shipped prior reads x_0. The residual-only class is used in Part 2 (see Numeric (finite-difference) Jacobians). Every factor batch is activated with SetNumActiveFactors.

// 4. Factors: the custom difference factor (one of the two classes above)
//    reads [x_i, x_{i+1}]; the shipped prior reads x_0. Capacity (fixed,
//    sizes the buffers) vs. active count (set per solve): see step 3.
const size_t diff_capacity = num_diff_factors;  // every slot solved
const size_t anchor_capacity = 1;
ScalarDifferenceFactorBatch analytic_factor(differences.data(), diff_capacity);
ScalarDifferenceResidualOnlyFactorBatch residual_only_factor(differences.data(), diff_capacity);
cunls::PriorFactorBatch<cunls::manifold::Vector<1>> anchor_factor(anchor.data(), anchor_capacity);
const size_t num_anchor_factors = 1;
analytic_factor.SetNumActiveFactors(num_diff_factors);  // active counts
residual_only_factor.SetNumActiveFactors(num_diff_factors);
anchor_factor.SetNumActiveFactors(num_anchor_factors);
std::vector<float *> diff_pointers;
for (size_t i = 0; i < num_diff_factors; ++i) {
  diff_pointers.push_back(state_batch.StateDevicePtr(i));
  diff_pointers.push_back(state_batch.StateDevicePtr(i + 1));
}

Step 7 — Assemble the problem. Part 2 registers the residual-only factor with JacobianMode::kNumeric; the prior stays analytic in the same problem.

// 5. The problem. The per-group JacobianMode::kNumeric override makes cuNLS
//    differentiate the residual-only factor; the prior stays analytic, so one
//    Problem can mix both modes.
cunls::Problem problem;
problem.AddStateBatch(&state_batch);
if (use_numeric_jacobian) {
  problem.AddFactorBatch(&residual_only_factor, diff_pointers, cunls::JacobianMode::kNumeric);
} else {
  problem.AddFactorBatch(&analytic_factor, diff_pointers);
}
problem.AddFactorBatch(&anchor_factor, {state_batch.StateDevicePtr(0)});
if (!problem.CheckConsistency()) {
  std::cerr << "Problem consistency check failed\n";
  return 1;
}

Step 8 — Solve with Levenberg-Marquardt. Same solver as in the previous examples.

// 6. Solve with Levenberg-Marquardt.
cunls::LevenbergMarquardtMinimizerOptions options;
options.base_options.max_num_iterations = 50;
options.base_options.state_tolerance = 1e-8f;
options.base_options.cost_tolerance = 1e-8f;
options.initial_lambda = 1e-3f;
cunls::LevenbergMarquardtMinimizer minimizer(options);
cunls::CudaStream stream;
const cunls::MinimizerSummary summary = minimizer.Minimize(stream.GetStream(), problem);
THROW_ON_CUDA_ERROR(cudaStreamSynchronize(stream.GetStream()));

Step 9 — Read back and validate. Compare the solved chain with the ground truth and report.

// 7. Read back and compare with the ground truth.
std::vector<Vector<1>> final_states(num_states);
states.CopyToHost(final_states.data(), num_states);
const float mse_before = examples::ComputeVectorMSE(scene.initial_states, scene.gt_states);
const float mse_after = examples::ComputeVectorMSE(final_states, scene.gt_states);
examples::PrintTitle(title);
examples::PrintSummary(summary);
examples::PrintChange("State MSE", mse_before, mse_after);

// Numeric Jacobians are float32 finite differences, so Part 2 reaches the
// same optimum with a slightly looser cost tolerance.
const float cost_tolerance = use_numeric_jacobian ? 5e-4f : 1e-5f;
return examples::QualityExitCode(summary.final_cost <= cost_tolerance &&
                                 mse_after <= mse_before * 0.02f);