ConcentratedGaussian

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 ConcentratedGaussian outputs.
Advanced users leveraging transport, reset, and fusion of Left Extended Concentrated Gaussians (L-ECGs).

Related notebooks: PriorFactor, ExtendedPriorFactor.

import 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 GTSAM

1. 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 mean m.
Error term internally: e(x) = -Local(x, o) - m (if mean provided).
Probability: P(x) = exp( logProbability(x) ).
evaluate(x) is a convenience synonym for P(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()

Mode probability (approx peak): 0.6015491419254174

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).

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()

Origin: (1, 2, 0.2)

Mode  : (1.31388691, 1.96159414, 0.35)

Mode probability: 9.769507109185751
After reset -> origin == mode? False
Reset mean (should be empty/None): None

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

# 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.