<!-- There's no good way to make an image that's different on light/dark mode that also links somewhere. The closest thing is to add a #gh-light-mode-only to the target, which does this, but is kinda confusing -->

<!-- DARK_MODE_ONLY -->

<!-- /DARK_MODE_ONLY --> <p align="center"> <a href="https://github.com/symforce-org/symforce/actions/workflows/ci.yml?query=branch%3Amain"><img alt="CI status" src="https://github.com/symforce-org/symforce/actions/workflows/ci.yml/badge.svg" /></a> <a href="https://symforce.org"><img alt="Documentation" src="https://img.shields.io/badge/api-docs-blue" /></a> <a href="https://github.com/symforce-org/symforce"><img alt="Source Code" src="https://img.shields.io/badge/source-code-blue" /></a> <a href="https://github.com/symforce-org/symforce/issues"><img alt="Issues" src="https://img.shields.io/badge/issue-tracker-blue" /></a> <img alt="Python 3.9 | 3.10 | 3.11 | 3.12" src="https://img.shields.io/pypi/pyversions/symforce" /> <img alt="C++17" src="https://img.shields.io/badge/c++-17-blue" /> <a href="https://pypi.org/project/symforce/"><img alt="PyPI" src="https://img.shields.io/pypi/v/symforce" /></a> <a href="https://github.com/symforce-org/symforce/tree/main/LICENSE"><img alt="Apache License" src="https://img.shields.io/pypi/l/symforce" /></a> </p>

---

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.

<br/> <img alt="SymForce" src="docs/static/images/symforce_diagram.png" width="700px"/> <br/>

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: <a href="https://arxiv.org/abs/2204.07889">https://arxiv.org/abs/2204.07889</a>

And watch the video: <a href="https://youtu.be/QO_ltJRNj0o">https://youtu.be/QO_ltJRNj0o</a>

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 <a href="#build-from-source">build from source</a>.

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:

<img alt="Robot 2D Localization Figure" src="docs/static/images/robot_2d_localization/problem_setup.png" width="350px"/>

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(pr