Pose2 SLAM Stress Test

This stress test creates a large chain of poses to test GTSAM’s performance on open-loop SLAM problems. The test:

Creates N 3D poses with unit translation (1, 0, 0) between consecutive poses
Adds random noise to the x value of each pose’s translation
Creates a prior factor for the first pose
Adds N-1 between factors for consecutive poses
Runs optimization using Levenberg-Marquardt

This tests GTSAM’s ability to handle large sparse factor graphs efficiently.

import numpy as np
import random
import sys
import time
import gtsam

Stress Test Function¶

def test_gtsam(number_nodes):
    """
    Test GTSAM on a large chain of poses.
    
    Args:
        number_nodes: Number of poses to create in the chain
    """
    print(f"Creating stress test with {number_nodes} nodes...")
    
    # Generate poses with random noise
    poses = []
    for i in range(number_nodes):
        # Add random noise to x translation (between -0.05 and 0.05)
        r = random.uniform(0, 1)
        r = (r - 0.5) / 10 + i
        
        # Create 4x4 transformation matrix
        # Translation: (r, 0, 0), no rotation
        pose = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(r, 0, 0))
        poses.append(pose)
    
    # Create factor graph
    graph = gtsam.NonlinearFactorGraph()
    
    # Prior factor for the first pose
    prior_model = gtsam.noiseModel.Isotropic.Variance(6, 1e-4)
    first_pose = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0, 0, 0))
    graph.add(gtsam.PriorFactorPose3(0, first_pose, prior_model))
    
    # VO (Visual Odometry) noise model
    vo_covariance_model = gtsam.noiseModel.Isotropic.Variance(6, 1e-3)
    relative_motion = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))
    
    # Add between factors for consecutive poses
    for i in range(number_nodes - 1):
        graph.add(gtsam.BetweenFactorPose3(i, i + 1, relative_motion, vo_covariance_model))
    
    print(f"Created factor graph with {graph.size()} factors")
    
    # Initial values
    initial = gtsam.Values()
    for i in range(number_nodes):
        initial.insert(i, poses[i])
    
    # Optimization parameters
    params = gtsam.LevenbergMarquardtParams()
    params.setVerbosity("ERROR")
    params.setOrderingType("METIS")
    params.setLinearSolverType("MULTIFRONTAL_CHOLESKY")
    
    # Run optimization
    print("Starting optimization...")
    start_time = time.time()
    
    optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
    result = optimizer.optimize()
    
    end_time = time.time()
    optimization_time = end_time - start_time
    
    print(f"Optimization completed in {optimization_time:.3f} seconds")
    print(f"Initial error: {graph.error(initial):.6f}")
    print(f"Final error: {graph.error(result):.6f}")
    
    return result, optimization_time

Main Execution¶

def main():
    """Main function to run the stress test."""
    # Default number of nodes if not specified
    number_nodes = 1000
    
    # Check if number of nodes is provided as command line argument
    if len(sys.argv) > 1:
        try:
            number_nodes = int(sys.argv[1])
        except ValueError:
            print("Invalid number of nodes. Using default value of 1000.")
    
    print(f"Running Pose2 SLAM Stress Test with {number_nodes} nodes")
    
    # Set random seed for reproducibility
    random.seed(42)
    np.random.seed(42)
    
    # Run the stress test
    result, opt_time = test_gtsam(number_nodes)
    
    print("\nStress test completed successfully!")
    print(f"Performance: {number_nodes / opt_time:.1f} nodes per second")

main()

Invalid number of nodes. Using default value of 1000.
Running Pose2 SLAM Stress Test with 1000 nodes
Creating stress test with 1000 nodes...
Created factor graph with 1000 factors
Starting optimization...
Initial error: 810.02902132Optimization completed in 0.020 seconds
Initial error: 810.029021
Final error: 0.000000

Stress test completed successfully!
Performance: 49290.2 nodes per second

newError: 7.18522916506e-11
errorThreshold: 7.18522916506e-11 > 0
absoluteDecrease: 810.02902132 >= 1e-05
relativeDecrease: 1 >= 1e-05
newError: 1.1004643035e-17
errorThreshold: 1.1004643035e-17 > 0
absoluteDecrease: 7.1852280646e-11 < 1e-05
relativeDecrease: 0.999999846844 >= 1e-05
converged
errorThreshold: 1.1004643035e-17 <? 0
absoluteDecrease: 7.1852280646e-11 <? 1e-05
relativeDecrease: 0.999999846844 <? 1e-05
iterations: 2 >? 100

Interactive Example¶

You can run the stress test with different numbers of nodes to see how GTSAM performs with increasing problem size.

test_sizes = [100, 500, 1000, 2000]

print("Running stress tests with different problem sizes:")
print("=" * 50)

for size in test_sizes:
    print(f"\nTesting with {size} nodes:")
    random.seed(42)  # Ensure reproducible results
    np.random.seed(42)
    
    result, opt_time = test_gtsam(size)
    print(f"Rate: {size / opt_time:.1f} nodes/second")

Running stress tests with different problem sizes:
==================================================

Testing with 100 nodes:
Creating stress test with 100 nodes...
Created factor graph with 100 factors
Starting optimization...
Optimization completed in 0.007 seconds
Initial error: 73.785267
Final error: 0.000000
Rate: 14021.2 nodes/second

Testing with 500 nodes:
Creating stress test with 500 nodes...
Initial error: 73.7852667959
Created factor graph with 500 factors
Starting optimization...
newError: 3.19648178005e-14
errorThreshold: 3.19648178005e-14 > 0
absoluteDecrease: 73.7852667959 >= 1e-05
relativeDecrease: 1 >= 1e-05
newError: 3.26892942612e-25
errorThreshold: 3.26892942612e-25 > 0
absoluteDecrease: 3.19648178002e-14 < 1e-05
relativeDecrease: 0.99999999999 >= 1e-05
converged
errorThreshold: 3.26892942612e-25 <? 0
absoluteDecrease: 3.19648178002e-14 <? 1e-05
relativeDecrease: 0.99999999999 <? 1e-05
iterations: 2 >? 100
Initial error: 403.93736772
newError: 1.44414083181e-12
errorThreshold: 1.44414083181e-12 > 0
absoluteDecrease: 403.93736772 >= 1e-05
relativeDecrease: 1 >= 1e-05
newError: 1.00814883652e-20
errorThreshold: 1.00814883652e-20 > 0
absoluteDecrease: 1.44414082173e-12 < 1e-05
relativeDecrease: 0.999999993019 >= 1e-05
converged
errorThreshold: 1.00814883652e-20 <? 0
absoluteDecrease: 1.44414082173e-12 <? 1e-05
relativeDecrease: 0.999999993019 <? 1e-05
iterations: 2 >? 100
Optimization completed in 0.023 seconds
Initial error: 403.937368
Final error: 0.000000
Rate: 21542.6 nodes/second

Testing with 1000 nodes:
Creating stress test with 1000 nodes...
Created factor graph with 1000 factors
Starting optimization...
Initial error: 810.02902132
newError: 7.18522916506e-11
errorThreshold: 7.18522916506e-11 > 0
absoluteDecrease: 810.02902132 >= 1e-05
relativeDecrease: 1 >= 1e-05
newError: 1.1004643035e-17
errorThreshold: 1.1004643035e-17 > 0
absoluteDecrease: 7.1852280646e-11 < 1e-05
relativeDecrease: 0.999999846844 >= 1e-05
converged
errorThreshold: 1.1004643035e-17 <? 0
absoluteDecrease: 7.1852280646e-11 <? 1e-05
relativeDecrease: 0.999999846844 <? 1e-05
iterations: 2 >? 100
Optimization completed in 0.041 seconds
Initial error: 810.029021
Final error: 0.000000
Rate: 24680.9 nodes/second

Testing with 2000 nodes:
Creating stress test with 2000 nodes...
Created factor graph with 2000 factors
Starting optimization...
Initial error: 1629.41004549
newError: 6.70560309189e-11
errorThreshold: 6.70560309189e-11 > 0
absoluteDecrease: 1629.41004549 >= 1e-05
relativeDecrease: 1 >= 1e-05
newError: 1.51418327449e-16
errorThreshold: 1.51418327449e-16 > 0
absoluteDecrease: 6.70558795006e-11 < 1e-05
relativeDecrease: 0.999997741913 >= 1e-05
converged
errorThreshold: 1.51418327449e-16 <? 0
absoluteDecrease: 6.70558795006e-11 <? 1e-05
relativeDecrease: 0.999997741913 <? 1e-05
iterations: 2 >? 100
Optimization completed in 0.040 seconds
Initial error: 1629.410045
Final error: 0.000000
Rate: 50276.6 nodes/second