Pose2 SLAM Example with Subgraph Solver (SPCG)

This example demonstrates 2D Pose SLAM using the SubgraphSolver (SPCG) - a specialized iterative linear solver that can be more efficient for large sparse problems.

Key Features:

Uses Levenberg-Marquardt as the outer optimization loop
Employs SubgraphSolver (SPCG) as the inner iterative linear solver
SPCG uses Preconditioned Conjugate Gradient with subgraph preconditioning
More memory efficient for large-scale SLAM problems

The problem setup is identical to the standard Pose2 SLAM example:

5 poses forming a square trajectory
Odometry measurements between consecutive poses
A loop closure constraint

import math
import numpy as np
import gtsam

Factor Graph Setup¶

# 1. Create a factor graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()

# 2a. Add a prior on the first pose, setting it to the origin
prior = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
graph.add(gtsam.PriorFactorPose2(1, prior, prior_noise))

Odometry and Loop Closure Factors¶

odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))
graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))
graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))
graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2.0, 0.0, math.pi/2), odometry_noise))

# 2c. Add the loop closure constraint
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(gtsam.BetweenFactorPose2(5, 1, gtsam.Pose2(0.0, 0.0, 0.0), model))

print("\nFactor Graph:")
print(graph)


Factor Graph:
NonlinearFactorGraph: size: 6

Factor 0: PriorFactor on 1
  prior mean:  (0, 0, 0)
  noise model: diagonal sigmas [0.3; 0.3; 0.1];

Factor 1: BetweenFactor(1,2)
  measured:  (2, 0, 1.57079633)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

Factor 2: BetweenFactor(2,3)
  measured:  (2, 0, 1.57079633)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

Factor 3: BetweenFactor(3,4)
  measured:  (2, 0, 1.57079633)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

Factor 4: BetweenFactor(4,5)
  measured:  (2, 0, 1.57079633)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

Factor 5: BetweenFactor(5,1)
  measured:  (0, 0, 0)
  noise model: diagonal sigmas [0.2; 0.2; 0.1];

Initial Estimate¶

initial_estimate = gtsam.Values()
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, 1.1))
initial_estimate.insert(3, gtsam.Pose2(2.1, 1.9, 2.8))
initial_estimate.insert(4, gtsam.Pose2(-0.3, 2.5, 4.2))
initial_estimate.insert(5, gtsam.Pose2(0.1, -0.7, 5.8))

print("\nInitial Estimate:")
print(initial_estimate)


Initial Estimate:
Values with 5 values:
Value 1: (gtsam::Pose2)
(0.5, 0, 0.2)

Value 2: (gtsam::Pose2)
(2.3, 0.1, 1.1)

Value 3: (gtsam::Pose2)
(2.1, 1.9, 2.8)

Value 4: (gtsam::Pose2)
(-0.3, 2.5, -2.08318531)

Value 5: (gtsam::Pose2)
(0.1, -0.7, -0.483185307)

Optimization with SubgraphSolver (SPCG)¶

The key difference from standard optimization is the use of SubgraphSolver as the iterative linear solver. This can be more efficient for large sparse problems.

parameters = gtsam.LevenbergMarquardtParams()
parameters.setVerbosity("ERROR")
parameters.setVerbosityLM("LAMBDA")

# Specify "ITERATIVE" to use an iterative linear solver.
parameters.setLinearSolverType("ITERATIVE")
parameters.setIterativeParams(gtsam.SubgraphSolverParameters())

optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, parameters)
result = optimizer.optimize()

Initial error: 130.17500174
newError: 5.78644755128
errorThreshold: 5.78644755128 > 0
absoluteDecrease: 124.388554189 >= 1e-05
relativeDecrease: 0.955548703868 >= 1e-05
newError: 0.00526196818429
errorThreshold: 0.00526196818429 > 0
absoluteDecrease: 5.78118558309 >= 1e-05
relativeDecrease: 0.999090639267 >= 1e-05
newError: 1.53140303232e-08
errorThreshold: 1.53140303232e-08 > 0
absoluteDecrease: 0.00526195287026 >= 1e-05
relativeDecrease: 0.999997089676 >= 1e-05
newError: 2.78539546447e-21
errorThreshold: 2.78539546447e-21 > 0
absoluteDecrease: 1.53140303232e-08 < 1e-05
relativeDecrease: 1 >= 1e-05
converged
errorThreshold: 2.78539546447e-21 <? 0
absoluteDecrease: 1.53140303232e-08 <? 1e-05
relativeDecrease: 1 <? 1e-05
iterations: 4 >? 100

print("\nFinal Result:")
print(result)
print(f"subgraph solver final error = {graph.error(result)}")


Final Result:
Values with 5 values:
Value 1: (gtsam::Pose2)
(3.23784789028e-15, 9.85867113204e-15, -8.54048365546e-16)

Value 2: (gtsam::Pose2)
(2, 1.50087829433e-12, 1.57079632679)

Value 3: (gtsam::Pose2)
(2, 2, 3.14159265359)

Value 4: (gtsam::Pose2)
(9.467927256e-12, 2, -1.57079632679)

Value 5: (gtsam::Pose2)
(3.80509879214e-15, 1.06998239513e-14, -8.33727794179e-16)


subgraph solver final error = 2.7853954644738e-21