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"],
    ))

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,
)

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,
)

result = optimizer.optimize(initial_values)

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

from symforce import geo
geo.Pose3.identity()

import sym
sym.Pose3.identity()

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:

0Likes

Written by

HackTech

View all posts by HackTech