---
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](https://skydio.com/). 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](https://roboticsconference.org/). 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:
```bash
pip install symforce
```
Verify the installation in Python:
```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:
```python
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.
```python
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:
```python
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 (, , ):
```python
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:
```python
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:
```python
sf.V3.symbolic("x").norm(epsilon=sf.epsilon())
```
$$
\sqrt{x_0^2 + x_1^2 + x_2^2 + \epsilon}
$$
See the [Epsilon Tutorial](https://symforce.org/tutorials/epsilon_tutorial.html) 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](https://symforce.org/tutorials/epsilon_tutorial.html) - for now, all you need to do is this:
```python
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`](https://symforce.org/api/symforce.values.values.html?highlight=values#module-symforce.values.values) for the problem, including an initial guess for our unknown poses (just set them to identity).
```python
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:
```python
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:
```python
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`](https://symforce.org/api/symforce.opt.factor.html?highlight=factor#module-symforce.opt.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`](https://symforce.org/api/symforce.values.values.html) class we later instantiate with numerical quantities.
```python
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`](https://symforce.org/api/symforce.opt.optimizer.html?highlight=optimizer#module-symforce.opt.optimizer)
with these factors and tell it to only optimize the pose keys (the rest are held constant):
```python
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`](https://symforce.org/api/symforce.opt.optimizer.html?highlight=optimizer#symforce.opt.optimizer.Optimizer.Result) object that contains the optimized values, error statistics, and per-iteration debug stats (if enabled).
```python
result = optimizer.optimize(initial_values)
```
We can check that the optimization succeeded, and look at the final error:
```python
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.
```python
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