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 gtsamFactor 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