State API#

The state module provides batched storage and manifold updates for optimization variables. State batches implement the Plus (retraction) operation so the solver can update states in tangent space while keeping them on the manifold. This page introduces manifolds, then documents the Python state batch classes, then the C++ API.

Python — pycunls
C++ — cunls/state

Pose convention. Every factor that reads a pose state (SE(2), SE(3), SO(2), SO(3)) takes it as world_from_rig \(T\): the pose of the rig (body, camera, robot) in the world, mapping rig coordinates to world coordinates. The reprojection and PnP factors see a world point at \(T^{-1} P\) (then camera_from_rig); the IMU, dynamics, clearance and motion-prior factors use \(T\) directly. If your data are world-to-camera (camera_from_world) poses, store their inverses.

States are updated on the right, \(T \leftarrow T\,\mathrm{Exp}(\delta)\), so a solver step \(\delta\) is expressed in the rig frame. Its scaling does not depend on where the world origin is: a scene kilometers away from the origin conditions and converges like one at the origin (with a rig-frame increment on camera_from_world poses, a rotation step would turn the world about its origin and move distant cameras by meters).

Manifolds#

What is a manifold?

Many variables in nonlinear least squares do not live in \(\mathbb{R}^n\) but on curved spaces: 2D/3D rotations (SO(2), SO(3)), rigid or similarity transforms (SE(2), SE(3), Sim(2), Sim(3)), projective linear groups (SL(4)), or other constrained sets. Such a space is a manifold: at each point \(x\) there is a tangent space (a linear space of “directions”) whose dimension is the intrinsic dimension of the manifold. The ambient space is the larger Euclidean space in which the manifold is embedded (e.g. 3×3 matrices for SO(3), so ambient dimension 9).

Why use manifolds?

Constraint satisfaction: Updates are applied in the tangent space and then mapped back onto the manifold, so the state never leaves the constraint set (e.g. rotation matrices stay orthogonal).
Correct dimension: The solver only works with as many unknowns as the tangent dimension (e.g. 3 for SO(3) instead of 9), which improves numerics and efficiency.

Plus (retraction)

The Plus operation (in the literature often written \(\boxplus\)) takes a point \(x\) on the manifold and a tangent vector \(\Delta\) and returns a new point on the manifold:

\[x \oplus \Delta = \mathrm{Plus}(x,\, \Delta)\]

So the solver computes an update \(\Delta\) in tangent space (e.g. from Gauss-Newton or Levenberg-Marquardt) and then sets \(x_{\mathrm{new}} = x \oplus \Delta\). For Euclidean space, \(x \oplus \Delta = x + \Delta\). For Lie groups (SO, SE, Sim), Plus is implemented as right-multiplication by the exponential of the Lie algebra element: \(x \oplus \Delta = x \cdot \mathrm{Exp}(\Delta)\).

How the minimizer uses state batches

The minimizer holds a current state \(x\) in ambient storage. It solves for a tangent update \(\Delta\) (using Jacobians that are w.r.t. tangent space). Then it calls StateBatch::Plus() (or StateBatchOps::Plus over multiple batches) to write \(x \oplus \Delta\) back into the state buffer. So the state batch is the object that knows how to apply \(\oplus\) for its manifold.

Python API (`pycunls`)#

All Python state batches inherit from the abstract StateBatch base class. Every constructor argument documented as DevicePointer accepts either a cupy.ndarray (the device pointer is extracted automatically via .data.ptr) or a raw int GPU device address.

The Plus formula, ambient and tangent dimensions, and memory layout of each built-in manifold are tabulated in State batch types (tables) (C++ API section); the Python classes use the same layouts.

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) (C++: SetNumActiveFactors / SetNumActiveStates) before solving, and again whenever the problem size changes. See Capacity and active count.

Common `StateBatch` interface#

Every state batch — built-in or user-defined — exposes the following methods and properties.

Methods

state_device_ptr(index: int) -> int — returns the GPU device pointer (as an int) for state index. The returned value is the address of the first float in the state’s ambient storage. Use these pointers to build the state_pointers list passed to Problem.add_factor_batch. index is zero-based and may be any slot below capacity (so connectivity can be built before the active count is set); passing a value >= capacity returns 0 (null pointer).
set_num_active_states(num_active_states, num_const_states=0) — sets the active state count (the first num_active_states states of the buffer) and the active constant-id count. Every batch starts with 0 active states: call it before the first solve, and again whenever the sizes change. Host-only; takes effect at the next minimize. Raises ValueError above the capacity.

Read-only properties

num_active_states (int) — number of active states, including any active constant states (0 until set_num_active_states).
capacity (int) — number of states the buffer holds (the constructor’s capacity); constant.
const_capacity (int) — number of entries the constant-id buffer holds (the constructor’s const_capacity, 0 without one).
num_const_states (int) — active constant-id count: the first num_const_states entries of the constant-id buffer are held constant (0 until set_num_active_states).
tangent_size (int) — tangent-space dimension per state. This is the number of unknowns the solver allocates per state (e.g. 6 for SE(3), 3 for SO(3)).
ambient_size (int) — ambient/storage dimension per state. The GPU buffer stores capacity * ambient_size contiguous floats (e.g. 16 for SE(3) = row-major 4×4 matrix).

`pycunls.VectorStateBatch1` / `VectorStateBatch2` / `VectorStateBatch3` / `VectorStateBatch6`#

Euclidean vector states where tangent and ambient dimensions coincide. The suffix indicates the dimension (1, 2, 3, or 6). Plus is simple addition: \(x \oplus \delta = x + \delta\).

Constructors

# All optimizable:
sb = pycunls.VectorStateBatch3(data, capacity)

# With constant (frozen) states:
sb = pycunls.VectorStateBatch3(data, capacity, const_state_ids, const_capacity)

data (DevicePointer) — contiguous GPU buffer of capacity × Dim floats. The state batch does not copy the data; it stores the pointer and reads/writes the buffer directly. The caller must keep the underlying allocation alive for the lifetime of the state batch.
capacity (int) — number of states the buffer holds. The batch starts with 0 active states: call set_num_active_states before solving.
const_state_ids (DevicePointer, optional) — GPU int32 array containing the zero-based indices of states that should be held constant during optimization. Constant states are excluded from the solver’s tangent vector; their ambient values are never modified.
const_capacity (int, optional) — number of entries the const_state_ids buffer holds (the active count is set with set_num_active_states).

`pycunls.SE3StateBatch`#

3-D rigid-body transform state batch. Ambient = 16 (row-major 4×4 homogeneous matrix), Tangent = 6 (twist \([\omega; \rho]\)). Plus is right-multiplication by the exponential map: \(T \oplus \delta = T \cdot \mathrm{Exp}(\delta)\).

Constructors

# All optimizable:
sb = pycunls.SE3StateBatch(data, capacity)

# With constant states:
sb = pycunls.SE3StateBatch(data, capacity, const_state_ids, const_capacity)

data (DevicePointer) — contiguous GPU buffer of capacity × 16 floats (row-major 4×4 matrices).
capacity (int) — number of states (poses) the buffer holds; 0 are active until set_num_active_states.
const_state_ids (DevicePointer, optional) — GPU int32 array of constant-state indices (e.g. a gauge anchor).
const_capacity (int, optional) — number of entries the const_state_ids buffer holds.

`pycunls.SO3StateBatch`#

3-D rotation state batch. Ambient = 9 (row-major 3×3 rotation matrix), Tangent = 3 (rotation vector / axis-angle). Plus: \(R \oplus \delta = R \cdot \mathrm{Exp}(\mathrm{skew}(\delta))\).