Batch optimizers solve their problem in one shot: build the whole graph, then call optimize() once. Real robots don’t get to wait until the end of the mission to know where they are -- measurements arrive continuously, and the estimate has to update incrementally.
This notebook is a first taste of that: a structure-from-motion problem where 8 cameras orbit a 10-meter cube of landmarks, always facing the center. As each new camera pose and its landmark observations arrive, we feed them to NonlinearISAM, which relinearizes and reorders the problem every few updates instead of resolving from scratch -- the classic “incremental SLAM” pattern.
GTSAM Copyright 2010-2026, Georgia Tech Research Corporation, Atlanta, Georgia 30332-0415 All Rights Reserved
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
See LICENSE for the license information
try:
import google.colab
%pip install --quiet gtsam-develop
except ImportError:
passimport numpy as np
import matplotlib.pyplot as plt
import gtsam
import gtsam.utils.plot as gtsam_plot
from gtsam.examples import SFMdata
from gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2,
NonlinearFactorGraph, NonlinearISAM, Pose3,
PriorFactorPoint3, PriorFactorPose3, Rot3,
PinholeCameraCal3_S2, Values, Point3)
from gtsam.symbol_shorthand import X, L1. Camera and scene setup¶
The scene comes from the shared SFMdata helper module: 8 ground-truth landmarks forming a 10-meter cube, and 8 ground-truth camera poses circling it. K is a simple pinhole calibration, and camera_noise models about one pixel of measurement noise.
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
camera_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses()2. Set up incremental ISAM¶
NonlinearISAM(reorderInterval=3) relinearizes and reorders the variables every 3 updates, rather than on every single one -- a tradeoff between staying close to the true nonlinear solution and not paying the reordering cost too often.
isam = NonlinearISAM(reorderInterval=3)
graph = NonlinearFactorGraph()
initial_estimate = Values()3. Feed observations incrementally¶
For each camera pose, we project every landmark into the image to get a (simulated) pixel measurement, and add a GenericProjectionFactorCal3_S2 per observation. The initial guesses -- for both poses and landmarks -- are deliberately perturbed away from ground truth.
The first frame is special: since iSAM needs each landmark observed at least twice before it becomes well constrained, we don’t call isam.update() yet. Instead we add priors on pose x0 (to fix the coordinate frame) and landmark l0 (to fix the scale), plus initial guesses for every landmark. From frame 1 onward, each new pose’s factors are handed to isam.update(), and we read back the current estimate.
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
# Add factors for each landmark observation
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(
measurement, camera_noise, X(i), L(j), K)
graph.push_back(factor)
# Intentionally initialize the variables off from the ground truth
noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25),
t=Point3(0.05, -0.10, 0.20))
initial_xi = pose.compose(noise)
# Add an initial guess for the current pose
initial_estimate.insert(X(i), initial_xi)
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM.
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Add a prior on landmark l0
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j] + noise
initial_estimate.insert(L(j), initial_lj)
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print('Current estimate: ')
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()**************************************************
Frame 1:
Current estimate:
Values with 10 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
9.99996;
9.99995;
9.99998
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-12.8291;
10.3572;
10.376
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-12.4628;
-11.5675;
10.3645
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
9.98338;
-11.2238;
9.85452
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
9.49906;
9.94737;
-10.8915
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-11.806;
10.1893;
-11.5028
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-11.9203;
-10.5771;
-10.774
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
8.77424;
-10.3664;
-10.6539
]
Value x0: (gtsam::Pose3)
R: [
0.0104087, -0.00870685, -0.999908;
0.99994, -0.00329421, 0.0104377;
-0.00338479, -0.999957, 0.00867204
]
t: 29.9994 -0.00118953 -0.000751309
Value x1: (gtsam::Pose3)
R: [
-0.683294, -0.0178895, -0.729924;
0.730038, -0.0336979, -0.682575;
-0.012386, -0.999272, 0.0360856
]
t: 21.2424 21.5775 -0.99152
**************************************************
Frame 2:
Current estimate:
Values with 11 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
9.99998;
9.99998;
9.99998
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-11.9967;
9.91545;
10.0246
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-11.8268;
-11.7644;
9.95286
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.044;
-11.393;
9.78106
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
8.97631;
9.71935;
-10.9304
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-11.568;
9.92191;
-11.4467
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-11.4585;
-10.7162;
-11.082
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
8.98789;
-10.4202;
-10.733
]
Value x0: (gtsam::Pose3)
R: [
0.00374741, -0.00584062, -0.999976;
0.999993, -0.000288842, 0.00374916;
-0.000310732, -0.999983, 0.00583949
]
t: 29.9996 -0.000387572 -0.00054771
Value x1: (gtsam::Pose3)
R: [
-0.696357, -0.00664829, -0.717664;
0.717688, -0.0109985, -0.696278;
-0.00326417, -0.999917, 0.0124303
]
t: 20.8367 21.64 -0.277574
Value x2: (gtsam::Pose3)
R: [
-0.999874, 0.013676, -0.00806307;
0.00795185, -0.00816706, -0.999935;
-0.013741, -0.999873, 0.00805728
]
t: -1.63593 30.5496 -0.222778
**************************************************
Frame 3:
Current estimate:
Values with 12 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
10
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.97696;
9.70132;
10.1426
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.2879;
-10.5526;
9.721
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
9.86576;
-10.2901;
10.0304
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.1409;
9.65675;
-9.80039
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.81776;
9.71664;
-9.84937
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.75746;
-9.98177;
-10.3888
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.1327;
-10.1179;
-9.97183
]
Value x0: (gtsam::Pose3)
R: [
-0.00668957, 0.000932254, -0.999977;
0.999974, 0.00285256, -0.00668688;
0.00284626, -0.999995, -0.000951311
]
t: 30 -2.9699e-05 2.43528e-06
Value x1: (gtsam::Pose3)
R: [
-0.70804, 0.014962, -0.706013;
0.706157, 0.00850201, -0.708004;
-0.00459061, -0.999852, -0.0165853
]
t: 21.1437 20.9244 0.536878
Value x2: (gtsam::Pose3)
R: [
-0.999973, 0.00527312, -0.00514657;
0.00519818, 0.00982042, -0.999938;
-0.00522225, -0.999938, -0.00984756
]
t: 0.272857 29.7215 0.365141
Value x3: (gtsam::Pose3)
R: [
-0.717696, 0.0500379, 0.694556;
-0.694829, 0.0145604, -0.719028;
-0.0460916, -0.998641, 0.0243178
]
t: -20.974 20.928 -0.628741
**************************************************
Frame 4:
Current estimate:
Values with 13 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
10
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.2175;
10.0958;
10.0266
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.98435;
-10.3532;
9.57353
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.0266;
-10.1551;
9.8945
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.3771;
9.61199;
-9.7331
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.33863;
9.27402;
-9.90671
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.42369;
-9.88269;
-10.2183
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.2931;
-9.9971;
-9.94775
]
Value x0: (gtsam::Pose3)
R: [
-0.00547431, 0.00270639, -0.999981;
0.999984, 0.00137513, -0.0054706;
0.0013603, -0.999995, -0.00271387
]
t: 30 -2.22569e-05 -6.81754e-06
Value x1: (gtsam::Pose3)
R: [
-0.708029, 0.0174675, -0.705967;
0.706048, -0.00204497, -0.708161;
-0.0138135, -0.999845, -0.010885
]
t: 21.2234 20.875 0.346871
Value x2: (gtsam::Pose3)
R: [
-0.999867, 0.0162812, -0.000743803;
0.000759469, 0.000956036, -0.999999;
-0.0162805, -0.999867, -0.000968274
]
t: 0.279827 29.6576 0.172535
Value x3: (gtsam::Pose3)
R: [
-0.715639, 0.0364834, 0.697517;
-0.697016, 0.0271085, -0.716543;
-0.0450506, -0.998967, 0.00602961
]
t: -20.803 20.8359 0.0789155
Value x4: (gtsam::Pose3)
R: [
-0.00205907, 0.00183112, 0.999996;
-0.998839, 0.0481298, -0.00214482;
-0.0481335, -0.998839, 0.00172989
]
t: -28.9094 -0.628466 0.310697
**************************************************
Frame 5:
Current estimate:
Values with 14 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
10
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.3294;
10.158;
9.82264
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.0218;
-10.1264;
9.69102
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.1683;
-10.0991;
9.81209
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.4567;
9.59648;
-9.69058
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.05738;
9.2479;
-9.89342
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-8.92256;
-9.90333;
-10.0206
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.3034;
-9.93371;
-9.97587
]
Value x0: (gtsam::Pose3)
R: [
-0.00486548, 0.00321872, -0.999983;
0.999988, 0.000820143, -0.00486286;
0.000804477, -0.999994, -0.00322267
]
t: 30 -1.89292e-05 -9.74907e-06
Value x1: (gtsam::Pose3)
R: [
-0.708817, 0.0186411, -0.705146;
0.705168, -0.00650984, -0.709011;
-0.0178071, -0.999805, -0.00853083
]
t: 21.2261 20.8675 0.264764
Value x2: (gtsam::Pose3)
R: [
-0.999721, 0.0231754, 0.0045882;
-0.00478936, -0.00863168, -0.999951;
-0.0231346, -0.999694, 0.00874026
]
t: 0.195246 29.5786 -0.117567
Value x3: (gtsam::Pose3)
R: [
-0.713124, 0.0416573, 0.699799;
-0.699337, 0.0272156, -0.714274;
-0.0488002, -0.998761, 0.00972435
]
t: -20.6541 20.708 0.0109808
Value x4: (gtsam::Pose3)
R: [
-0.00523291, 0.00388494, 0.999979;
-0.999191, 0.0398587, -0.00538363;
-0.0398787, -0.999198, 0.00367322
]
t: -28.7048 -0.426185 0.395639
Value x5: (gtsam::Pose3)
R: [
0.703305, 0.000669088, 0.710888;
-0.70993, 0.052569, 0.702307;
-0.0369008, -0.998617, 0.037447
]
t: -19.7723 -20.6982 -0.584802
**************************************************
Frame 6:
Current estimate:
Values with 15 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
9.99999
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.0219;
9.92921;
9.99582
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.169;
-10.1261;
10.0072
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.1637;
-9.96976;
10.0201
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
9.98976;
9.98664;
-9.94795
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.85103;
10.0035;
-9.86214
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.64164;
-9.67932;
-9.77278
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.0291;
-9.97891;
-9.97128
]
Value x0: (gtsam::Pose3)
R: [
-0.000177607, -0.00159482, -0.999999;
1, 0.000892652, -0.000179031;
0.000892936, -0.999998, 0.00159466
]
t: 30 -2.32307e-06 7.54851e-06
Value x1: (gtsam::Pose3)
R: [
-0.70921, -0.0013554, -0.704996;
0.704997, -0.000423707, -0.709211;
0.000662551, -0.999999, 0.00125605
]
t: 21.1492 21.217 0.00448664
Value x2: (gtsam::Pose3)
R: [
-0.99999, 0.00122015, 0.00435425;
-0.00435836, -0.00338973, -0.999985;
-0.00120537, -0.999994, 0.00339502
]
t: -0.071174 29.885 -0.0535718
Value x3: (gtsam::Pose3)
R: [
-0.704649, 0.00599783, 0.709531;
-0.709539, 0.000910294, -0.704665;
-0.00487234, -0.999982, 0.00361426
]
t: -21.1225 21.048 -0.0176301
Value x4: (gtsam::Pose3)
R: [
0.00323515, 0.00198442, 0.999993;
-0.999977, 0.00598982, 0.00322321;
-0.00598338, -0.99998, 0.00200375
]
t: -29.7666 -0.0812114 0.098582
Value x5: (gtsam::Pose3)
R: [
0.706042, -0.0110079, 0.708085;
-0.708169, -0.00886715, 0.705988;
-0.00149277, -0.9999, -0.0140561
]
t: -21.0355 -21.006 0.604511
Value x6: (gtsam::Pose3)
R: [
0.9999, -0.0135399, 0.00416021;
-0.00423575, -0.00555538, 0.999976;
-0.0135165, -0.999893, -0.00561217
]
t: 0.513276 -29.6569 0.734982
**************************************************
Frame 7:
Current estimate:
Values with 16 values:
Value l0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10;
10;
9.99999
]
Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.151;
9.84668;
9.97018
]
Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-10.3024;
-10.3693;
9.90461
]
Value l3: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.0181;
-10.108;
10.0908
]
Value l4: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.0063;
9.9282;
-10.0053
]
Value l5: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.8856;
9.9648;
-9.94954
]
Value l6: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
-9.65268;
-9.68721;
-9.85196
]
Value l7: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)
[
10.107;
-9.90484;
-9.88594
]
Value x0: (gtsam::Pose3)
R: [
-0.00140292, -0.000822064, -0.999999;
0.999998, 0.00111816, -0.00140384;
0.00111931, -0.999999, 0.000820494
]
t: 30 -7.26208e-06 5.11837e-06
Value x1: (gtsam::Pose3)
R: [
-0.708757, 0.00307172, -0.705446;
0.705449, -6.04046e-06, -0.708761;
-0.00218138, -0.999995, -0.00216266
]
t: 21.1763 21.1945 0.0700789
Value x2: (gtsam::Pose3)
R: [
-0.999981, 0.00457838, 0.00416421;
-0.00418486, -0.00450995, -0.999981;
-0.00455951, -0.999979, 0.00452903
]
t: -0.0971033 29.9111 -0.126252
Value x3: (gtsam::Pose3)
R: [
-0.703393, 0.0119506, 0.710701;
-0.710741, 0.00120685, -0.703453;
-0.00926438, -0.999928, 0.00764487
]
t: -21.2646 21.0034 -0.170016
Value x4: (gtsam::Pose3)
R: [
0.00720172, 0.0103087, 0.999921;
-0.99993, 0.00946178, 0.00710424;
-0.00938779, -0.999902, 0.0103761
]
t: -29.8864 -0.275862 -0.165907
Value x5: (gtsam::Pose3)
R: [
0.707702, -0.00881747, 0.706456;
-0.706511, -0.00787646, 0.707658;
-0.000675385, -0.99993, -0.0118038
]
t: -21.0699 -21.143 0.554305
Value x6: (gtsam::Pose3)
R: [
0.999971, -0.00599312, 0.00476305;
-0.00479728, -0.00571087, 0.999972;
-0.00596575, -0.999966, -0.00573945
]
t: 0.436101 -29.7856 0.775382
Value x7: (gtsam::Pose3)
R: [
0.711029, -0.0250445, -0.702716;
0.703032, 0.0061032, 0.711132;
-0.0135212, -0.999668, 0.0219467
]
t: 21.3687 -20.7842 -0.133853
4. Visualize the final estimate¶
The printed Values above are hard to picture -- 8 poses and 8 landmarks as raw numbers. Plotting the final iSAM estimate in 3D shows the payoff directly: the recovered landmarks should form roughly a cube, and the recovered camera poses should trace out roughly a circle around it, despite every pose and landmark having started from a deliberately wrong initial guess.
gtsam_plot.plot_3d_points(0, current_estimate, linespec="g*", title="Final iSAM Estimate")
for i in range(len(poses)):
if current_estimate.exists(X(i)):
gtsam_plot.plot_pose3(0, current_estimate.atPose3(X(i)), axis_length=3)
plt.show()
This is exactly the kind of incremental estimation problem NonlinearISAM was built for -- and it’s the direct predecessor to ISAM2, GTSAM’s more efficient incremental solver, used in the companion VisualISAM2Example notebook.