GPS Factor Example - GTSAM Docs

A GPSFactor ties a 3D position measurement -- for example, a reading from a GPS receiver -- to a Pose3 variable in a factor graph. Because GPS only observes position, a single GPSFactor constrains the translation part of a pose but says nothing about orientation.

This notebook builds the smallest possible example of that idea: one pose, one prior, one GPS measurement. We optimize with Levenberg-Marquardt and see the pose’s position pulled toward the GPS reading while its orientation is left to the prior.

Authors: Mandy Xie, 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:
    pass

import gtsam

1. Set up the problem¶

lat0, lon0, h0 play the role of a single GPS reading -- the ENU origin where the plane was in hold next to the runway. We use two noise models:

GPS_NOISE is 3-dimensional, since a GPS measurement only observes x, y, z position.
PRIOR_NOISE is 6-dimensional, since the prior constrains the full Pose3 (position and orientation).

# ENU origin is where the plane was in hold next to runway
lat0 = 33.86998
lon0 = -84.30626
h0 = 274

GPS_NOISE = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
PRIOR_NOISE = gtsam.noiseModel.Isotropic.Sigma(6, 0.25)

2. Build the factor graph¶

The PriorFactorPose3 anchors pose key 1 at the identity pose -- deliberately different from the GPS reading, so the optimizer has real work to do. The GPSFactor then adds the position-only measurement on the same key.

graph = gtsam.NonlinearFactorGraph()

# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose3()  # prior at origin
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))

# Add the GPS factor
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))

print(graph)

NonlinearFactorGraph: size: 2

Factor 0: PriorFactor on 1
  prior mean:  R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t: 0 0 0
isotropic dim=6 sigma=0.25

Factor 1:  GPSFactor on 1
  GPS measurement:    33.87
-84.3063
     274
isotropic dim=3 sigma=0.1

3. Initial estimate¶

For illustrative purposes, the initial estimate is deliberately set to the identity pose -- far from the GPS reading -- so we can watch the optimizer correct it.

initial = gtsam.Values()
initial.insert(1, gtsam.Pose3())
print(initial)

Values with 1 values:
Value 1: (gtsam::Pose3)
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t: 0 0 0

4. Optimize¶

We solve with LevenbergMarquardtOptimizer, the general-purpose nonlinear least-squares solver used throughout GTSAM.

params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print(result)

Values with 1 values:
Value 1: (gtsam::Pose3)
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t:  29.1983 -72.6778  236.207

Notice that the optimized translation lands close to, but not exactly at, the GPS reading (lat0, lon0, h0). It’s a noise-weighted compromise between the two factors: the prior pulls it toward (0, 0, 0) with sigma=0.25, while the GPS factor pulls it toward the measurement with a tighter sigma=0.1, so the tighter (more confident) factor dominates. The rotation, meanwhile, stays exactly at the prior’s identity value -- a single GPSFactor has no way to observe orientation from one position measurement. That information has to come from somewhere else (multiple GPS readings over time, an IMU, a compass, etc.).