This notebook demonstrates a simple 3D SLAM problem using the SL(4) group to represent poses. The scenario is as follows:
- A robot starts at the origin.
- The robot moves in a sequence of 5 poses.
- Odometry measurements are given as
BetweenFactorSL4factors. - A loop closure is added between pose 5 and pose 2.
We will:
- Define noise models.
- Create a factor graph representing the problem.
- Provide initial estimates for poses.
- Optimize the graph using Levenberg-Marquardt to find the most probable configuration.
try:
import google.colab
%pip install --quiet gtsam-develop
except ImportError:
passThe Special Linear Group SL(4)¶
In this example, we use the Special Linear Group SL(4) to represent the robot’s pose. SL(4) is the group of 4x4 matrices with determinant 1. In computer vision, 3D homographies are represented by 4x4 matrices, and the group of all invertible 4x4 real matrices is the General Linear Group GL(4). By constraining the determinant to 1, we obtain the SL(4) subgroup. This is analogous to how 2D homographies can be represented by elements of SL(3) to resolve the scale ambiguity.
import numpy as np
import gtsam
from gtsam import Values, NonlinearFactorGraph, LevenbergMarquardtOptimizer, noiseModel
# Import your custom SL4 class and bindings (assumed already wrapped)
from gtsam import SL4, PriorFactorSL4, BetweenFactorSL41. Setup Factor Graph and Priors¶
# Create the factor graph
graph = NonlinearFactorGraph()
# Prior factor at node 1
prior_sigmas = np.full(15, 0.3)
prior_noise = noiseModel.Diagonal.Sigmas(prior_sigmas)
graph.add(PriorFactorSL4(1, SL4(np.eye(4)), prior_noise))2. Define the Noise Model and Transformations¶
# Between factor noise
model_sigmas = np.full(15, 0.2)
model = noiseModel.Diagonal.Sigmas(model_sigmas)
# Define SL4 transformations (homography matrices)
H12 = np.array([
[1.0, 0.1, 0.0, 2.0],
[0.0, 1.0, 0.0, 3.0],
[0.0, 0.0, 1.0, 5.0],
[0.001, 0.002, 0.0, 1.0]
])
H23 = np.array([
[0.9, 0.2, 0.0, 1.5],
[0.1, 1.1, 0.0, -2.0],
[0.0, 0.0, 0.8, 4.0],
[0.002, 0.003, 0.0005, 1.0]
])
H34 = np.array([
[1.05, -0.1, 0.0, 3.0],
[0.2, 0.95, 0.0, 1.0],
[0.0, 0.0, 0.9, 2.5],
[0.0015, -0.001, 0.0003, 1.0]
])
H45 = np.array([
[0.98, 0.05, 0.0, -1.0],
[-0.05, 1.02, 0.0, 2.0],
[0.0, 0.0, 1.1, 0.5],
[0.0008, 0.0015, -0.0002, 1.0]
])
H52 = np.linalg.inv(H23 @ H34 @ H45)3. Calculate Ground Truth Poses¶
# Create SL4 objects
H12_SL4 = SL4(H12)
H23_SL4 = SL4(H23)
H34_SL4 = SL4(H34)
H45_SL4 = SL4(H45)
H52_SL4 = SL4(H52)
H1 = SL4(np.eye(4))
# Ground-truth poses
H2 = H1.compose(H12_SL4)
H3 = H2.compose(H23_SL4)
H4 = H3.compose(H34_SL4)
H5 = H4.compose(H45_SL4)
gt_poses = [H1, H2, H3, H4, H5]4. Add Odometry Factors¶
# Add odometry factors
graph.add(BetweenFactorSL4(1, 2, H12_SL4, model))
graph.add(BetweenFactorSL4(2, 3, H23_SL4, model))
graph.add(BetweenFactorSL4(3, 4, H34_SL4, model))
graph.add(BetweenFactorSL4(4, 5, H45_SL4, model))
graph.add(BetweenFactorSL4(5, 2, H52_SL4, model)) # loop closure5. Create Initial Estimates¶
# Create initial estimate by perturbing GT
initial = Values()
rng = np.random.default_rng(seed=42)
def random_noise_vector(dim=15):
return rng.uniform(low=-0.1, high=0.1, size=dim)
for i, gt_pose in enumerate(gt_poses, 1):
noise = random_noise_vector()
noisy_pose = gt_pose.compose(SL4.Expmap(noise))
initial.insert(i, noisy_pose)6. Optimize¶
# Optimize
params = gtsam.LevenbergMarquardtParams()
params.setRelativeErrorTol(1e-5)
params.setMaxIterations(100)
optimizer = LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()7. Results¶
print("Final result:")
result.print()
# Check accuracy
for i, gt_pose in enumerate(gt_poses, 1):
opt_pose = result.atSL4(i)
if not gt_pose.equals(opt_pose, 1e-5):
print(f"\033[1;31mPose {i} is outside tolerance!")
print("\033[1;32mSuccessfully optimized!\033[0m")Final result:
Values with 5 values:
Value 1: (gtsam::SL4)
1 -1.49358e-13 -5.67733e-13 -5.17202e-14
3.20484e-15 1 -6.02405e-16 -6.00672e-16
-1.08258e-12 -5.88179e-13 1 -1.88337e-13
1.36759e-11 7.42468e-12 2.58617e-11 1
Value 2: (gtsam::SL4)
1.00193 0.100193 6.35519e-12 2.00387
6.73137e-12 1.00193 1.05811e-11 3.0058
9.81407e-12 -2.13022e-13 1.00193 5.00967
0.00100193 0.00200387 2.94353e-11 1.00193
Value 3: (gtsam::SL4)
0.975878 0.337393 0.0010677 3.52341
0.113176 1.18408 0.00160155 1.0677
0.010677 0.0160155 0.85683 9.6093
0.00330987 0.00576558 0.00053385 1.06503
Value 4: (gtsam::SL4)
1.12318 0.22456 0.0020653 6.95044
0.365634 1.13859 0.00180304 2.6562
0.0295043 0.00464419 0.79219 12.0763
0.0063721 0.00417704 0.000818743 1.10745
Value 5: (gtsam::SL4)
1.06921 0.288659 0.000860931 6.12926
0.296354 1.1557 0.00141783 4.46083
0.0374381 0.0237526 0.848485 12.1583
0.00675841 0.00609308 0.000663101 1.08365
Successfully optimized!