symforce.examples.robot_3d_localization.robot_3d_localization module

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.

build_values(num_poses)[source]
Parameters:

num_poses (int)

Return type:

Tuple[Values, int]

matching_residual(world_T_body, world_t_landmark, body_t_landmark, sigma)[source]

Residual from a relative translation mesurement of a 3D pose to a landmark.

Parameters:

  • world_T_body (Pose3) – 3D pose of the robot in the world frame

  • world_t_landmark (Matrix31) – World location of the landmark

  • body_t_landmark (Matrix31) – Measured body-frame location of the landmark

  • sigma (float) – Isotropic standard deviation of the measurement [m]

Return type:

Matrix31

odometry_residual(world_T_a, world_T_b, a_T_b, diagonal_sigmas, epsilon)[source]

Residual on the relative pose between two timesteps of the robot.

Parameters:

  • world_T_a (Pose3) – First pose in the world frame

  • world_T_b (Pose3) – Second pose in the world frame

  • a_T_b (Pose3) – Relative pose measurement between the poses

  • diagonal_sigmas (Matrix61) – Diagonal standard deviation of the tangent-space error

  • epsilon (float) – Small number for singularity handling

Return type:

Matrix61

build_factors(num_poses, num_landmarks)[source]

Build factors for a problem of the given dimensionality.

Parameters:

  • num_poses (int)

  • num_landmarks (int)

Return type:

Iterator[Factor]

build_residual(num_poses, num_landmarks, values)[source]
Parameters:
Return type:

Matrix

main()[source]
Return type:

None

build_codegen_object(num_poses, config=None)[source]

Create Codegen object for the linearization function

Parameters:
Return type:

Codegen

generate_matching_residual_code(output_dir=None, print_code=False)[source]

Generate C++ code for the matching residual function. A C++ Factor can then be constructed and optimized from this function without any Python dependency.

Parameters:

  • output_dir (Path | None)

  • print_code (bool)

Return type:

None

generate_odometry_residual_code(output_dir=None, print_code=False)[source]

Generate C++ code for the odometry residual function. A C++ Factor can then be constructed and optimized from this function without any Python dependency.

Parameters:

  • output_dir (Path | None)

  • print_code (bool)

Return type:

None

generate(output_dir)[source]
Parameters:

output_dir (Path)

Return type:

None