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 set_num_active_factors(n) /
set_num_active_states(n) before solving, and again whenever the problem
size changes. See Capacity and active count.
Overview#
This tutorial walks through four complete pycunls examples, each demonstrating a different optimization pattern. Every example follows the same high-level flow described in the Introduction:
Allocate state data on the GPU (CuPy arrays).
Wrap the GPU memory in one or more state batch objects.
Build one or more factor batch objects from observations.
Add state batches and factor batches to a Problem.
Run a minimizer and inspect MinimizerSummary.
The examples increase in complexity. For problems with gross outliers, see
Robust Estimation with RANSAC and python/examples/ransac_pnp.py; for visual-inertial
bundle adjustment, see IMU factor and
python/examples/imu_bundle_adjustment.py; for frame-by-frame inertial PnP
with RANSAC, see python/examples/tartan_vio.py; for the full custom-type
contract (needed by RANSAC), see Custom Factors and States (Python and C++).
Sparse Bundle Adjustment — uses ReprojectionFactorBatch to jointly optimize camera poses and 3D landmarks from multi-view observations.
Pose Graph Optimization — uses SE3BetweenFactorBatch to recover a chain of SE(3) poses from consecutive relative-transform measurements.
Custom Warp Factor — shows how to implement a user-defined factor kernel using NVIDIA Warp and WarpFactorBatch.
Custom Warp State — shows how to implement a custom manifold retraction using NVIDIA Warp and WarpStateBatch.
Full source code for all examples lives in the python/examples/
directory.
Note
Using cuNLS from C++. The C++ versions of these examples (and a custom factor written as a CUDA kernel) are in C++ Tutorial.
Sparse Bundle Adjustment#
Source: python/examples/sparse_bundle_adjustment.py
SBA problem statement#
This is a Python port of the C++ bundle adjustment example (see Sparse Bundle Adjustment for the full mathematical formulation). Given \(M\) cameras with poses \(T_1, \ldots, T_M \in \mathrm{SE}(3)\) (world_from_camera) and \(N\) 3D landmarks \(\mathbf{p}_1, \ldots, \mathbf{p}_N \in \mathbb{R}^3\), we minimize the sum of squared reprojection errors across all \(K\) observations:
Poses \(T_0\) and \(T_1\) are held constant as gauge anchors (frame and scale).
SBA API used#
Class |
Role |
|---|---|
Stores camera poses (world_from_camera) on the SE(3) manifold. The
first two poses are marked constant via |
|
Stores 3D landmark coordinates in \(\mathbb{R}^3\). All points are optimization variables. |
|
Computes normalized reprojection residuals and Jacobians for each (pose, point) pair. |
|
Assembles the factor graph. |
|
Iteratively solves the nonlinear least-squares problem with adaptive damping. |
SBA code walkthrough#
Step 1 — Generate synthetic data.
bundle_adjustment_scene returns ground-truth SE(3) poses and 3D points,
a perturbed initial guess (pose 0 stays exact) and the normalized observation of
every point in every camera, as NumPy arrays.
The data comes from python/examples/example_utils/datasets.py; the
generators are ordinary NumPy code and are not part of the lesson.
# 1. Synthetic data: ground truth, perturbed initial guess, observations.
scene = datasets.bundle_adjustment_scene(num_poses=6, num_points=800)
num_poses, num_points = len(scene.gt_poses), len(scene.gt_points)
Step 2 — Upload to the GPU.
CuPy arrays hold the device data. Poses are row-major 4x4 matrices
(16 floats each). The solver updates poses_gpu and points_gpu in place.
# 2. Upload to the GPU (row-major 4x4 poses, xyz points, xy observations).
poses_gpu = cp.asarray(scene.initial_poses.reshape(-1))
points_gpu = cp.asarray(scene.initial_points.reshape(-1))
observations_gpu = cp.asarray(scene.observations.reshape(-1))
# Gauge anchors: pose 0 fixes the frame, pose 1 the scale (unobservable from
# reprojections alone).
const_ids_gpu = cp.array([0, 1], dtype=cp.int32)
Step 3 — Build the state batches. SE3StateBatch with poses 0 and 1 constant (gauge anchors) and VectorStateBatch3 for the points.
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. set_num_active_states /
set_num_active_factors 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: 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: set_num_active_states / set_num_active_factors 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.
poses_capacity = num_poses # every slot solved: active = capacity
points_capacity = num_points
const_poses_capacity = 2 # entries of const_ids_gpu
pose_states = pycunls.SE3StateBatch(poses_gpu, poses_capacity, const_ids_gpu,
const_poses_capacity)
point_states = pycunls.VectorStateBatch3(points_gpu, points_capacity)
num_const_poses = 2 # active constant ids: the gauge anchors
pose_states.set_num_active_states(num_poses, num_const_poses) # active counts
point_states.set_num_active_states(num_points)
Step 4 — Build the reprojection factor batch and its state pointers.
Each factor reads [pose, point]: the state-pointer list is flattened in
factor order, two device pointers per factor. The factor batch also starts
with 0 active factors; set_num_active_factors activates them.
# 4. One reprojection factor per (pose, point); each reads [pose, point].
# Capacity (fixed, sizes the buffers) vs. active count (set per solve): see step 3.
num_observations = num_poses * num_points
observations_capacity = num_observations # every slot solved
reprojection = pycunls.ReprojectionFactorBatch(observations_gpu, observations_capacity, 1e-3)
reprojection.set_num_active_factors(num_observations) # active count
state_pointers = []
for pi in range(num_poses):
for qi in range(num_points):
state_pointers.append(pose_states.state_device_ptr(pi))
state_pointers.append(point_states.state_device_ptr(qi))
Step 5 — Assemble the problem. Register the state batches and the factor batch.
# 5. Problem.
problem = pycunls.Problem()
problem.add_state_batch(pose_states)
problem.add_state_batch(point_states)
problem.add_factor_batch(reprojection, state_pointers)
assert problem.check_consistency(), "Problem consistency check failed"
Step 6 — Solve with Levenberg-Marquardt.
minimize writes the solution into the state batches’ memory.
# 6. Solve with Levenberg-Marquardt.
options = pycunls.LevenbergMarquardtMinimizerOptions()
options.base_options.max_num_iterations = 80
options.base_options.state_tolerance = 1e-8
options.base_options.cost_tolerance = 1e-8
options.initial_lambda = 1e-3
stream = pycunls.CudaStream()
summary = pycunls.LevenbergMarquardtMinimizer(options).minimize(stream, problem)
cp.cuda.runtime.streamSynchronize(stream.get_stream())
Step 7 — Read back and validate. Copy the points back, compare with the ground truth, print and check.
# 7. Results and checks.
optimized_points = cp.asnumpy(points_gpu).reshape(-1, 3)
mse_before = metrics.mse(scene.initial_points, scene.gt_points)
mse_after = metrics.mse(optimized_points, scene.gt_points)
report.print_summary("Sparse Bundle Adjustment (pycunls)", summary,
Point_MSE=f"{mse_before:.6f} -> {mse_after:.6f}")
report.check(mse_after < 0.1 * mse_before, "point error did not decrease")
Pose Graph Optimization#
Source: python/examples/pose_graph_optimization.py
PGO problem statement#
This is a Python port of the C++ pose graph example (see Pose Graph Optimization for the full mathematical formulation). A chain of \(N\) SE(3) poses is connected by \(N{-}1\) between constraints. The residual for constraint \(i\) is:
Pose \(T_0\) is held constant as a gauge anchor.
PGO API used#
Class |
Role |
|---|---|
A single instance stores the full pose chain. The first pose is marked constant; the rest are optimized. |
|
Computes the relative-transform residual and its Jacobians w.r.t. both poses. |
|
Assembles the factor graph. |
|
Solves the nonlinear system. |
PGO code walkthrough#
Step 1 — Generate the pose chain and its measurements.
pose_chain returns a ground-truth chain, the relative transform between
each consecutive pair, and a perturbed initial guess.
The data comes from python/examples/example_utils/datasets.py; the
generators are ordinary NumPy code and are not part of the lesson.
# 1. Synthetic data: ground-truth chain, its relative measurements, a
# perturbed initial guess.
chain = datasets.pose_chain(num_poses=201)
num_poses = len(chain.gt_poses)
num_constraints = num_poses - 1
Step 2 — Upload to the GPU. Poses and measurements as row-major 4x4 matrices; pose 0 is the gauge anchor.
# 2. Upload to the GPU (row-major 4x4 matrices).
poses_gpu = cp.asarray(chain.initial_poses.reshape(-1))
deltas_gpu = cp.asarray(chain.deltas.reshape(-1))
const_ids_gpu = cp.array([0], dtype=cp.int32) # pose 0 is constant
Step 3 — Build the state batch and the between factors.
One SE3StateBatch; factor \(i\) of the
SE3BetweenFactorBatch reads [T_i, T_{i+1}].
Both are constructed with their capacity and activated with
set_num_active_states / set_num_active_factors.
# 3. One SE(3) state batch; one between factor per consecutive pair.
# 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: set_num_active_states / set_num_active_factors 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.
poses_capacity = num_poses # every slot solved: active = capacity
const_poses_capacity = 1 # entries of const_ids_gpu
constraints_capacity = num_constraints
pose_states = pycunls.SE3StateBatch(poses_gpu, poses_capacity, const_ids_gpu,
const_poses_capacity)
between = pycunls.SE3BetweenFactorBatch(deltas_gpu, constraints_capacity)
num_const_poses = 1 # active constant ids: the gauge anchor
pose_states.set_num_active_states(num_poses, num_const_poses) # active counts
between.set_num_active_factors(num_constraints)
state_pointers = []
for i in range(num_constraints):
state_pointers.append(pose_states.state_device_ptr(i))
state_pointers.append(pose_states.state_device_ptr(i + 1))
Step 4 — Assemble the problem. Register the state batch and the factor batch.
# 4. Problem.
problem = pycunls.Problem()
problem.add_state_batch(pose_states)
problem.add_factor_batch(between, state_pointers)
assert problem.check_consistency(), "Problem consistency check failed"
Step 5 — Solve with Levenberg-Marquardt. Same solver as in the bundle adjustment example.
# 5. Solve with Levenberg-Marquardt.
options = pycunls.LevenbergMarquardtMinimizerOptions()
options.base_options.max_num_iterations = 60
options.base_options.state_tolerance = 1e-8
options.base_options.cost_tolerance = 1e-8
options.initial_lambda = 1e-3
stream = pycunls.CudaStream()
summary = pycunls.LevenbergMarquardtMinimizer(options).minimize(stream, problem)
cp.cuda.runtime.streamSynchronize(stream.get_stream())
Step 6 — Report and check. Print the summary and check that the cost dropped.
# 6. Report and check.
report.print_summary("Pose Graph Optimization (pycunls)", summary,
Num_poses=num_poses, Num_factors=num_constraints)
report.check(summary.final_cost < 1e-3 * summary.initial_cost, "cost did not decrease")
Custom Warp Factor#
Source: python/examples/custom_warp_factor.py
Note
For the full evaluate / plus contract (items, factor_ids_ptr,
num_factor_ids, num_replicas), plain-CuPy versions of a custom
factor and state, and how to use them with the RANSAC minimizers, see
Custom Factors and States (Python and C++).
Custom Warp factor problem statement#
This is a Python port of the C++ custom factor example (see
Custom Factor). It builds a chain of scalar states
connected by difference constraints implemented as an NVIDIA Warp kernel
through WarpFactorBatch (from
pycunls.warp):
A prior factor anchors \(x_0\) to its observed value:
Warp factor API used#
Class |
Role |
|---|---|
Stores all \(N\) scalar states in \(\mathbb{R}^1\). |
|
Base class for user-defined factors evaluated via Warp kernels.
Provides |
|
Built-in prior factor that anchors \(x_0\). |
|
Assembles the factor graph. |
|
Solves the nonlinear system. |
Warp factor code walkthrough#
The solve lives in run_chain_example, which main calls twice: Part 1
with ScalarDiffFactor, Part 2 with the residual-only factor and numeric
Jacobians.
Step 1 — Define the Warp kernel.
One thread per item: one factor evaluated at one set of states. Item t
reads the measurement of its factor ids[t] and its own two state values,
and writes row t. The regular minimizers evaluate each factor once
(ids[t] == t); the RANSAC minimizers evaluate many items per factor (see
Custom Factors and States (Python and C++)).
@wp.kernel
def scalar_diff_kernel(measurements: wp.array(dtype=wp.float32),
ids: wp.array(dtype=wp.int32),
left: wp.array(dtype=wp.float32),
right: wp.array(dtype=wp.float32),
residuals: wp.array(dtype=wp.float32),
jacobians: wp.array(dtype=wp.float32),
write_jacobians: int):
t = wp.tid() # item
residuals[t] = (right[t] - left[t]) - measurements[ids[t]] # measurement of its factor
if write_jacobians != 0:
jacobians[2 * t] = -1.0 # d r / d x_i
jacobians[2 * t + 1] = 1.0 # d r / d x_{i+1}
Step 2 — Subclass WarpFactorBatch.
evaluate receives raw device pointers. factor_ids gives the factor
of every item (t % num_active_factors when cuNLS passes none),
gather_state_pairs copies each item’s two state values into contiguous
arrays (Warp cannot dereference the pointer table), and the kernel runs on
cuNLS’s stream. When jacobians_ptr is 0 only residuals are wanted. The
constructor passes the capacity (measurements the buffer holds) to the base
class; num_active_factors is the active count, 0 until
set_num_active_factors.
class ScalarDiffFactor(WarpFactorBatch):
"""residual = (x_right - x_left) - m; one residual, two scalar states."""
def __init__(self, measurements, capacity):
# capacity: measurements the buffer holds (fixed). self.num_active_factors is the
# active count: 0 until set_num_active_factors().
super().__init__(residual_size=1, state_sizes=[1, 1], capacity=capacity)
self.measurements = measurements
def evaluate(self, residuals_ptr, jacobians_ptr, state_pointers_ptr, stream_handle,
factor_ids_ptr, num_factor_ids):
n = num_factor_ids # number of items
ids = self.factor_ids(factor_ids_ptr, n) # factor of each item
# Item t's states are state_pointers[2t] (x_i) and [2t + 1] (x_{i+1}).
left, right = gather_state_pairs(state_pointers_ptr, n, stream_handle)
write_jacobians = 1 if jacobians_ptr != 0 else 0 # 0 = residuals only
jacobians = (self.wrap_array(jacobians_ptr, wp.float32, 2 * n) if write_jacobians
else wp.zeros(1, dtype=wp.float32, device=self._device))
wp.launch(scalar_diff_kernel, dim=n,
inputs=[self.measurements, ids, wp.from_dlpack(left), wp.from_dlpack(right),
self.wrap_array(residuals_ptr, wp.float32, n), jacobians,
write_jacobians],
stream=self.make_warp_stream(stream_handle))
return True
Step 3 — The gather helper.
gather_state_pairs lives in example_utils/gpu.py and is reusable
for any factor with two scalar states:
def gather_state_pairs(state_ptrs_ptr, num_items, stream_handle):
"""For factors reading two scalar states: returns (left, right) CuPy arrays
with left[t] = *state_pointers[2t] and right[t] = *state_pointers[2t + 1]."""
values = gather_state_values(state_ptrs_ptr, 2 * num_items, stream_handle)
with cupy_stream(stream_handle): # keep the copies ordered after the gather
return values[0::2].copy(), values[1::2].copy()
Step 4 — Generate synthetic data.
scalar_chain returns a monotonic chain, exact consecutive differences and a
noisy initial guess.
The data comes from python/examples/example_utils/datasets.py; the
generators are ordinary NumPy code and are not part of the lesson.
# 1. Synthetic data: monotonic chain, exact differences, noisy initial guess.
chain = datasets.scalar_chain(num_states=256)
num_states = len(chain.gt)
Step 5 — Upload to the GPU. States and the prior target go to CuPy arrays; the measurements, read inside the Warp kernel, to a Warp array.
# 2. Upload: states and the anchor value with CuPy, measurements with Warp.
states_gpu = cp.asarray(chain.initial)
prior_gpu = cp.asarray(chain.gt[:1])
measurements_wp = wp.array(chain.measurements, dtype=wp.float32, device="cuda:0")
Step 6 — Build states, factors and state pointers.
Difference factors read [x_i, x_{i+1}]; the built-in prior anchors
x_0. Every batch starts with 0 active entries and is activated with
set_num_active_states / set_num_active_factors.
# 3. Scalar states; one difference factor per pair (x_i, x_{i+1}); a
# built-in prior anchoring x_0.
# 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: set_num_active_states / set_num_active_factors 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.
states_capacity = num_states # every slot solved: active = capacity
states = pycunls.VectorStateBatch1(states_gpu, states_capacity)
states.set_num_active_states(num_states) # active count
diff_pointers = []
for i in range(num_states - 1):
diff_pointers.append(states.state_device_ptr(i))
diff_pointers.append(states.state_device_ptr(i + 1))
num_diff_factors = num_states - 1
diff_capacity = num_diff_factors # used by the difference factor in step 4
prior_capacity = 1
prior = pycunls.PriorVectorFactorBatch1(prior_gpu, prior_capacity)
num_prior_factors = 1
prior.set_num_active_factors(num_prior_factors)
Step 7 — Assemble the problem.
Part 2 registers the residual-only variant (same file,
ScalarDiffResidualOnlyFactor) with JacobianMode.numeric; the prior
keeps its analytic Jacobian (see Numeric (finite-difference) Jacobians).
# 4. Problem. Part 2 overrides the Jacobian mode of the difference factors
# only; the prior keeps its analytic Jacobian.
problem = pycunls.Problem()
problem.add_state_batch(states)
if use_numeric_jacobian:
diff = ScalarDiffResidualOnlyFactor(measurements_wp, diff_capacity)
diff.set_num_active_factors(num_diff_factors) # active count
problem.add_factor_batch(diff, diff_pointers,
jacobian_mode_override=pycunls.JacobianMode.numeric)
else:
diff = ScalarDiffFactor(measurements_wp, diff_capacity)
diff.set_num_active_factors(num_diff_factors) # active count
problem.add_factor_batch(diff, diff_pointers)
problem.add_factor_batch(prior, [states.state_device_ptr(0)])
assert problem.check_consistency(), "Problem consistency check failed"
Step 8 — Solve with Levenberg-Marquardt. Same solver as in the previous examples.
# 5. Solve with Levenberg-Marquardt.
options = pycunls.LevenbergMarquardtMinimizerOptions()
options.base_options.max_num_iterations = 50
options.base_options.state_tolerance = 1e-8
options.base_options.cost_tolerance = 1e-8
options.initial_lambda = 1e-3
stream = pycunls.CudaStream()
summary = pycunls.LevenbergMarquardtMinimizer(options).minimize(stream, problem)
cp.cuda.runtime.streamSynchronize(stream.get_stream())
Step 9 — Report and check. Compare with the ground truth and check.
# 6. Report and check.
mse_before = metrics.mse(chain.initial, chain.gt)
mse_after = metrics.mse(cp.asnumpy(states_gpu), chain.gt)
report.print_summary(title, summary, State_MSE=f"{mse_before:.6f} -> {mse_after:.6f}")
report.check(mse_after < 0.01 * mse_before, "state error did not decrease")
Custom Warp State#
Source: python/examples/custom_warp_state.py
Custom Warp state problem statement#
This example demonstrates WarpStateBatch (from pycunls.warp) by
defining a positive-scalar manifold where the Plus (retraction)
operation is multiplicative:
This makes the tangent space the reals (\(\delta \in \mathbb{R}\)), while states stay strictly positive — the natural parameterization for quantities like scales, variances, or rates.
A chain of positive scalars is connected by log-ratio between-factors:
All Jacobians equal \(\pm 1\) because the problem is linear in the tangent (log) space.
Warp state API used#
Class |
Role |
|---|---|
Base class for user-defined state batches with a Warp-based Plus.
Provides |
|
Base class for the custom log-prior and log-ratio factors. |
|
Assembles the factor graph. |
|
Solves the nonlinear system. |
Warp state code walkthrough#
Step 1 — Define the Plus kernel. The retraction \(x \oplus \delta = x\,e^{\delta}\) keeps every state positive.
@wp.kernel
def positive_plus_kernel(x: wp.array(dtype=wp.float32), delta: wp.array(dtype=wp.float32),
x_plus_delta: wp.array(dtype=wp.float32)):
i = wp.tid()
x_plus_delta[i] = x[i] * wp.exp(delta[i])
Step 2 — Subclass WarpStateBatch.
plus receives num_replicas contiguous copies of the batch (1 for the
regular minimizers, one per hypothesis for RANSAC). Every state is independent,
so all copies are one flat launch over num_replicas * num_active_states
states (num_active_states is the active count, at most the capacity).
class PositiveScalarStateBatch(WarpStateBatch):
"""Ambient size 1 (the positive value), tangent size 1 (delta in R)."""
def __init__(self, data, capacity, **kwargs):
super().__init__(data, ambient_size=1, tangent_size=1, capacity=capacity, **kwargs)
def plus(self, x_ptr, delta_ptr, x_plus_delta_ptr, stream_handle, num_replicas):
# The arrays hold num_replicas contiguous copies of the active states
# (num_active_states, at most the capacity); every state is independent,
# so all copies are one flat launch.
n = self.num_active_states * num_replicas
wp.launch(positive_plus_kernel, dim=n,
inputs=[self.wrap_array(x_ptr, wp.float32, n),
self.wrap_array(delta_ptr, wp.float32, n),
self.wrap_array(x_plus_delta_ptr, wp.float32, n)],
stream=self.make_warp_stream(stream_handle))
Step 3 — Define the factors.
Two custom factors in log space: a prior on x_0 and a log-ratio between
consecutive states. Both follow the same item pattern as in the custom factor
example: measurement by ids[t], states and outputs by t.
@wp.kernel
def log_ratio_kernel(measurements: wp.array(dtype=wp.float32), ids: wp.array(dtype=wp.int32),
left: wp.array(dtype=wp.float32), right: wp.array(dtype=wp.float32),
residuals: wp.array(dtype=wp.float32), jacobians: wp.array(dtype=wp.float32),
write_jacobians: int):
t = wp.tid()
residuals[t] = wp.log(right[t]) - wp.log(left[t]) - measurements[ids[t]]
if write_jacobians != 0:
jacobians[2 * t] = -1.0
jacobians[2 * t + 1] = 1.0
class LogRatioBetweenFactor(WarpFactorBatch):
"""residual = log(x_right / x_left) - m; two states."""
def __init__(self, measurements, capacity):
super().__init__(residual_size=1, state_sizes=[1, 1], capacity=capacity)
self.measurements = measurements
def evaluate(self, res_ptr, jac_ptr, sp_ptr, stream_handle, factor_ids_ptr, num_factor_ids):
n = num_factor_ids
ids = self.factor_ids(factor_ids_ptr, n)
left, right = gather_state_pairs(sp_ptr, n, stream_handle)
write_jacobians = 1 if jac_ptr != 0 else 0
jacobians = (self.wrap_array(jac_ptr, wp.float32, 2 * n) if write_jacobians
else wp.zeros(1, dtype=wp.float32, device=self._device))
wp.launch(log_ratio_kernel, dim=n,
inputs=[self.measurements, ids, wp.from_dlpack(left), wp.from_dlpack(right),
self.wrap_array(res_ptr, wp.float32, n), jacobians, write_jacobians],
stream=self.make_warp_stream(stream_handle))
return True
Step 4 — Generate synthetic data.
positive_chain returns a growing positive chain, its log-ratio
measurements and a noisy initial guess.
The data comes from python/examples/example_utils/datasets.py; the
generators are ordinary NumPy code and are not part of the lesson.
# 1. Synthetic data: growing positive chain, log-ratio measurements, noisy guess.
chain = datasets.positive_chain(num_states=128)
num_states = len(chain.gt)
Step 5 — Upload to the GPU. States go to a CuPy array; factor data to Warp arrays.
# 2. Upload: states with CuPy, factor data with Warp.
states_gpu = cp.asarray(chain.initial)
log_ratios_wp = wp.array(chain.measurements, dtype=wp.float32, device="cuda:0")
prior_wp = wp.array(chain.gt[:1], dtype=wp.float32, device="cuda:0")
Step 6 — Build the custom state and factor batches.
The custom state batch wraps the CuPy array; the factors read
[x_i, x_{i+1}] and x_0. Custom batches follow the same capacity rule
as the built-in ones: 0 active entries until set_num_* is called.
# 3. The custom state batch and the two custom factor batches.
# 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: set_num_active_states / set_num_active_factors 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.
states_capacity = num_states # every slot solved: active = capacity
between_capacity = num_states - 1
prior_capacity = 1
states = PositiveScalarStateBatch(states_gpu, states_capacity)
between = LogRatioBetweenFactor(log_ratios_wp, between_capacity)
prior = LogPriorFactor(prior_wp, prior_capacity)
num_between_factors = between_capacity
num_prior_factors = 1
states.set_num_active_states(num_states) # active counts
between.set_num_active_factors(num_between_factors)
prior.set_num_active_factors(num_prior_factors)
between_pointers = []
for i in range(num_states - 1):
between_pointers.append(states.state_device_ptr(i))
between_pointers.append(states.state_device_ptr(i + 1))
Step 7 — Assemble the problem. Register the state batch and both factor batches.
# 4. Problem.
problem = pycunls.Problem()
problem.add_state_batch(states)
problem.add_factor_batch(between, between_pointers)
problem.add_factor_batch(prior, [states.state_device_ptr(0)])
assert problem.check_consistency(), "Problem consistency check failed"
Step 8 — Solve with Levenberg-Marquardt.
Same solver as in the previous examples; every step goes through the custom
plus.
# 5. Solve with Levenberg-Marquardt.
options = pycunls.LevenbergMarquardtMinimizerOptions()
options.base_options.max_num_iterations = 30
options.base_options.state_tolerance = 1e-8
options.base_options.cost_tolerance = 1e-8
options.initial_lambda = 1e-3
stream = pycunls.CudaStream()
summary = pycunls.LevenbergMarquardtMinimizer(options).minimize(stream, problem)
cp.cuda.runtime.streamSynchronize(stream.get_stream())
Step 9 — Report and check. Errors are compared in log space; all states must stay positive.
# 6. Report and check (errors measured in log space).
optimized = cp.asnumpy(states_gpu)
mse_before = metrics.mse(np.log(chain.initial), np.log(chain.gt))
mse_after = metrics.mse(np.log(optimized), np.log(chain.gt))
report.print_summary(
"Custom Warp State Batch Example (positive-scalar manifold)", summary,
Num_states=num_states, Log_MSE=f"{mse_before:.6f} -> {mse_after:.6f}",
Range_gt=f"[{chain.gt.min():.2f}, {chain.gt.max():.2f}]",
Range_opt=f"[{optimized.min():.2f}, {optimized.max():.2f}]")
report.check(mse_after < 0.01 * mse_before and optimized.min() > 0,
"log error did not decrease or a state left the manifold")