A unified probabilistic primitive in GTSAM for representing a (possibly shifted) Gaussian density on a manifold value type. This notebook serves three audiences:
General GTSAM users who just want a probability-like object.
EKF users receiving
ConcentratedGaussianoutputs.Advanced users leveraging transport, reset, and fusion of Left Extended Concentrated Gaussians (L-ECGs).
Related notebooks: PriorFactor, ExtendedPriorFactor.
# Install GTSAM and Plotly from pip if running in Google Colab
try:
import google.colab
%pip install --quiet gtsam-develop
except ImportError:
pass # Not in Colabimport math
from typing import TypeAlias
import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots
import gtsam
from gtsam import Point2, Point3, Pose2, Rot2
# Type aliases for clarity when passing to GTSAM
V: TypeAlias = np.ndarray # Vector (1D) passed into GTSAM
M: TypeAlias = np.ndarray # Matrix (2D) passed into GTSAM1. General Usage¶
ConcentratedGaussian<T> behaves like a continuous probability density over a manifold variable T. For Gaussian noise models it evaluates to properly normalized probabilities (up to floating-point limits):
Construct with origin
o, covarianceΣ(or noise model), and optional tangent meanm.Error term internally:
e(x) = -Local(x, o) - m(if mean provided).Probability:
P(x) = exp( logProbability(x) ).evaluate(x)is a convenience synonym forP(x).
We’ll start with a simple 2D example using Point2 (Euclidean).
# Zero-mean Point2 density
origin: V = Point2(0.0, 0.0)
cov: M = np.array([[0.4, 0.1],
[0.1, 0.2]], dtype=float)
density = gtsam.ConcentratedGaussianPoint2(1, origin, cov)
xs: V = np.linspace(-2,2,160)
ys: V = np.linspace(-2,2,160)
X, Y = np.meshgrid(xs, ys)
Z: M = np.zeros_like(X)
for i in range(X.shape[0]):
for j in range(X.shape[1]):
Z[i,j] = density.evaluate(Point2(X[i,j], Y[i,j]))
# NOTE: In the Python wrapper Point2 is a numpy ndarray alias, so we use p[0], p[1].
fig = go.Figure()
fig.add_trace(go.Contour(x=xs, y=ys, z=Z, colorscale='Viridis', contours=dict(showlabels=True)))
fig.add_trace(go.Scatter(x=[origin[0]], y=[origin[1]], mode='markers', marker=dict(color='red', size=10), name='origin'))
fig.update_layout(title='Point2 ConcentratedGaussian (zero-mean)', xaxis_title='x', yaxis_title='y')
fig.show()Adding a Tangent-Space Mean¶
A non-zero mean shifts the mode to origin.retract(mean) (Point2: just addition).
# Non-zero tangent mean shifts the mode to origin.retract(mean)
mean: V = np.array([0.8, -0.4], dtype=float)
density_shifted = gtsam.ConcentratedGaussianPoint2(2, origin, mean, cov)
mode_point: V = origin + mean # numpy-based Point2 + mean
mode_prob = density_shifted.evaluate(Point2(mode_point[0], mode_point[1]))
print('Mode probability (approx peak):', mode_prob)
Zs: M = np.zeros_like(Z)
for i in range(X.shape[0]):
for j in range(X.shape[1]):
Zs[i,j] = density_shifted.evaluate(Point2(X[i,j], Y[i,j]))
fig2 = go.Figure()
fig2.add_trace(go.Contour(x=xs, y=ys, z=Zs, colorscale='Inferno', contours=dict(showlabels=True)))
fig2.add_trace(go.Scatter(x=[origin[0]], y=[origin[1]], mode='markers', marker=dict(color='red', size=10), name='origin'))
fig2.add_trace(go.Scatter(x=[mode_point[0]], y=[mode_point[1]], mode='markers', marker=dict(color='green', size=10, symbol='x'), name='mode'))
fig2.update_layout(title='Shifted ConcentratedGaussian (Point2)', xaxis_title='x', yaxis_title='y')
fig2.show()Pose2 Example with Samples (Banana Shape)¶
Let us simulate a vehicle taking N steps, but with large uncertainty in angle. To obtain a pronounced “banana” distribution we simulate many short forward motion increments with heading noise. Each sample is a random walk of small steps:
Start at identity pose.
For each of K steps, sample a small forward translation with little lateral drift and a heading perturbation.
Compose these incremental poses.
Record the final (x,y,theta).
Accumulated heading noise bends trajectories, producing a curved (banana-like) marginal over (x,y).
Source
def plot_pose2_samples(title: str, poses: list, trajectory: list = None) -> go.Figure:
xs, ys, thetas = zip(*[(p.x(), p.y(), np.rad2deg(p.theta())) for p in poses])
fig = go.Figure()
fig.add_trace(go.Scattergl(
x=xs, y=ys, mode='markers',
marker=dict(size=3, opacity=0.35, color=thetas, colorscale='Turbo', colorbar=dict(title='theta')),
name='end poses'))
if trajectory is not None:
# Draw tiny arrows for each pose in trajectory
for p in trajectory:
x = p.x()
y = p.y()
th = p.theta()
dx = 0.03 * np.cos(th)
dy = 0.03 * np.sin(th)
fig.add_trace(go.Scatter(
x=[x, x + dx], y=[y, y + dy],
mode='lines',
line=dict(color='black', width=1),
showlegend=False
))
fig.update_layout(
title=title,
xaxis_title='x', yaxis_title='y', yaxis_scaleanchor='x'
)
fig.update_layout(margin=dict(l=0, r=0, t=40, b=0))
return fig# Incremental random-walk generation of a banana-shaped distribution
rng = np.random.default_rng(1)
N: int = 5000 # number of trajectories
K: int = 20 # number of small increments per trajectory
# Constant twist: mean increment is a fixed 3-vector (vx, vy, omega)
mean_twist :V = np.array([0.12, 0.0, np.deg2rad(2)], dtype=float) # forward, lateral, angular velocity (2 deg/step)
cov_twist :M = np.diag([0.01**2, 0.01**2, np.deg2rad(5)**2]) # diagonal covariance
poses = []
for n in range(N):
pose = Pose2(0, 0, 0)
for _ in range(K):
delta :V = rng.multivariate_normal(mean_twist, cov_twist)
pose = pose.retract(delta)
poses.append(pose)
pose = Pose2(0, 0, 0)
trajectory = [pose]
for _ in range(K):
pose = pose.retract(mean_twist)
trajectory.append(pose)
plot_pose2_samples("Pose2 constant twist + noise, banana-shaped distribution", poses, trajectory)
Just as in the RSS 2008 paper, “The Banana Distribution is Gaussian”, we can now fit a Gaussian *to the exponential coordinates:
# Stack xs, ys, thetas into Pose2s, Logmap to tangent vectors, fit mean/covariance
tangent_vecs = np.array([Pose2.Logmap(p) for p in poses])
mean_tangent : V = np.mean(tangent_vecs, axis=0)
cov_tangent : M = np.cov(tangent_vecs, rowvar=False)
# Create a ConcentratedGaussianPose2 using mean_tangent and cov_tangent at origin_pose
density_pose2 = gtsam.ConcentratedGaussianPose2(20, Pose2(), mean_tangent, cov_tangent)
print(density_pose2)ConcentratedGaussian on 20
origin: (0, 0, 0)
tangent space mean: [2.36917357; -0.0453072606; 0.694605119];
noise model: Gaussian [
16.4385844, -0.0183798711, -0.0487508153;
0, 3.66909727, 0.227405048;
0, 0, 2.57923483
]
# Sample from the tangent Gaussian, push through ExpMap, and plot (x, y, theta)
maybe_mP = density_pose2.gaussian()
assert maybe_mP is not None, "Expected gaussian() to return a value"
m, P = maybe_mP # P=covariance, m=mean
samples_tangent = rng.multivariate_normal(m, P, size=N)
poses_push = []
for v in samples_tangent:
p = Pose2.Expmap(v)
poses_push.append(p)
plot_pose2_samples("Samples from fitted ConcentratedGaussianPose2", poses_push, trajectory)The banana shape crucially depends on having a Gaussian with a mean far from the origin: geodesics in Pose2 trace out circles in the x-y plane, and hence an offset Gaussian, as provided by ConcentratedGaussian, is able to approximate the banana distribution well.
We can even analytically predict the Gaussian:
# Compute the analytic mean and covariance in tangent space
mean_analytic = Pose2.Logmap(trajectory[-1])
cov_analytic = K * cov_twist # This is an approximation; for true analytic, propagate with Jacobians
analytic = gtsam.ConcentratedGaussianPose2(20, Pose2(), mean_analytic, cov_analytic)
# Sample from the analytically obtained Gaussian
maybe_mP = analytic.gaussian()
assert maybe_mP is not None, "Expected gaussian() to return a value"
m, P = maybe_mP # P=covariance, m=mean
samples_tangent = rng.multivariate_normal(m, P, size=N)
poses_push = []
for v in samples_tangent:
p = Pose2.Expmap(v)
poses_push.append(p)
plot_pose2_samples("Samples from an analytically obtained ConcentratedGaussianPose2", poses_push, trajectory)2. EKF Perspective¶
An Extended Kalman Filter (EKF) posterior can be represented as a ConcentratedGaussian:
Origin: the linearization/reference state.
Tangent mean: offset from origin to the mode.
Covariance: uncertainty in that tangent space.
Before using this posterior as a prior for the next prediction, call reset() to move the origin to the mode and zero the mean (keeping covariance transported).
# Simulated EKF posterior with non-zero tangent mean
origin_post = Pose2(1.0, 2.0, 0.2)
cov_post: M = np.diag([0.05, 0.04, 0.02]).astype(float)
mean_post: V = np.array([0.3, -0.1, 0.15], dtype=float)
posterior = gtsam.ConcentratedGaussianPose2(5, origin_post, mean_post, cov_post)
mode_pose = posterior.retractMean()
print('Origin:', origin_post)
print('Mode :', mode_pose)
print('Mode probability:', posterior.evaluate(mode_pose))
reset_density = posterior.reset()
print('After reset -> origin == mode?', reset_density.origin() == mode_pose)
print('Reset mean (should be empty/None):', reset_density.mean())
fig_ekf = go.Figure()
fig_ekf.add_trace(go.Scatter(x=[origin_post.x()], y=[origin_post.y()], mode='markers',
marker=dict(color='red', size=10), name='origin'))
fig_ekf.add_trace(go.Scatter(x=[mode_pose.x()], y=[mode_pose.y()], mode='markers',
marker=dict(color='green', size=10, symbol='x'), name='mode'))
fig_ekf.update_layout(title='EKF Posterior: Origin vs Mode', xaxis_title='x', yaxis_title='y')
fig_ekf.show()3. Advanced Operations: L-ECG Mechanics¶
ConcentratedGaussian implements a Left Extended Concentrated Gaussian (L-ECG). Advanced capabilities:
reset()– Move origin to mode, zero the tangent mean, transport covariance.transportTo(x_hat)– Express density in a new chart (new origin) inducing a mean and transported covariance.operator*– Approximate fusion of two densities (same key) in a common chart followed by reset.
Below: transport and fusion examples (Pose2).
# Transport & Fusion with separated, differently oriented densities
originA = Pose2(-2.0, 0.0, math.pi/4)
covA: M = np.diag([0.25, 0.12, 0.10]).astype(float)
meanA: V = np.array([0.4, 0.0, 0.0], dtype=float)
originB = Pose2(2.0, 0.0, -math.pi/3)
covB: M = np.diag([0.25, 0.12, 0.10]).astype(float)
meanB: V = np.array([0.4, 0.0, 0.0], dtype=float)
dA = gtsam.ConcentratedGaussianPose2(10, originA, meanA, covA)
dB = gtsam.ConcentratedGaussianPose2(10, originB, meanB, covB)
fused = dA * dB
print('Fused origin:', fused.origin())
print('Fused mean (should be None):', fused.mean())Fused origin: (0.320684765, 0.704438054, -0.130899694)
Fused mean (should be None): None
Source
# Plot the things
fig_fusion = go.Figure()
def heading_arrow(pose, length: float = 0.7, color: str = 'black', name: str = 'heading') -> None:
x = pose.x(); y = pose.y(); th = pose.theta()
x2 = x + length*math.cos(th)
y2 = y + length*math.sin(th)
fig_fusion.add_trace(go.Scatter(x=[x, x2], y=[y, y2], mode='lines', line=dict(color=color, width=3), name=name, showlegend=False))
fig_fusion.add_trace(go.Scatter(x=[originA.x()], y=[originA.y()], mode='markers', marker=dict(color='blue', size=10), name='Origin A'))
fig_fusion.add_trace(go.Scatter(x=[originB.x()], y=[originB.y()], mode='markers', marker=dict(color='red', size=10), name='Origin B'))
fig_fusion.add_trace(go.Scatter(x=[fused.origin().x()], y=[fused.origin().y()], mode='markers', marker=dict(color='green', size=12, symbol='x'), name='Fused'))
heading_arrow(originA, color='blue')
heading_arrow(originB, color='red')
heading_arrow(fused.origin(), color='green')
fig_fusion.update_layout(title='Fusion of Two Opposed Pose2 Densities (Result ~ centered, heading ~ 0)',
xaxis_title='x', yaxis_title='y', yaxis_scaleanchor='x')
fig_fusion.show()Caveats¶
Fusion and transport are first-order; for large separations, consider iterative refinement.
Large tangent means may signal re-centering (
reset) to maintain Gaussian fidelity.Current examples track covariance externally for sampling; exposing underlying Gaussian model in Python would make advanced visualization easier.
Summary¶
ConcentratedGaussian = manifold-aware Gaussian density: basic probability evaluation (Section 1), EKF posterior handling with retractMean()/reset() (Section 2), and geometric operations (transport, fusion) for advanced workflows (Section 3). Choose the simplest interface that solves your task.