symforce

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.

Features

• Symbolic implementations of geometry and camera types with Lie group operations
• Code generation of fast native runtime code from symbolic expressions, reducing duplication and minimizing bugs
• Novel tools to compute fast and correct tangent-space jacobians for any expression, avoiding all handwritten derivatives
• Strategies for flattening computation and leveraging sparsity that can yield 10x speedups over standard autodiff
• A fast tangent-space optimization library in C++ and Python based on factor graphs
• Rapid prototyping and analysis of complex problems with symbolic math, with a seamless workflow into production use
• Embedded-friendly C++ generation of templated Eigen code with zero dynamic memory allocation
• Highly performant, modular, tested, and extensible code

Read the paper: https://arxiv.org/abs/2204.07889

And watch the video: https://youtu.be/QO_ltJRNj0o

SymForce was published to RSS 2022. Please cite it as follows:

@inproceedings{Martiros-RSS-22,
    author    = {Hayk Martiros AND Aaron Miller AND Nathan Bucki AND Bradley Solliday AND Ryan Kennedy AND Jack Zhu AND Tung Dang AND Dominic Pattison AND Harrison Zheng AND Teo Tomic AND Peter Henry AND Gareth Cross AND Josiah VanderMey AND Alvin Sun AND Samuel Wang AND Kristen Holtz},
    title     = {{SymForce: Symbolic Computation and Code Generation for Robotics}},
    booktitle = {Proceedings of Robotics: Science and Systems},
    year      = {2022},
    doi       = {10.15607/RSS.2022.XVIII.041}
}

Install

Install with pip:

pip install symforce

Verify the installation in Python:

>>> import symforce.symbolic as sf
>>> sf.Rot3()

This installs pre-compiled C++ components of SymForce on Linux and Mac using pip wheels, but does not include C++ headers. If you want to compile against C++ SymForce types (like sym::Optimizer), you currently need to build from source.

Tutorial

Let's walk through a simple example of modeling and solving an optimization problem with SymForce. In this example a robot moves through a 2D plane and the goal is to estimate its pose at multiple time steps given noisy measurements.

The robot measures:

• the distance it traveled from an odometry sensor
• relative bearing angles to known landmarks in the scene

The robot's heading angle is defined counter-clockwise from the x-axis, and its relative bearing measurements are defined from the robot's forward direction:

Explore the math

Import the SymForce symbolic API, which contains the augmented SymPy API, as well as geometry and camera types:

import symforce.symbolic as sf

Create a symbolic 2D pose and landmark location. Using symbolic variables lets us explore and build up the math in a pure form.

pose = sf.Pose2(
    t=sf.V2.symbolic("t"),
    R=sf.Rot2.symbolic("R")
)
landmark = sf.V2.symbolic("L")

Let's transform the landmark into the local frame of the robot. We choose to represent poses as world_T_body, meaning that to take a landmark in the world frame and get its position in the body frame, we do:

landmark_body = pose.inverse() * landmark

$$ \begin{bmatrix} R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0 \ -R_{im} L_0 + R_{re} L_1 + R_{im} t_0 + R_{re} t_1 \end{bmatrix} $$

You can see that sf.Rot2 is represented internally by a complex number (, ) and we can study how it rotates the landmark .

For exploration purposes, let's take the jacobian of the body-frame landmark with respect to the tangent space of the Pose2, parameterized as (, , ):

landmark_body.jacobian(pose)

$$ \begin{bmatrix} -L_0 R_{im} + L_1 R_{re} + t_0 R_{im} - t_1 R_{re}, & -R_{re}, & -R_{im} \ -L_0 R_{re} - L_1 R_{im} + t_0 R_{re} + t_1 R_{im}, & R_{im}, & -R_{re} \end{bmatrix} $$

Note that even though the orientation is stored as a complex number, the tangent space is a scalar angle and SymForce understands that.

Now compute the relative bearing angle:

sf.atan2(landmark_body[1], landmark_body[0])

$$ atan_2(-R_{im} L_0 + R_{re} L_1 + R_{im} t_0 + R_{re} t_1, R_{re} L_0 + R_{im} L_1 - R_{im} t_1 - R_{re} t_0) $$

One important note is that atan2 is singular at (0, 0). In SymForce we handle this by placing a symbol ϵ (epsilon) that preserves the value of an expression in the limit of ϵ → 0, but allows evaluating at runtime with a very small nonzero value. Functions with singularities accept an epsilon argument:

sf.V3.symbolic("x").norm(epsilon=sf.epsilon())

$$ \sqrt{x_0^2 + x_1^2 + x_2^2 + \epsilon} $$

See the Epsilon Tutorial in the SymForce Docs for more information.

Build an optimization problem

We will model this problem as a factor graph and solve it with nonlinear least-squares.

First, we need to tell SymForce to use a nonzero epsilon to prevent singularities. This isn't necessary when playing around with symbolic expressions like we were above, but it's important now that we want to numerically evaluate some results. For more information, check out the Epsilon Tutorial - for now, all you need to do is this:

import symforce
symforce.set_epsilon_to_symbol()

This needs to be done before other parts of symforce are imported - if you're following along in a notebook you should add this at the top and restart the kernel.

Now that epsilon is set up, we will instantiate numerical Values for the problem, including an initial guess for our unknown poses (just set them to identity).

import numpy as np
from symforce.values import Values

num_poses = 3
num_landmarks = 3

initial_values = Values(
    poses=[sf.Pose2.identity()] * num_poses,
    landmarks=[sf.V2(-2, 2), sf.V2(1, -3), sf.V2(5, 2)],
    distances=[1.7, 1.4],
    angles=np.deg2rad([[145, 335, 55], [185, 310, 70], [215, 310, 70]]).tolist(),
    epsilon=sf.numeric_epsilon,
)

Next, we can set up the factors connecting our variables. The residual function comprises of two terms - one for the bearing measurements and one for the odometry measurements. Let's formalize the math we just defined for the bearing measurements into a symbolic residual function:

def bearing_residual(
    pose: sf.Pose2, landmark: sf.V2, angle: sf.Scalar, epsilon: sf.Scalar
) -> sf.V1:
    t_body = pose.inverse() * landmark
    predicted_angle = sf.atan2(t_body[1], t_body[0], epsilon=epsilon)
    return sf.V1(sf.wrap_angle(predicted_angle - angle))

This function takes in a pose and landmark variable and returns the error between the predicted bearing angle and a measured value. Note that we call sf.wrap_angle on the angle difference to prevent wraparound effects.

The residual for distance traveled is even simpler:

def odometry_residual(
    pose_a: sf.Pose2, pose_b: sf.Pose2, dist: sf.Scalar, epsilon: sf.Scalar
) -> sf.V1:
    return sf.V1((pose_b.t - pose_a.t).norm(epsilon=epsilon) - dist)

Now we can create Factor objects from the residual functions and a set of keys. The keys are named strings for the function arguments, which will be accessed by name from a Values class we later instantiate with numerical quantities.

from symforce.opt.factor import Factor

factors = []

# Bearing factors
for i in range(num_poses):
    for j in range(num_landmarks):
        factors.append(Factor(
            residual=bearing_residual,
            keys=[f"poses[{i}]", f"landmarks[{j}]", f"angles[{i}][{j}]", "epsilon"],
        ))

# Odometry factors
for i in range(num_poses - 1):
    factors.append(Factor(
        residual=odometry_residual,
        keys=[f"poses[{i}]", f"poses[{i + 1}]", f"distances[{i}]", "epsilon"],
    ))

Here is a visualization of the structure of this factor graph:

Solve the problem

Our goal is to find poses of the robot that minimize the residual of this factor graph, assuming the landmark positions in the world are known. We create an Optimizer with these factors and tell it to only optimize the pose keys (the rest are held constant):

from symforce.opt.optimizer import Optimizer

optimizer = Optimizer(
    factors=factors,
    optimized_keys=[f"poses[{i}]" for i in range(num_poses)],
    # So that we save more information about each iteration, to visualize later:
    debug_stats=True,
)

Now run the optimization! This returns an Optimizer.Result object that contains the optimized values, error statistics, and per-iteration debug stats (if enabled).

result = optimizer.optimize(initial_values)

We can check that the optimization succeeded, and look at the final error:

assert result.status == Optimizer.Status.SUCCESS
print(result.error())

Let's visualize what the optimizer did. The orange circles represent the fixed landmarks, the blue circles represent the robot, and the dotted lines represent the bearing measurements.

from symforce.examples.robot_2d_localization.plotting import plot_solution
plot_solution(optimizer, result)

All of the code for this example can also be found in symforce/examples/robot_2d_localization.

Symbolic vs Numerical Types

SymForce provides sym packages with runtime code for geometry and camera types that are generated from its symbolic geo and cam packages. As such, there are multiple versions of a