Fisheye Camera SfM Example

A visualSLAM example for the structure-from-motion problem on a simulated dataset. This version uses a fisheye camera model and a GaussNewton solver to solve the graph in one batch.

Problem Description¶

A structure-from-motion example with landmarks:

The landmarks form a 10 meter cube
The robot rotates around the landmarks, always facing towards the cube

This example demonstrates:

Camera observations of landmarks (pixel coordinates) stored as Point2 (x, y)
Each variable (poses and landmarks) identified with unique keys using Symbols
Projection factors to model camera’s landmark observations
Prior factors to initialize robot location
Fisheye camera model (Cal3Fisheye) with distortion parameters

import numpy as np

import gtsam
from gtsam import symbol_shorthand

L = symbol_shorthand.L
X = symbol_shorthand.X

from gtsam.examples import SFMdata

from gtsam import (Cal3Fisheye, GaussNewtonOptimizer, GaussNewtonParams,
                   GenericProjectionFactorCal3Fisheye,
                   NonlinearFactorGraph, PinholeCameraCal3Fisheye,
                   Point3, Pose3, PriorFactorPoint3, PriorFactorPose3,
                   Rot3, Values)

Camera Setup, Data Generation¶

K = Cal3Fisheye(
    278.66, 278.48, 0.0, 319.75, 241.96,
    -0.013721808247486035, 0.020727425669427896,
    -0.012786476702685545, 0.0025242267320687625
)

# Define the camera observation noise model, 1 pixel stddev
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)

# Create the set of ground-truth landmarks
points = SFMdata.createPoints()

# Create the set of ground-truth poses
poses = SFMdata.createPoses()

Factor Graph Setup¶

# Create a Factor Graph and Values to hold the new data
graph = NonlinearFactorGraph()
initial_estimate = Values()

# Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
pose_prior = gtsam.noiseModel.Diagonal.Sigmas(
    np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])
)
graph.add(PriorFactorPose3(X(0), poses[0], pose_prior))

# Add a prior on landmark l0
point_prior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
graph.add(PriorFactorPoint3(L(0), points[0], point_prior))

# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
delta_point = Point3(-0.25, 0.20, 0.15)
for j, point in enumerate(points):
    initial_estimate.insert(L(j), point + delta_point)

Add observations, initial estimates¶

for i, pose in enumerate(poses):
    # Add factors for each landmark observation
    for j, point in enumerate(points):
        camera = PinholeCameraCal3Fisheye(pose, K)
        measurement = camera.project(point)
        factor = GenericProjectionFactorCal3Fisheye(
            measurement, measurement_noise, X(i), L(j), K
        )
        graph.add(factor)

    # Add an initial guess for the current pose
    # Intentionally initialize the variables off from the ground truth
    delta_pose = Pose3(
        Rot3.Rodrigues(-0.1, 0.2, 0.25),
        Point3(0.05, -0.10, 0.20)
    )
    initial_estimate.insert(X(i), pose.compose(delta_pose))

# Configure and run the optimizer
params = GaussNewtonParams()
params.setVerbosity("TERMINATION")
params.setMaxIterations(10000)

print("Optimizing the factor graph")
optimizer = GaussNewtonOptimizer(graph, initial_estimate, params)
result = optimizer.optimize()
print("Optimization complete")

Optimizing the factor graph
converged
errorThreshold: Optimization complete
4.55607e-21 <? 0
absoluteDecrease: 6.26303251472e-08 <? 1e-05
relativeDecrease: 1 <? 1e-05
iterations: 4 >? 10000

Results¶

print(f"initial error={graph.error(initial_estimate)}")
print(f"final error={graph.error(result)}")

# Save the factor graph visualization
try:
    graph.saveGraph("python/gtsam/examples/fisheye_batch.dot", result)
    print("Saved graph to python/gtsam/examples/fisheye_batch.dot")
except Exception as e:
    print(f"Could not save graph: {e}")

initial error=152507.22412516878
final error=4.5560711028681596e-21
Saved graph to python/gtsam/examples/fisheye_batch.dot