https://github.com/symforce-org/symforce Skip to content Sign up * Product + Features + Mobile + Actions + Codespaces + Packages + Security + Code review + Issues + Integrations + GitHub Sponsors + Customer stories * Team * Enterprise * Explore + Explore GitHub + Learn and contribute + Topics + Collections + Trending + Learning Lab + Open source guides + Connect with others + The ReadME Project + Events + Community forum + GitHub Education + GitHub Stars program * Marketplace * Pricing + Plans + Compare plans + Contact Sales + Education [ ] * # In this repository All GitHub | Jump to | * No suggested jump to results * # In this repository All GitHub | Jump to | * # In this organization All GitHub | Jump to | * # In this repository All GitHub | Jump to | Sign in Sign up {{ message }} symforce-org / symforce Public * Notifications * Fork 43 * Star 351 Fast symbolic computation, code generation, and nonlinear optimization for robotics symforce.org License Apache-2.0 license 351 stars 43 forks Star Notifications * Code * Issues 104 * Pull requests 1 * Actions * Projects 0 * Security * Insights More * Code * Issues * Pull requests * Actions * Projects * Security * Insights This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository. main Switch branches/tags [ ] Branches Tags Could not load branches Nothing to show {{ refName }} default View all branches Could not load tags Nothing to show {{ refName }} default View all tags 11 branches 2 tags Code Latest commit @aaron-skydio aaron-skydio [SymForce-External] Epsilon tutorial, rename ops tutorial ... ad2a796 May 24, 2022 [SymForce-External] Epsilon tutorial, rename ops tutorial Import of #135 ORIGINAL_AUTHOR = Aaron Miller 78561124+aaron-skydio@users.noreply.github.com - Epsilon tutorial, rename ops tutorial - More docs and tutorials updates - add intro to epsilon tutorial - Update optimization tutorial - Update README.md - Update wording on build in readme - Rename requirements.txt to dev_requirements.txt - Update theme settings - skymarshal updates - update sym packaging - Fix more stuff - Fix dev.rst, new badges + classifiers - Fix skymarshal setup.py - Data files, sym -> symforce-sym, scipy required - Fix tests, hopefully - Bump version to 0.4.0 - Fix tests ad2a796 Git stats * 543 commits Files Permalink Failed to load latest commit information. Type Name Latest commit message Commit time .github/ISSUE_TEMPLATE [SymForce-External] Add issue templates May 23, 2022 cmake [SymForce] Support pip install [-e] . May 16, 2022 docs [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 gen [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 lcmtypes [Symforce] Initial work for valuesyp to support DataBuffer entries May 16, 2022 notebooks [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 symforce [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 test [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 third_party [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 .clang-format [SymForce] Separate symforce includes Dec 1, 2021 .coveragerc SymForce basic init framework Apr 14, 2020 .gitattributes [SymForce] New Images Apr 16, 2022 .gitignore [SymForce-External] Examples and Benchmarks Apr 1, 2022 CITATION [SymForce] Add citation to repo, README Apr 20, 2022 CMakeLists.txt [SymForce] Support pip install [-e] . May 16, 2022 LICENSE [SymForce] Update licenses Feb 18, 2022 Makefile [SymForce] Pin pip requirements May 16, 2022 README.md [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 dev_requirements.txt [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 pylintrc [SymForce] Added some useful timestep objectives Apr 16, 2022 pyproject.toml [SymForce] Support pip install [-e] . May 16, 2022 setup.cfg [SymForce-External] Examples and Benchmarks Apr 1, 2022 setup.py [SymForce-External] Epsilon tutorial, rename ops tutorial May 24, 2022 View code [ ] Features: Read the paper: https://arxiv.org/abs/2204.07889 Install Tutorial Explore the math Build an optimization problem Solve the problem Symbolic vs Numerical Types Generate runtime C++ code Optimize from C++ Build from Source Build with pip Build with CMake License Sponsors Contributing README.md SymForce SymForce Documentation Source Code Issues Python 3.8 | 3.9 | 3.10 C++14 PyPI Apache License --------------------------------------------------------------------- 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. SymForce 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 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: >>> from symforce import geo >>> geo.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: Robot 2D Triangulation Figure Explore the math Import the augmented SymPy API and geometry module: from symforce import sympy as sm from symforce import geo Create a symbolic 2D pose and landmark location. Using symbolic variables lets us explore and build up the math in a pure form. pose = geo.Pose2( t=geo.V2.symbolic("t"), R=geo.Rot2.symbolic("R") ) landmark = geo.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 [math] [math] You can see that geo.Rot2 is represented internally by a complex number (Rre, Rim) and we can study how it rotates the landmark L. For exploration purposes, let's take the jacobian of the body-frame landmark with respect to the tangent space of the Pose2, parameterized as (, x, y): landmark_body.jacobian(pose) [math] [math] 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: sm.atan2(landmark_body[1], landmark_body[0]) [math] [math] 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: geo.V3.symbolic("x").norm(epsilon=sm.epsilon) [math] [math] 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. 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: from symforce import typing as T def bearing_residual( pose: geo.Pose2, landmark: geo.V2, angle: T.Scalar, epsilon: T.Scalar ) -> geo.V1: t_body = pose.inverse() * landmark predicted_angle = sm.atan2(t_body[1], t_body[0], epsilon=epsilon) return geo.V1(sm.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 sm.wrap_angle on the angle difference to prevent wraparound effects. The residual for distance traveled is even simpler: def odometry_residual( pose_a: geo.Pose2, pose_b: geo.Pose2, dist: T.Scalar, epsilon: T.Scalar ) -> geo.V1: return geo.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 num_poses = 3 num_landmarks = 3 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: Robot 2D Triangulation 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 we need to 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 initial_values = Values( poses=[geo.Pose2.identity()] * num_poses, landmarks=[geo.V2(-2, 2), geo.V2(1, -3), geo.V2(5, 2)], distances=[1.7, 1.4], angles=np.deg2rad([[145, 335, 55], [185, 310, 70], [215, 310, 70]]).tolist(), epsilon=sm.default_epsilon, ) 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) 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_triangulation.plotting import plot_solution plot_solution(optimizer, result) Robot 2D Triangulation Solution All of the code for this example can also be found in symforce/ examples/robot_2d_triangulation. 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 class like Pose3 and it can be a common source of confusion. The canonical symbolic class geo.Pose3 lives in the symforce package: from symforce import geo geo.Pose3.identity() The autogenerated Python runtime class sym.Pose3 lives in the sym package: import sym sym.Pose3.identity() The autogenerated C++ runtime class sym::Pose3 lives in the sym:: namespace: sym::Pose3::Identity() The matrix type for symbolic code is geo.Matrix, for generated Python is numpy.ndarray, and for C++ is Eigen::Matrix. The symbolic classes can also handle numerical values, but will be dramatically slower than the generated classes. The symbolic classes must be used when defining functions for codegen and optimization. Generated functions always accept the runtime types. The Codegen or Factor objects require symbolic types and functions to do math and generate code. However, once code is generated, numerical types should be used when invoking generated functions and in the initial values when calling the Optimizer. As a convenience, the Python Optimizer class can accept symbolic types in its Values and convert to numerical types before invoking generated functions. This is done in the tutorial example for simplicity. Generate runtime C++ code Let's look under the hood to understand how that optimization worked. For each factor, SymForce introspects the form of the symbolic function, passes through symbolic inputs to build an output expression, automatically computes tangent-space jacobians of those output expressions wrt the optimized variables, and generates fast runtime code for them. The Codegen class is the central tool for generating runtime code from symbolic expressions. In this case, we pass it the bearing residual function and configure it to generate C++ code: from symforce.codegen import Codegen, CppConfig codegen = Codegen.function(bearing_residual, config=CppConfig()) We can then create another Codegen object that computes a Gauss-Newton linearization from this Codegen object. It does this by introspecting and symbolically differentiating the given arguments: codegen_linearization = codegen.with_linearization( which_args=["pose"] ) Generate a C++ function that computes the linearization wrt the pose argument: metadata = codegen_linearization.generate_function() print(open(metadata.generated_files[0]).read()) This C++ code depends only on Eigen and computes the results in a single flat function that shares all common sub-expressions: #pragma once #include #include namespace sym { /** * This function was autogenerated from a symbolic function. Do not modify by hand. * * Symbolic function: bearing_residual * * Args: * pose: Pose2 * landmark: Matrix21 * angle: Scalar * epsilon: Scalar * * Outputs: * res: Matrix11 * jacobian: (1x3) jacobian of res wrt arg pose (3) * hessian: (3x3) Gauss-Newton hessian for arg pose (3) * rhs: (3x1) Gauss-Newton rhs for arg pose (3) */ template void BearingFactor(const sym::Pose2& pose, const Eigen::Matrix& landmark, const Scalar angle, const Scalar epsilon, Eigen::Matrix* const res = nullptr, Eigen::Matrix* const jacobian = nullptr, Eigen::Matrix* const hessian = nullptr, Eigen::Matrix* const rhs = nullptr) { // Total ops: 66 // Input arrays const Eigen::Matrix& _pose = pose.Data(); // Intermediate terms (24) const Scalar _tmp0 = _pose[1] * _pose[2]; const Scalar _tmp1 = _pose[0] * _pose[3]; const Scalar _tmp2 = _pose[0] * landmark(1, 0) - _pose[1] * landmark(0, 0); const Scalar _tmp3 = _tmp0 - _tmp1 + _tmp2; const Scalar _tmp4 = _pose[0] * _pose[2] + _pose[1] * _pose[3]; const Scalar _tmp5 = _pose[1] * landmark(1, 0); const Scalar _tmp6 = _pose[0] * landmark(0, 0); const Scalar _tmp7 = -_tmp4 + _tmp5 + _tmp6; const Scalar _tmp8 = _tmp7 + epsilon * ((((_tmp7) > 0) - ((_tmp7) < 0)) + Scalar(0.5)); const Scalar _tmp9 = -angle + std::atan2(_tmp3, _tmp8); const Scalar _tmp10 = _tmp9 - 2 * Scalar(M_PI) * std::floor((Scalar(1) / Scalar(2)) * (_tmp9 + Scalar(M_PI)) / Scalar(M_PI)); const Scalar _tmp11 = Scalar(1.0) / (_tmp8); const Scalar _tmp12 = std::pow(_tmp8, Scalar(2)); const Scalar _tmp13 = _tmp3 / _tmp12; const Scalar _tmp14 = _tmp11 * (_tmp4 - _tmp5 - _tmp6) - _tmp13 * (_tmp0 - _tmp1 + _tmp2); const Scalar _tmp15 = _tmp12 + std::pow(_tmp3, Scalar(2)); const Scalar _tmp16 = _tmp12 / _tmp15; const Scalar _tmp17 = _tmp14 * _tmp16; const Scalar _tmp18 = _pose[0] * _tmp13 + _pose[1] * _tmp11; const Scalar _tmp19 = _tmp16 * _tmp18; const Scalar _tmp20 = -_pose[0] * _tmp11 + _pose[1] * _tmp13; const Scalar _tmp21 = _tmp16 * _tmp20; const Scalar _tmp22 = std::pow(_tmp8, Scalar(4)) / std::pow(_tmp15, Scalar(2)); const Scalar _tmp23 = _tmp18 * _tmp22; // Output terms (4) if (res != nullptr) { Eigen::Matrix& _res = (*res); _res(0, 0) = _tmp10; } if (jacobian != nullptr) { Eigen::Matrix& _jacobian = (*jacobian); _jacobian(0, 0) = _tmp17; _jacobian(0, 1) = _tmp19; _jacobian(0, 2) = _tmp21; } if (hessian != nullptr) { Eigen::Matrix& _hessian = (*hessian); _hessian(0, 0) = std::pow(_tmp14, Scalar(2)) * _tmp22; _hessian(0, 1) = 0; _hessian(0, 2) = 0; _hessian(1, 0) = _tmp14 * _tmp23; _hessian(1, 1) = std::pow(_tmp18, Scalar(2)) * _tmp22; _hessian(1, 2) = 0; _hessian(2, 0) = _tmp14 * _tmp20 * _tmp22; _hessian(2, 1) = _tmp20 * _tmp23; _hessian(2, 2) = std::pow(_tmp20, Scalar(2)) * _tmp22; } if (rhs != nullptr) { Eigen::Matrix& _rhs = (*rhs); _rhs(0, 0) = _tmp10 * _tmp17; _rhs(1, 0) = _tmp10 * _tmp19; _rhs(2, 0) = _tmp10 * _tmp21; } } } // namespace sym SymForce can also generate runtime Python code that depends only on numpy. The code generation system is written with pluggable jinja templates to minimize the work to add new backend languages. Some of our top candidates to add are TypeScript, CUDA, and PyTorch. Optimize from C++ Now that we can generate C++ functions for each residual function, we can also run the optimization purely from C++ to get Python entirely out of the loop for production use. const int num_poses = 3; const int num_landmarks = 3; std::vector> factors; // Bearing factors for (int i = 0; i < num_poses; ++i) { for (int j = 0; j < num_landmarks; ++j) { factors.push_back(sym::Factor::Hessian( &sym::BearingFactor, {{'P', i}, {'L', j}, {'a', i, j}, {'e'}}, // keys {{'P', i}} // keys to optimize )); } } // Odometry factors for (int i = 0; i < num_poses - 1; ++i) { factors.push_back(sym::Factor::Hessian( &sym::OdometryFactor, {{'P', i}, {'P', i + 1}, {'d', i}, {'e'}}, // keys {{'P', i}, {'P', i + 1}} // keys to optimize )); } sym::Optimizer optimizer( params, factors, sym::kDefaultEpsilon ); sym::Values values; for (int i = 0; i < num_poses; ++i) { values.Set({'P', i}, sym::Pose2::Identity()); } // ... (initialize all keys) // Optimize! const auto stats = optimizer.Optimize(&values); std::cout << "Optimized values:" << values << std::endl; This tutorial shows the central workflow in SymForce for creating symbolic expressions, generating code, and optimizing. This approach works well for a wide range of complex problems in robotics, computer vision, and applied science. However, each piece may also be used independently. The optimization machinery can work with handwritten functions, and the symbolic math and code generation is useful outside of any optimization context. To learn more, visit the SymForce tutorials here. Build from Source SymForce requires Python 3.8 or later. We strongly suggest creating a virtual python environment. Install the gmp package with one of: apt install libgmp-dev # Ubuntu brew install gmp # Mac conda install -c conda-forge gmp # Conda SymForce contains both C++ and Python code. The C++ code is built using CMake. You can build the package either by calling pip, or by calling CMake directly. If building with pip, this will call CMake under the hood, and run the same CMake build for the C++ components. If you encounter build issues, please file an issue. Build with pip The recommended way to build and install SymForce if you only plan on making Python changes is with pip. From the symforce directory: pip install -e . This will build the C++ components of SymForce, but you won't be able to run pip install -e . repeatedly if you need to rebuild C++ code. If you're changing C++ code and rebuilding, you should build with CMake directly as described below. pip install . will not install pinned versions of SymForce's dependencies, it'll install any compatible versions. It also won't install all packages required to run all of the SymForce tests and build all of the targets (e.g. building the docs or running the linters). If you want all packages required for that, you should pip install .[dev] instead (or one of the other groups of extra requirements in our setup.py). If you additionally want pinned versions of our dependencies, which are the exact versions guaranteed by CI to pass all of our tests, you can install them from pip install -r dev_requirements.txt. Build with CMake If you'll be modifying the C++ parts of SymForce, you should build with CMake directly instead - this method will not install SymForce into your Python environment, so you'll need to add it to your PYTHONPATH separately. Install python requirements: pip install -r dev_requirements.txt Build SymForce (requires C++14 or later): mkdir build cd build cmake .. make -j $(nproc) You'll then need to add SymForce (along with gen/python and third_party/skymarshal within symforce) to your PYTHONPATH in order to use them. License SymForce is released under the Apache 2.0 license. See the LICENSE file for more information. Sponsors SymForce is developed and maintained by Skydio. It is released as a free and open-source library for the robotics community. Skydio Logo Skydio Logo Contributing While SymForce already powers tens of thousands of robots at Skydio, the public library is new and we are releasing it in beta stage. This is just the beginning, and we are excited for engagement from the community. Thank you for helping us develop SymForce! The best way to get started is to file issues for bugs or desired features. There are many features we're excited to add to SymForce and would love to see contributed by the community. Most are outlined in the issues, but some major desired contributions are: * Add more backend languages, such as TypeScript, CUDA, GLSL/HLSL, and PyTorch * Easily swap in approximate or architecture-specific implementations of primitive functions, such as trig functions * Support for WebAssembly compilation * More Lie group types, in particular Sim(3) * Support for constraints in our optimizer * Integration with ISPC About Fast symbolic computation, code generation, and nonlinear optimization for robotics symforce.org Topics python computer-vision cpp robotics optimization structure-from-motion motion-planning code-generation slam autonomous-vehicles symbolic-computation Resources Readme License Apache-2.0 license Stars 351 stars Watchers 12 watching Forks 43 forks Releases 2 v0.4.0 Latest May 24, 2022 + 1 release Contributors 21 * @aaron-skydio * @bradley-solliday-skydio * @hayk-skydio * @nathan-skydio * @nlbucki * @harrison-skydio * @ryan-skydio * @dominic-skydio * @william-smith-skydio * @samuel-wang-skydio * @eric-skydio + 10 contributors Languages * C++ 65.8% * Python 26.8% * Jinja 3.4% * Jupyter Notebook 3.3% * CMake 0.6% * Makefile 0.1% * (c) 2022 GitHub, Inc. * Terms * Privacy * Security * Status * Docs * Contact GitHub * Pricing * API * Training * Blog * About You can't perform that action at this time. You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.