Symforce

Introduction

SymForce is a fast symbolic computation and code generation library for robotics applications like computer vision, state estimation, motion planning, and controls. It combines the development speed and flexibility of symbolic mathematics with the performance of autogenerated, highly optimized code in C++ or any target runtime language. SymForce contains three independently useful systems:

Symbolic Toolkit - builds on the SymPy API to provide rigorous geometric and camera types, lie group calculus, singularity handling, and tools to model complex problems

Code Generator - transforms symbolic expressions into blazing-fast, branchless code with clean APIs and minimal dependencies, with a template system to target any language

Optimization Library - a fast tangent-space optimization library based on factor graphs, with a highly optimized implementation for real-time robotics applications

SymForce automatically computes tangent space Jacobians, eliminating the need for any bug-prone handwritten derivatives. Generated functions can be directly used as factors in our nonlinear optimizer. This workflow enables faster runtime functions, faster development time, and fewer lines of handwritten code versus alternative methods.

SymForce is developed and maintained by Skydio . It is used in production to accelerate tasks like SLAM, bundle adjustment, calibration, and sparse nonlinear MPC for autonomous robots at scale.

Tutorials

In [1]:

Geo Package

Matrix

In [2]: 
(2, 3)
(2, 3)
(3, 1)
(3, 1)
(3, 1)

Rotations

Quaternion is used as the underlying representation.

In [3]: 
<Rot3 <Q xyzw=[0, 0, 0, 1]>>
$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{matrix}\right]$ 
array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])
<Rot3 <Q xyzw=[R_x, R_y, R_z, R_w]>>
$\displaystyle \left[\begin{matrix}- 2 R_{y}^{2} - 2 R_{z}^{2} + 1 & - 2 R_{w} R_{z} + 2 R_{x} R_{y} & 2 R_{w} R_{y} + 2 R_{x} R_{z}\\2 R_{w} R_{z} + 2 R_{x} R_{y} & - 2 R_{x}^{2} - 2 R_{z}^{2} + 1 & - 2 R_{w} R_{x} + 2 R_{y} R_{z}\\- 2 R_{w} R_{y} + 2 R_{x} R_{z} & 2 R_{w} R_{x} + 2 R_{y} R_{z} & - 2 R_{x}^{2} - 2 R_{y}^{2} + 1\end{matrix}\right]$ 
<Q xyzw=[Q_x, Q_y, Q_z, Q_w]>

symforce.symbolic.Pose3 and sym.Pose3

SymForce provides sym packages with runtime code for geometry and camera types, e.g. sym.Pose3 that are generated from its symbolic geo and cam packages , e.g. symforce.symbolic.Pose3 . As such, there are multiple versions of a class like Pose3 and it can be a common source of confusion.

In [4]: 
<Pose3 R=<Rot3 <Q xyzw=[0, 0, 0, 1.0]>>, t=(0, 0, 0)>
In [5]: 
# sym runtime code
import sym
import symforce.symbolic as sf
print(f"sym package rotation matrix type: {type(sym.Pose3.identity().R.to_rotation_matrix())}")
print(f"symbolic package rotation matrix type: {type(sf.Pose3.identity().R.to_rotation_matrix())}")
print(f"symbolic package rotation matrix to_numpy type: { type(sf.Pose3.identity().R.to_rotation_matrix().to_numpy())}")

sym package rotation matrix type: <class 'numpy.ndarray'>
symbolic package rotation matrix type: <class 'symforce.geo.matrix.Matrix33'>
symbolic package rotation matrix to_numpy type: <class 'numpy.ndarray'>
In [6]:
In [ ]:

Values

Values objects are ordered dict-like containers used to store multiple heterogeneous objects, normally for the purpose of function generation.

In [8]: 
Values(
  x: x,
  y: y,
  z: z,
  t:   Values(
    h: h,
    m: m,
    s: s,
    timezone:     Values(
      time_zone: zone,
    ),
  ),
  id: id,
)
[('x', x),
 ('y', y),
 ('z', z),
 ('t.h', h),
 ('t.m', m),
 ('t.s', s),
 ('t.timezone.time_zone', zone),
 ('id', id)]
Before modifying id: id
After modifying id: 1024
In [9]: 
Values(
  x: x,
  y: y,
  z: z,
  t:   Values(
    h: h,
    m: m,
    s: s,
    timezone:     Values(
      time_zone: zone,
    ),
  ),
  id: 1024,
  foo:   Values(
    x: foo.x,
    bar:     Values(
      x: 1024,
      y: foo.bar.y,
      z: z,
    ),
  ),
  foo2:   Values(
    x: foo2.x,
    bar2:     Values(
      x: bar2.x,
    ),
  ),
)
[('x', x),
 ('y', y),
 ('z', z),
 ('t.h', h),
 ('t.m', m),
 ('t.s', s),
 ('t.timezone.time_zone', zone),
 ('id', 1024),
 ('foo.x', foo.x),
 ('foo.bar.x', 1024),
 ('foo.bar.y', foo.bar.y),
 ('foo.bar.z', z),
 ('foo2.x', foo2.x),
 ('foo2.bar2.x', bar2.x)]
In [10]: 
[('distances[0]',
  Matrix([
  [t0],
  [t1]])),
 ('distances[1]',
  Matrix([
  [t0],
  [t1]])),
 ('epsilon', 2.220446049250313e-15)]
In [11]: 
tangent dim: 6, storage dim: 7
storage_D_tangent shape: (7, 6)
$\displaystyle \left[\begin{matrix}T.t0 + b_{0} \left(- 2 T.R_{y}^{2} - 2 T.R_{z}^{2} + 1\right) + b_{1} \left(- 2 T.R_{w} T.R_{z} + 2 T.R_{x} T.R_{y}\right) + b_{2} \cdot \left(2 T.R_{w} T.R_{y} + 2 T.R_{x} T.R_{z}\right) - w_{0}\\T.t1 + b_{0} \cdot \left(2 T.R_{w} T.R_{z} + 2 T.R_{x} T.R_{y}\right) + b_{1} \left(- 2 T.R_{x}^{2} - 2 T.R_{z}^{2} + 1\right) + b_{2} \left(- 2 T.R_{w} T.R_{x} + 2 T.R_{y} T.R_{z}\right) - w_{1}\\T.t2 + b_{0} \left(- 2 T.R_{w} T.R_{y} + 2 T.R_{x} T.R_{z}\right) + b_{1} \cdot \left(2 T.R_{w} T.R_{x} + 2 T.R_{y} T.R_{z}\right) + b_{2} \left(- 2 T.R_{x}^{2} - 2 T.R_{y}^{2} + 1\right) - w_{2}\end{matrix}\right]$
$\displaystyle \left( 3, \ 6\right)$
$\displaystyle \left[\begin{matrix}b_{1} \cdot \left(2 T.R_{w} T.R_{y} + 2 T.R_{x} T.R_{z}\right) + b_{2} \cdot \left(2 T.R_{w} T.R_{z} - 2 T.R_{x} T.R_{y}\right) & b_{0} \left(- 2 T.R_{w} T.R_{y} - 2 T.R_{x} T.R_{z}\right) + b_{2} \left(T.R_{w}^{2} + T.R_{x}^{2} - T.R_{y}^{2} - T.R_{z}^{2}\right) & b_{0} \left(- 2 T.R_{w} T.R_{z} + 2 T.R_{x} T.R_{y}\right) + b_{1} \left(- T.R_{w}^{2} - T.R_{x}^{2} + T.R_{y}^{2} + T.R_{z}^{2}\right) & 1 & 0 & 0\\b_{1} \left(- 2 T.R_{w} T.R_{x} + 2 T.R_{y} T.R_{z}\right) + b_{2} \left(- T.R_{w}^{2} + T.R_{x}^{2} - T.R_{y}^{2} + T.R_{z}^{2}\right) & b_{0} \cdot \left(2 T.R_{w} T.R_{x} - 2 T.R_{y} T.R_{z}\right) + b_{2} \cdot \left(2 T.R_{w} T.R_{z} + 2 T.R_{x} T.R_{y}\right) & b_{0} \left(T.R_{w}^{2} - T.R_{x}^{2} + T.R_{y}^{2} - T.R_{z}^{2}\right) + b_{1} \left(- 2 T.R_{w} T.R_{z} - 2 T.R_{x} T.R_{y}\right) & 0 & 1 & 0\\b_{1} \left(T.R_{w}^{2} - T.R_{x}^{2} - T.R_{y}^{2} + T.R_{z}^{2}\right) + b_{2} \left(- 2 T.R_{w} T.R_{x} - 2 T.R_{y} T.R_{z}\right) & b_{0} \left(- T.R_{w}^{2} + T.R_{x}^{2} + T.R_{y}^{2} - T.R_{z}^{2}\right) + b_{2} \left(- 2 T.R_{w} T.R_{y} + 2 T.R_{x} T.R_{z}\right) & b_{0} \cdot \left(2 T.R_{w} T.R_{x} + 2 T.R_{y} T.R_{z}\right) + b_{1} \cdot \left(2 T.R_{w} T.R_{y} - 2 T.R_{x} T.R_{z}\right) & 0 & 0 & 1\end{matrix}\right]$

Ops Package

Storage Ops

Data type that can be serialized to and from a vector of scalar quantities. .storage_dim() , .to_storage() , .from_storage() , .symbolic() , .evalf() , .subs() , .simplify()

In [12]: 
# Storage Ops

from symforce.ops import StorageOps

# Element-wise operations on Values object with multiple types of elements
values = Values(
    pose=sf.Pose3(),
    scalar=sf.Symbol("x"),
)
print(f"Storage dim: {StorageOps.storage_dim(values)}")  # 4 quaternion + 3 position + 1 scalar

# Serialize geometric type and reconstruct
T = sf.Pose3.symbolic("T")
T_serialized = StorageOps.to_storage(T)
T_recovered = StorageOps.from_storage(sf.Pose3, T_serialized)
display(T_serialized)
assert(T == T_recovered)

Storage dim: 8
$\displaystyle \left[ T.R_{x}, \ T.R_{y}, \ T.R_{z}, \ T.R_{w}, \ T.t0, \ T.t1, \ T.t2\right]$

Group Ops

Mathematical group that implements closure, associativity, identity and invertibility. Methods: .identity() , .inverse() , .compose() , .between()

In [13]: 
# Group Ops
from symforce.ops import GroupOps
# Identity of a pose
w_T_b = sf.Pose3()
display(GroupOps.identity(w_T_b))
# Inverse of a vector
p_b = sf.V3(1, 1, 1)
display(GroupOps.inverse(p_b).T)

# Compose a pose and a point (wrong, they are not the same group)
p_w = w_T_b * p_b # GroupOps.compose(w_T_b, p_b)
# display(p_w)
display(GroupOps.compose(p_b, GroupOps.inverse(p_b)).T)

<Pose3 R=<Rot3 <Q xyzw=[0, 0, 0, 1]>>, t=(0, 0, 0)>
$\displaystyle \left[\begin{matrix}-1 & -1 & -1\end{matrix}\right]$
$\displaystyle \left[\begin{matrix}0 & 0 & 0\end{matrix}\right]$
In [14]: 
# Relative rotation -> b1_R_b2 = GroupOps.between(w_R_b1, w_R_b2)
w_R_b1 = sf.Rot3.from_angle_axis(
    angle=sf.Scalar(0.2),
    axis=sf.V3(0, 1, 2).normalized(),
)
w_R_b2 = sf.Rot3.from_angle_axis(
    angle=sf.Scalar(0.1),
    axis=sf.V3(1, 0, 3).normalized(),
)
b1_R_b2 = GroupOps.between(w_R_b1, w_R_b2)
display(w_R_b2)
display(GroupOps.simplify(GroupOps.compose(w_R_b1, b1_R_b2)))

<Rot3 <Q xyzw=[0.00499791692706783*sqrt(10), 0, 0.0149937507812035*sqrt(10), 0.998750260394966]>>
<Rot3 <Q xyzw=[0.00499791692706783*sqrt(10), 0, 0.0149937507812035*sqrt(10), 0.998750260394966]>>

Lie Group Ops

Group that is also a differentiable manifold, e.g. SO3, SE3. Methods: .tangent_dim() , .from_tangent() , to_tangent() , .retract() , .local_coordinates() , .storage_D_tangent()

In [15]: 
# Lie Group Ops: basics
from symforce.ops import LieGroupOps
R = sf.Rot3.symbolic('theta')
# Underlying dimension of a 3D rotation's tangent space
print(f"3D rotation tangent space: {LieGroupOps.tangent_dim(R)}")
R_tangent = sf.V3(LieGroupOps.to_tangent(R))
display(R_tangent.T)
R_reconstruct_ = sf.Rot3.from_angle_axis(R_tangent.norm(), R_tangent.normalized())
R_reconstruct = LieGroupOps.from_tangent(sf.Rot3, R_tangent)
assert R_reconstruct_ == R_reconstruct

3D rotation tangent space: 3
$\displaystyle \left[\begin{matrix}\frac{2 \theta_{x} \left(2 \min\left(0, \operatorname{sign}{\left(\theta_{w} \right)}\right) + 1\right) \operatorname{acos}{\left(\min\left(1 - \epsilon, \left|{\theta_{w}}\right|\right) \right)}}{\sqrt{1 - \min\left(1 - \epsilon, \left|{\theta_{w}}\right|\right)^{2}}} & \frac{2 \theta_{y} \left(2 \min\left(0, \operatorname{sign}{\left(\theta_{w} \right)}\right) + 1\right) \operatorname{acos}{\left(\min\left(1 - \epsilon, \left|{\theta_{w}}\right|\right) \right)}}{\sqrt{1 - \min\left(1 - \epsilon, \left|{\theta_{w}}\right|\right)^{2}}} & \frac{2 \theta_{z} \left(2 \min\left(0, \operatorname{sign}{\left(\theta_{w} \right)}\right) + 1\right) \operatorname{acos}{\left(\min\left(1 - \epsilon, \left|{\theta_{w}}\right|\right) \right)}}{\sqrt{1 - \min\left(1 - \epsilon, \left|{\theta_{w}}\right|\right)^{2}}}\end{matrix}\right]$ 
---------------------------------------------------------------------------
AssertionError                            Traceback (most recent call last)
Cell In[15], line 10
      8 R_reconstruct_ = sf.Rot3.from_angle_axis(R_tangent.norm(), R_tangent.normalized())
      9 R_reconstruct = LieGroupOps.from_tangent(sf.Rot3, R_tangent)
---> 10 assert R_reconstruct_ == R_reconstruct

AssertionError:
In [ ]: 
# Lie Group Ops, retract: R_perturbed = R * delta_R, local_coordinates: delta_R = R.inv() * R_perturbed

# Retract perturbs the given element in the tangent space and returns the updated element
R = sf.Rot3.from_yaw_pitch_roll(0, 0, 1)
R_tangent_delta = sf.V3(0, 0, 0.1)
R_perturbed = LieGroupOps.retract(R, R_tangent_delta)
# Local coordinates compute the tangent space perturbation between one element and another
R_tangent_delta_reconstruct = LieGroupOps.local_coordinates(R, R_perturbed)
display(R_tangent_delta.T)
display(R_tangent_delta_reconstruct)

Geo Package Ops Operations

Geo Storage Operations

In [ ]:

Geo Group Operations

In [ ]:

Geo Tangent Operations

In [ ]:

Special SE(n)

Perturbation on SE(3) is done separately on SO(3) and R(3). 

Pose3(R=self.R.retract(vec[:3], epsilon=epsilon),
t=ops.LieGroupOps.retract(self.t, vec[3:], epsilon=epsilon))
In [ ]: 
# SE(n)
T = sf.Pose3(sf.Rot3.random(), sf.V3(0, 0, 1))
display(T)
T_tangent_delta = [0.1, 0.1, 0.1, 0.1, 0, 0.1]
T_perturb = T.retract(T_tangent_delta)
display(T_perturb)
T_perturb_reconstruct =  T.compose(sf.Pose3.from_tangent(T_tangent_delta))
display(T_perturb_reconstruct)
assert T_perturb_reconstruct != T_perturb # This is expected since SE(3) is modeled as SO(3) + R(3)
T_perturb_reconstruct = sf.Pose3(T.rotation().compose(sf.Rot3.from_tangent(T_tangent_delta[0:3])), 
                                             T.t + sf.V3(T_tangent_delta[3:]))
assert T_perturb_reconstruct == T_perturb
display(T_perturb_reconstruct)

Code Generation

In [ ]:

Code generation using implicit functions with Values objects

In [ ]: 
# Generate functions
from symforce.values import Values
inputs = Values(
    w_T_b = sf.Pose3.symbolic("T"),
    b_t_p = sf.V3.symbolic("p")
)
namespace = "my_sym"
outputs = Values()
outputs["w_t_p"] = inputs["w_T_b"] * inputs.attr.b_t_p

input_values = Values(inputs=inputs)
output_values = Values(outputs=outputs)
point_projection_condegen = codegen.Codegen(
    inputs=input_values,
    outputs=output_values,
    config=codegen.CppConfig(),
    name="point_projection",
    #  If specified, the output with this key is returned rather than filled in as a named output argument
    return_key="outputs"
)

point_projection_condegen_data = point_projection_condegen.generate_function(namespace=namespace)

# Print what we generated
print("Files generated in {}:\n".format(point_projection_condegen_data.output_dir))
for f in point_projection_condegen_data.generated_files:
    print("  |- {}".format(os.path.relpath(f, point_projection_condegen_data.output_dir)))
display_code_file(point_projection_condegen_data.function_dir / "point_projection.h", "C++")
# display_code_file(point_projection_condegen_data.cpp_types_dir / namespace / "inputs_t.hpp", "C++")
# display_code_file(point_projection_condegen_data.cpp_types_dir / namespace / "outputs_t.hpp", "C++")

Code generation using python functions

In [ ]:
In [ ]:
In [ ]:

Optimization

Let's walk through solving a simplified point cloud registration problem with Symforce. In this example, we know the correspondences between two point clouds and we would like to find a sf.Pose3 to align two point clouds.

Symforce setup

In this example, we will use symbolic variables to define our residual function so we need to import symforce.symbolic .

In [ ]: 
import symforce.symbolic as sf

We build up input Values using runtime types, e.g. sym.pose3 , np.ndarray instead of symbolic types.

In [ ]: 
from symforce.values import Values
import numpy as np
import sym

For building a factor graph and solve the problem, we need to import Factor and Optimizer .

In [ ]: 
from symforce.opt.factor import Factor
from symforce.opt.optimizer import Optimizer

In this example, we will also show the usage of customized cost functions for computing residuals.

In [ ]: 
from symforce.opt.noise_models import IsotropicNoiseModel

Set up the problem

We first generate 30 points in robot body frame.

In [ ]: 
np.random.seed(seed=1024)
num_points = 30
points_b = []
for i in range(num_points):
    points_b.append(np.random.uniform(low=0.0, high=1.0, size=(3,)))

We create a ground truth w_T_b_GT , and use it to transform the 30 points in body frame to world frame.

In [ ]: 
pose_tangent_delta = np.array([0.1, 0.1, 0, 0, 0, 10])
w_T_b_GT = sym.Pose3.identity().retract(pose_tangent_delta)
points_w = []
for i in range(num_points):
    points_w.append(w_T_b_GT * points_b[i])

Build an optimization problem

We define a simple residual function which simply calculates the difference between two corresponded points in the aligned point clouds. Note that we apply a IsotropicNoiseModel to whiten the residual. It doesn't really affect the optimization results but you can see the final total error is scaled by the sigma .

In [ ]: 
def projection_residual(w_T_b: sf.Pose3, b_t_p: sf.V3, w_t_p: sf.V3, sigma: sf.Scalar) -> sf.V3:
    noise_model = IsotropicNoiseModel.from_sigma(sigma)
    return noise_model.whiten(w_T_b*b_t_p - w_t_p)

We build up the inputs to the optimization problem using symforce runtime types. Note that the initial pose value is sym.Pose3.identity() which is different from w_T_b_GT . We hope the optimizer can give us a solution very close to w_T_b_GT .

In [ ]: 
inputs = Values(
    w_T_b=sym.Pose3.identity(),
    points_b=points_b,
    points_w=points_w,
    sigma=10.0,
)

Create Factor s from the residual functions and a set of keys.

In [ ]: 
factors = []
for i in range(num_points):
    factors.append(
        Factor(
            residual=projection_residual,
            keys=["w_T_b", f"points_b[{i}]", f"points_w[{i}]", "sigma"],
        )
    )

Solve the optimization problem

We create an Optimizer with factors and tell it to only optimize the pose key (the rest are held constant). Here we show lots of configure parameters for the optimizer you can tune.

In [ ]: 
optimizer = Optimizer(
    factors=factors,
    optimized_keys=["w_T_b"],
    debug_stats=False,
    params=Optimizer.Params(
        verbose = False,
        initial_lambda = 1.0,
        lambda_up_factor = 4.0,
        lambda_down_factor = 1 / 4.0,
        lambda_lower_bound = 0.0,
        lambda_upper_bound = 10000.0,
        use_diagonal_damping = False,
        use_unit_damping = False,
        keep_max_diagonal_damping = False,
        diagonal_damping_min = 1e-6,
        iterations = 500,
        early_exit_min_reduction = 1e-6,
        enable_bold_updates = False,
    )
)

Run the optimization! This returns an Optimizer.Result object that contains the optimized values and many other things.

In [ ]: 
results = optimizer.optimize(inputs)

We can print out the pose before and after the optimization and compare it against our w_T_b_GT .

In [ ]: 
print(f"Early exist? : {results.early_exited}")
print("w_T_b_GT: ")
display(w_T_b_GT)
print("Initial w_T_b: ")
display(results.initial_values.attr.w_T_b)
print("Optimized w_T_b: ")
display(results.optimized_values.attr.w_T_b)

We can also print out the total error before and after optimization. Try to change the sigma to see how it will change the error value.

In [ ]: 
initial_linearization = optimizer.linearize(results.initial_values)
print(f"Initial error: {initial_linearization.error()}")

optimized_linearization = optimizer.linearize(results.optimized_values)
print(f"Final error: {optimized_linearization.error()}")

Cameras

Camera Calibrations

Symforce implements several different calibration models, such as linear model( LinearCameraCal ), polynomial model( PolynomialCameraCal ), Kannala-Brandt camera model( SphericalCameraCal ), etc. They basically contains 1) linear model + distortion coeffs 2) projection and un-projection methods could be used by Camera class.

In [ ]: 
# Linear model

linear_camera_cal = sf.LinearCameraCal.symbolic("cal")
display(linear_camera_cal)

camera_pixel = sf.V2.symbolic("uv")
camera_ray, valid = linear_camera_cal.camera_ray_from_pixel(camera_pixel)
print("camera_ray_from_pixel: ")
display(camera_ray)
#print(f"Is valid? {valid == 1}")

camera_point = sf.V3.symbolic("p")
camera_point_reprojected, _ = linear_camera_cal.pixel_from_camera_point(
    camera_point,
)
print("pixel_from_camera_point: ")
display(camera_point_reprojected)

# Jacobians
print("Jacobians: ")
display(camera_point_reprojected.jacobian(linear_camera_cal.focal_length))
display(camera_point_reprojected.jacobian(linear_camera_cal.principal_point))


linear_camera_cal_num = sf.LinearCameraCal(
    focal_length=(440, 400),
    principal_point=(320, 240),
)

# Numerical substitution for fun...
linear_camera_cal = linear_camera_cal.subs(linear_camera_cal, linear_camera_cal_num)

linear_camera = sf.Camera(
    calibration=linear_camera_cal,
    image_size=(640, 480),
)
print("Linear Camera: ")
display(linear_camera)

Solve Problems in Real World

This blog is converted from symforce.ipynb
Written on December 31, 2022