Robot 3D Localization#
Demonstrates solving a 3D localization problem with SymForce. A robot moving in 3D performs scan matching and gets relative translation constraints to landmarks in the environment. It also has odometry constraints between its poses. The goal is to estimate the trajectory of the robot given known landmarks and noisy measurements.
The main entry point for the symbolic problem. In this file, we:
Define the symbolic residual functions we’ll need
Build a symbolic factor graph for the full problem, and turn that into a single combined residual vector
Build a Python
Valueswith numerical inputs to the problem - this includes the sampled measurements as well as initial guesses for the optimized variables (the poses)
Run the optimization from Python, creating an
Optimizerand calling it with the created factors and
Generate a linearization function to solve the same problem, but in C++
Constructs and optimizes a factor graph in C++, with the linearization functions we’ve generated. The size of the problem (the number of poses, landmarks, and observations) can be changed at runtime.
Runs the optimization from C++, using a fixed-size linearization function for the entire problem. This is significantly faster than the dynamic size version, but requires that the number of poses and landmarks is known at codegen time.
Contains helper functions to build up the C++
sym::Values for the problem and create default params for the
Contains helper functions for visualizing the optimization problem