symforce.cc_sym module¶

This module wraps many of the C++ optimization classes.

class Factor¶

Bases: pybind11_object

A residual term for optimization.

Created from a function and a set of Keys that act as inputs. Given a Values as an evaluation point, generates a linear approximation to the residual function.

all_keys(self: cc_sym.Factor) → List[cc_sym.Key]¶: Get all keys required to evaluate this factor.

is_sparse(self: cc_sym.Factor) → bool¶: Does this factor use a sparse jacobian/hessian matrix?

static jacobian(*args, **kwargs)¶

Overloaded function.

jacobian(jacobian_func: Callable[[cc_sym.Values, List[lcmtypes.sym._index_entry_t.index_entry_t]], tuple], keys: List[cc_sym.Key], sparse: bool = False) -> cc_sym.Factor
Create from a function that computes the jacobian. The hessian will be computed using the Gauss Newton approximation:
H = J.T * J rhs = J.T * b
Args:
keys: The set of input arguments, in order, accepted by func. sparse: Create a sparse factor if True, dense factor if false. Defaults to dense.

Precondition:
The jacobian returned by jacobian_func has type scipy.sparse.csc_matrix if and only if sparse = True.
jacobian(jacobian_func: Callable[[cc_sym.Values, List[lcmtypes.sym._index_entry_t.index_entry_t]], tuple], keys_to_func: List[cc_sym.Key], keys_to_optimize: List[cc_sym.Key], sparse: bool = False) -> cc_sym.Factor
Create from a function that computes the jacobian. The hessian will be computed using the Gauss Newton approximation:
H = J.T * J rhs = J.T * b
Args:
keys_to_func: The set of input arguments, in order, accepted by func. keys_to_optimize: The set of input arguments that correspond to the derivative in func. Must be a subset of keys_to_func. sparse: Create a sparse factor if True, dense factor if false. Defaults to dense.

Precondition:
The jacobian returned by jacobian_func has type scipy.sparse.csc_matrix if and only if sparse = True.

linearize(self: cc_sym.Factor, arg0: cc_sym.Values) → tuple¶: Evaluate the factor at the given linearization point and output just the numerical values of the residual and jacobian.

linearized_factor(self: cc_sym.Factor, values: cc_sym.Values) → lcmtypes.sym._linearized_dense_factor_t.linearized_dense_factor_t¶

Evaluate the factor at the given linearization point and output a LinearizedDenseFactor that contains the numerical values of the residual, jacobian, hessian, and right-hand-side.

This can only be called if is_sparse is false; otherwise, it will throw.

optimized_keys(self: cc_sym.Factor) → List[cc_sym.Key]¶: Get the optimized keys for this factor.

class ImuPreintegrator¶

Bases: pybind11_object

Class to on-manifold preintegrate IMU measurements for usage in a SymForce optimization problem.

covariance(self: cc_sym.ImuPreintegrator) → numpy.ndarray[numpy.float64[9, 9]]¶

integrate_measurement(self: cc_sym.ImuPreintegrator, measured_accel: numpy.ndarray[numpy.float64[3, 1]], measured_gyro: numpy.ndarray[numpy.float64[3, 1]], accel_cov: numpy.ndarray[numpy.float64[3, 1]], gyro_cov: numpy.ndarray[numpy.float64[3, 1]], dt: float, epsilon: float = 2.220446049250313e-15) → None¶

preintegrated_measurements(self: cc_sym.ImuPreintegrator) → cc_sym.PreintegratedImuMeasurements¶

class Key¶

Bases: pybind11_object

Key type for Values. Contains a letter plus an integral subscript and superscript. Can construct with a letter, a letter + sub, or a letter + sub + super, but not a letter + super.

INVALID_LETTER = '\x00'¶

INVALID_SUB = -9223372036854775808¶

INVALID_SUPER = -9223372036854775808¶

get_lcm_type(self: cc_sym.Key) → lcmtypes.sym._key_t.key_t¶

property letter¶: The letter value of the key.

lexical_less_than(self: cc_sym.Key, arg0: cc_sym.Key) → bool¶: Return true if a is LESS than b, in dictionary order of the tuple (letter, sub, super).

property sub¶: The subscript value of the key (INVALID_SUB if not set).

property super¶: The superscript value of the key (INVALID_SUPER if not set).

with_letter(self: cc_sym.Key, letter: str) → cc_sym.Key¶: Creates a new key with a modified letter from an existing one.

with_sub(self: cc_sym.Key, sub: int) → cc_sym.Key¶: Creates a new key with a modified subscript from an existing one.

with_super(self: cc_sym.Key, super: int) → cc_sym.Key¶: Creates a new key with a modified superscript from an existing one.

class Linearization¶

Bases: pybind11_object

Class for storing a problem linearization evaluated at a Values (i.e. a residual, jacobian, hessian, and rhs).

error(self: cc_sym.Linearization) → float¶

property hessian_lower¶

is_initialized(self: cc_sym.Linearization) → bool¶: Returns whether the linearization is currently valid for the corresponding values. Accessing any of the members when this is false could result in unexpected behavior.

property jacobian¶

linear_delta_error(self: cc_sym.Linearization, x_update: numpy.ndarray[numpy.float64[m, 1]], damping_vector: numpy.ndarray[numpy.float64[m, 1]]) → float¶

reset(self: cc_sym.Linearization) → None¶: Set to invalid.

property residual¶

property rhs¶

set_initialized(self: cc_sym.Linearization, initialized: bool = True) → None¶

class OptimizationStats¶

Bases: pybind11_object

Debug stats for a full optimization run.

property best_index¶: Index into iterations of the best iteration (containing the optimal Values).

property best_linearization¶: The linearization at best_index (at optimized_values), filled out if populate_best_linearization=True

property cholesky_factor_sparsity¶: Sparsity pattern of the cholesky factor L (filled out if debug_stats=True)

property failure_reason¶: If status == FAILED, why? This should be cast to the NonlinearSolver::FailureReason enum for the nonlinear solver you used.

get_lcm_type(self: cc_sym.OptimizationStats) → lcmtypes.sym._optimization_stats_t.optimization_stats_t¶

property iterations¶

property jacobian_sparsity¶: Sparsity pattern of the problem jacobian (filled out if debug_stats=True and include_jacobians=True)

property linear_solver_ordering¶: Ordering used by the linear solver (filled out if debug_stats=True)

property status¶: What was the result of the optimization? (did it converge, fail, etc.)

class Optimizer¶

Bases: pybind11_object

Class for optimizing a nonlinear least-squares problem specified as a list of Factors. For efficient use, create once and call Optimize() multiple times with different initial guesses, as long as the factors remain constant and the structure of the Values is identical.

compute_all_covariances(self: cc_sym.Optimizer, linearization: cc_sym.Linearization) → Dict[cc_sym.Key, numpy.ndarray[numpy.float64[m, n]]]¶

Get covariances for each optimized key at the given linearization

May not be called before either optimize or linearize has been called.

compute_covariances(self: cc_sym.Optimizer, linearization: cc_sym.Linearization, keys: List[cc_sym.Key]) → Dict[cc_sym.Key, numpy.ndarray[numpy.float64[m, n]]]¶

Get covariances for the given subset of keys at the given linearization

This version is potentially much more efficient than computing the covariances for all keys in the problem.

Currently requires that keys corresponds to a set of keys at the start of the list of keys for the full problem, and in the same order. It uses the Schur complement trick, so will be most efficient if the hessian is of the following form, with C block diagonal:

A = ( B    E )
    ( E^T  C )

compute_full_covariance(self: cc_sym.Optimizer, linearization: cc_sym.Linearization) → numpy.ndarray[numpy.float64[m, n]]¶

Get the full problem covariance at the given linearization

Unlike compute_covariance and compute_all_covariances, this includes the off-diagonal blocks, i.e. the cross-covariances between different keys.

The ordering of entries here is the same as the ordering of the keys in the linearization, which can be accessed via linearization_index().

May not be called before either optimize or linearize has been called.

factors(self: cc_sym.Optimizer) → List[cc_sym.Factor]¶: Get the factors.

keys(self: cc_sym.Optimizer) → List[cc_sym.Key]¶: Get the optimized keys.

linearization_index(self: cc_sym.Optimizer) → dict¶

linearization_index_entry(self: cc_sym.Optimizer, key: cc_sym.Key) → lcmtypes.sym._index_entry_t.index_entry_t¶

linearize(self: cc_sym.Optimizer, values: cc_sym.Values) → cc_sym.Linearization¶: Linearize the problem around the given values.

optimize(*args, **kwargs)¶

Overloaded function.

optimize(self: cc_sym.Optimizer, values: cc_sym.Values, num_iterations: int = -1, populate_best_linearization: bool = False) -> cc_sym.OptimizationStats

Optimize the given values in-place

Args:
num_iterations: If < 0 (the default), uses the number of iterations specified by the params at construction.

populate_best_linearization: If true, the linearization at the best values will be filled out in the stats.

Returns:
The optimization stats
optimize(self: cc_sym.Optimizer, values: cc_sym.Values, num_iterations: int, populate_best_linearization: bool, stats: cc_sym.OptimizationStats) -> None

Optimize the given values in-place

This overload takes the stats as an argument, and stores into there. This allows users to avoid reallocating memory for any of the entries in the stats, for use cases where that’s important. If passed, stats must not be None.

Args:
num_iterations: If < 0 (the default), uses the number of iterations specified by the params at construction

populate_best_linearization: If true, the linearization at the best values will be filled out in the stats

stats: An OptimizationStats to fill out with the result - if filling out dynamically allocated fields here, will not reallocate if memory is already allocated in the required shape (e.g. for repeated calls to Optimize)
optimize(self: cc_sym.Optimizer, values: cc_sym.Values, num_iterations: int, stats: cc_sym.OptimizationStats) -> None

Optimize the given values in-place

This overload takes the stats as an argument, and stores into there. This allows users to avoid reallocating memory for any of the entries in the stats, for use cases where that’s important. If passed, stats must not be None.

Args:
num_iterations: If < 0 (the default), uses the number of iterations specified by the params at construction

stats: An OptimizationStats to fill out with the result - if filling out dynamically allocated fields here, will not reallocate if memory is already allocated in the required shape (e.g. for repeated calls to Optimize)
optimize(self: cc_sym.Optimizer, values: cc_sym.Values, stats: cc_sym.OptimizationStats) -> None

Optimize the given values in-place

This overload takes the stats as an argument, and stores into there. This allows users to avoid reallocating memory for any of the entries in the stats, for use cases where that’s important. If passed, stats must not be None.

Args:
stats: An OptimizationStats to fill out with the result - if filling out dynamically allocated fields here, will not reallocate if memory is already allocated in the required shape (e.g. for repeated calls to Optimize)

update_params(self: cc_sym.Optimizer, params: lcmtypes.sym._optimizer_params_t.optimizer_params_t) → None¶: Update the optimizer params.

class PreintegratedImuMeasurements¶

Bases: pybind11_object

Struct of Preintegrated IMU Measurements (not including the covariance of change in orientation, velocity, and position).

property DR_D_gyro_bias¶

class Delta¶

Bases: pybind11_object

A convenient struct that holds the Preintegrated delta.

property DR¶

property Dp¶

property Dt¶

property Dv¶

static from_lcm(arg0: lcmtypes.sym._imu_integrated_measurement_delta_t.imu_integrated_measurement_delta_t) → cc_sym.PreintegratedImuMeasurements.Delta¶

get_lcm_type(self: cc_sym.PreintegratedImuMeasurements.Delta) → lcmtypes.sym._imu_integrated_measurement_delta_t.imu_integrated_measurement_delta_t¶

roll_forward_state(self: cc_sym.PreintegratedImuMeasurements.Delta, pose_i: sym.Pose3, vel_i: numpy.ndarray[numpy.float64[3, 1]], gravity: numpy.ndarray[numpy.float64[3, 1]]) → Tuple[sym.Pose3, numpy.ndarray[numpy.float64[3, 1]]]¶

property Dp_D_accel_bias¶

property Dp_D_gyro_bias¶

property Dv_D_accel_bias¶

property Dv_D_gyro_bias¶

property accel_bias¶

property delta¶

static from_lcm(arg0: lcmtypes.sym._imu_integrated_measurement_t.imu_integrated_measurement_t) → cc_sym.PreintegratedImuMeasurements¶

get_bias_corrected_delta(self: cc_sym.PreintegratedImuMeasurements, new_accel_bias: numpy.ndarray[numpy.float64[3, 1]], new_gyro_bias: numpy.ndarray[numpy.float64[3, 1]]) → cc_sym.PreintegratedImuMeasurements.Delta¶

get_lcm_type(self: cc_sym.PreintegratedImuMeasurements) → lcmtypes.sym._imu_integrated_measurement_t.imu_integrated_measurement_t¶

property gyro_bias¶

class Values¶

Bases: pybind11_object

Efficient polymorphic data structure to store named types with a dict-like interface and support efficient repeated operations using a key index. Supports on-manifold optimization.

Compatible types are given by the type_t enum. All types implement the StorageOps and LieGroupOps concepts, which are the core operating mechanisms in this class.

at(*args, **kwargs)¶

Overloaded function.

at(self: cc_sym.Values, key: cc_sym.Key) -> object

Retrieve a value by key.

at(self: cc_sym.Values, entry: lcmtypes.sym._index_entry_t.index_entry_t) -> object

Retrieve a value by index entry. This avoids a map lookup compared to at(key).

cleanup(self: cc_sym.Values) → int¶

Repack the data array to get rid of empty space from removed keys. If regularly removing keys, it’s up to the user to call this appropriately to avoid storage growth. Returns the number of Scalar elements cleaned up from the data array.

It will INVALIDATE all indices, offset increments, and pointers. Re-create an index with create_index().

create_index(self: cc_sym.Values, keys: List[cc_sym.Key]) → lcmtypes.sym._index_t.index_t¶

Create an index from the given ordered subset of keys. This object can then be used for repeated efficient operations on that subset of keys.

If you want an index of all the keys, call values.create_index(values.keys()).

An index will be INVALIDATED if the following happens:

remove() is called with a contained key, or remove_all() is called
cleanup() is called to re-pack the data array

data(self: cc_sym.Values) → List[float]¶: Raw data buffer.

empty(self: cc_sym.Values) → bool¶: Has zero keys.

get_lcm_type(self: cc_sym.Values, sort_keys: bool = False) → lcmtypes.sym._values_t.values_t¶: Serialize to LCM.

has(self: cc_sym.Values, key: cc_sym.Key) → bool¶: Return whether the key exists.

items(self: cc_sym.Values) → Dict[cc_sym.Key, lcmtypes.sym._index_entry_t.index_entry_t]¶: Expose map type to allow iteration.

keys(self: cc_sym.Values, sort_by_offset: bool = True) → List[cc_sym.Key]¶

Get all keys.

Parameters:: sort_by_offset – Sorts by storage order to make iteration safer and more memory efficient

local_coordinates(self: cc_sym.Values, others: cc_sym.Values, index: lcmtypes.sym._index_t.index_t, epsilon: float) → numpy.ndarray[numpy.float64[m, 1]]¶

Express this Values in the local coordinate of others Values, i.e., this ominus others

Parameters:

others – The other Values that the local coordinate is relative to
index – Ordered list of keys to include (MUST be valid for both this and others Values)
epsilon – Small constant to avoid singularities (do not use zero)

num_entries(self: cc_sym.Values) → int¶: Number of keys.

remove(self: cc_sym.Values, key: cc_sym.Key) → bool¶

Remove the given key. Only removes the index entry, does not change the data array. Returns true if removed, false if already not present.

Call cleanup() to re-pack the data array.

remove_all(self: cc_sym.Values) → None¶: Remove all keys and empty out the storage.

retract(self: cc_sym.Values, index: lcmtypes.sym._index_t.index_t, delta: List[float], epsilon: float) → None¶

Perform a retraction from an update vector.

Parameters:

index – Ordered list of keys in the delta vector
delta – Update vector - MUST be the size of index.tangent_dim!
epsilon – Small constant to avoid singularities (do not use zero)

set(*args, **kwargs)¶

Overloaded function.

set(self: cc_sym.Values, key: cc_sym.Key, value: float) -> bool

Add or update a value by key. Returns true if added, false if updated.

set(self: cc_sym.Values, key: lcmtypes.sym._index_entry_t.index_entry_t, value: float) -> None