factorama 1.1.1

Factor graph optimization library with Python bindings

Factorama Python

A Python interface to the Factorama C++ factor graph optimization library. Factorama provides a simple and efficient framework for factor graph-based optimization, perfect for small to medium-scale SLAM, calibration, and structure-from-motion problems.

Purpose

Factor graphs are a powerful framework for non-linear optimization problems commonly found in robotics and computer vision. Factorama's Python bindings make it easy to:

• Build factor graphs with poses, landmarks, and custom variables
• Add constraints through various factor types (bearing observations, priors, relative constraints)
• Optimize using Gauss-Newton or Levenberg-Marquardt algorithms
• Visualize results with built-in plotting capabilities

Perfect for prototyping SLAM algorithms, camera calibration, bundle adjustment, and sensor fusion applications.

Installation

pip install factorama

Dependencies

Factorama requires:

• numpy - For matrix operations and numerical arrays
• matplotlib - For visualization and plotting (optional, but recommended)

Examples

Basic Robot Localization

from factorama import FactorGraph, PoseVariable, LandmarkVariable, PosePositionPriorFactor, PoseOrientationPriorFactor, BearingObservationFactor

# Create factor graph and add variables
graph = FactorGraph()
robot_pose = PoseVariable(1, initial_pos, initial_dcm)
landmark = LandmarkVariable(2, landmark_pos)
graph.add_variable(robot_pose)
graph.add_variable(landmark)

# Add factors
position_prior = PosePositionPriorFactor(100, robot_pose, prior_pos, 0.1)
orientation_prior = PoseOrientationPriorFactor(101, robot_pose, prior_dcm, 0.1)
bearing_factor = BearingObservationFactor(200, robot_pose, landmark, bearing_obs, 0.01)
graph.add_factor(position_prior)
graph.add_factor(orientation_prior)
graph.add_factor(bearing_factor)

# Optimize
graph.finalize_structure()
optimizer = SparseOptimizer()
settings = OptimizerSettings()
settings.method = OptimizerMethod.LevenbergMarquardt
optimizer.setup(graph, settings)
optimizer.optimize()

→ Complete runnable example: examples/basic_localization.py

Bundle Adjustment with Multiple Views

from factorama import FactorGraph, PoseVariable, LandmarkVariable, PosePositionPriorFactor, PoseOrientationPriorFactor, BearingObservationFactor, PlotFactorGraph

# Create factor graph with multiple poses and landmarks
graph = FactorGraph()

# Add camera poses
poses = []
for i in range(3):
    pose = PoseVariable(i + 1, pose_pos, pose_dcm)
    poses.append(pose)
    graph.add_variable(pose)

# Add landmarks and priors
landmarks = []
for i, pos in enumerate(landmark_positions):
    landmark = LandmarkVariable(10 + i, pos)
    landmarks.append(landmark)
    graph.add_variable(landmark)

# Add bearing observations between all pose-landmark pairs
for pose in poses:
    for landmark in landmarks:
        factor = BearingObservationFactor(factor_id, pose, landmark, bearing, 0.01)
        graph.add_factor(factor)

# Optimize and visualize
graph.finalize_structure()
optimizer.optimize()
PlotFactorGraph(graph)

→ Complete runnable example: examples/bundle_adjustment.py

Inverse Depth Parameterization

import numpy as np
from factorama import FactorGraph, PoseVariable, InverseRangeVariable, InverseRangeBearingFactor, SparseOptimizer, OptimizerSettings

graph = FactorGraph()

# Camera pose
camera_pose = PoseVariable(1, camera_pos, camera_dcm)
graph.add_variable(camera_pose)

# Inverse depth landmark (origin, bearing direction, initial range)
origin_pos = np.array([0.0, 0.0, 0.0])
bearing_direction = np.array([1.0, 0.0, 0.0])
initial_range = 10.0
inv_depth_landmark = InverseRangeVariable(2, origin_pos, bearing_direction, initial_range)
graph.add_variable(inv_depth_landmark)

# Bearing observation factor
bearing_obs = np.array([1.0, 0.0, 0.0])
bearing_factor = InverseRangeBearingFactor(100, camera_pose, inv_depth_landmark, bearing_obs, 0.01)
graph.add_factor(bearing_factor)

# Optimize
graph.finalize_structure()
optimizer = SparseOptimizer()
settings = OptimizerSettings()
optimizer.setup(graph, settings)
optimizer.optimize()

print(f"Final landmark position: {inv_depth_landmark.pos_W()}")

Variables

PoseVariable

Represents SE(3) poses with 6 DOF (position + orientation)

from factorama import PoseVariable

# From position and rotation matrix
pose = PoseVariable(id, position_3d, rotation_matrix_3x3)

# Alternative: From SE(3) vector [tx, ty, tz, rx, ry, rz]
pose = PoseVariable(id, pose_vector)

# Access position and rotation
position = pose.pos_W()
rotation_matrix = pose.dcm_CW()

LandmarkVariable

Represents 3D landmarks with 3 DOF

from factorama import LandmarkVariable

landmark = LandmarkVariable(id, position_3d)
position = landmark.pos_W()

GenericVariable

Represents arbitrary N-dimensional variables

from factorama import GenericVariable

generic = GenericVariable(id, initial_vector)

RotationVariable

Represents SO(3) rotations with 3 DOF

from factorama import RotationVariable

rotation = RotationVariable(id, rotation_matrix_3x3)
dcm = rotation.dcm_AB()

InverseRangeVariable

Represents landmarks using inverse depth parameterization (1 DOF)

from factorama import InverseRangeVariable

inv_range = InverseRangeVariable(id, origin_pos, bearing_direction, initial_range)
position = inv_range.pos_W()
inverse_depth = inv_range.inverse_range()

Factors

Prior Factors

• GenericPriorFactor: Prior constraints on any variable type
• PosePositionPriorFactor: Position-only prior for poses
• PoseOrientationPriorFactor: Orientation-only prior for poses
• RotationPriorFactor: Prior constraints on rotation variables

Observation Factors

• BearingObservationFactor: 3D bearing measurements from poses to landmarks
• InverseRangeBearingFactor: Bearing constraints with inverse depth parameterization
• BearingProjectionFactor2D: 2D bearing projections

Relative Constraint Factors

• GenericBetweenFactor: Relative constraints between any variable types
• PosePositionBetweenFactor: Position-only relative constraints between poses
• PoseOrientationBetweenFactor: Orientation-only relative constraints between poses

Optimization

from factorama import SparseOptimizer, OptimizerSettings, OptimizerMethod

# Create optimizer
optimizer = SparseOptimizer()

# Configure settings
settings = OptimizerSettings()
settings.method = OptimizerMethod.LevenbergMarquardt  # or GaussNewton
settings.max_num_iterations = 100
settings.step_tolerance = 1e-6
settings.residual_tolerance = 1e-6
settings.verbose = True

# Setup and optimize
optimizer.setup(factor_graph, settings)
optimizer.optimize()

# Access results
print(f"Iterations: {optimizer.current_stats.current_iteration}")
print(f"Final chi2: {optimizer.current_stats.chi2}")

Utility Functions

from factorama import ExpMapSO3, LogMapSO3, PlotFactorGraph

# SO(3) exponential and logarithm maps
rotation_matrix = ExpMapSO3(rotation_vector)
rotation_vector = LogMapSO3(rotation_matrix)

# Factor graph visualization
PlotFactorGraph(graph, plot_3d=True)