geodex Basics

This tutorial introduces the core operations of geodex: creating manifolds, computing exponential and logarithmic maps, measuring distances, and interpolating along geodesics. We will work through each concept step by step starting with the underlying mathematics and then show how geodex expresses it in the code.

By the end, you will be comfortable with the four built-in manifolds in geodex (the sphere, Euclidean space, the flat torus, and SE(2)) and understand how to swap metrics and retractions to change the geometry.

Note

This tutorial assumes you have installed geodex and have a basic familiarity with the geometric vocabulary (manifolds, tangent spaces, metrics). If terms like “exponential map” or “Riemannian metric” are unfamiliar, we recommend reading Core Concepts first.

All C++ examples in this tutorial use the main geodex header:

#include <geodex/geodex.hpp>

import geodex
import numpy as np

Your First Manifold

The simplest way to get started is with the 2-sphere, defined as the set of unit vectors in \(\mathbb{R}^3\):

\[\mathbb{S}^2 = \bigl\{ p \in \mathbb{R}^3 : \|p\| = 1 \bigr\}.\]

In geodex, the Sphere<Dim> class template models \(\mathbb{S}^n\) for any \(n \geq 1\). Sphere<> is shorthand for Sphere<2> — the default 2-sphere. Creating one is straightforward:

geodex::Sphere<> sphere;
std::cout << "dim = " << sphere.dim() << "\n";  // 2

sphere = geodex.Sphere()
print("dim =", sphere.dim())  # 2

The empty angle brackets <> mean we are using the default intrinsic dimension (2), the default metric (SphereRoundMetric), and the default retraction (SphereExponentialMap). Writing Sphere<> is equivalent to writing the fully-qualified type:

geodex::Sphere<2, geodex::SphereRoundMetric, geodex::SphereExponentialMap> sphere;

The default names SphereRoundMetric, EuclideanStandardMetric<N>, and TorusFlatMetric<N> are type aliases for ConstantSPDMetric<K> with an identity matrix — the ambient identity inner product.

This is the policy-based design at the heart of geodex: the manifold is parameterized by a metric policy (what “length” means) and a retraction policy (how to move along the manifold). We will explore both in later sections.

Points on \(\mathbb{S}^n\) are Eigen::Vector<double, n+1> unit vectors, and tangent vectors share the same type (orthogonal to the base point). For the default Sphere<> (\(\mathbb{S}^2\)), both are Eigen::Vector3d.

Manifolds at a Glance

geodex ships with four manifold families. The table below summarises their essential properties:

Manifold	Symbol	Point type	dim	Default metric
`Sphere<N>`	\(\mathbb{S}^N\)	`Eigen::Vector<double, N+1>`	N	`SphereRoundMetric`
`Euclidean<N>`	\(\mathbb{R}^N\)	`Eigen::Vector<double, N>`	N	`EuclideanStandardMetric<N>`
`Torus<N>`	\(\mathbb{T}^N\)	`Eigen::Vector<double, N>`	N	`TorusFlatMetric<N>`
`SE2<>`	\(\mathrm{SE}(2)\)	`Eigen::Vector3d`	3	`SE2LeftInvariantMetric`

Creating instances of these manifolds follows the same pattern:

geodex::Euclidean<3> R3;  // R^3 with standard metric
geodex::Torus<2>     T2;  // 2-torus with flat metric
geodex::SE2<>        se2; // SE(2) with left-invariant metric

euclidean = geodex.Euclidean(3)  # R^3 with standard metric
torus     = geodex.Torus(2)      # 2-torus with flat metric
se2       = geodex.SE2()         # SE(2) with left-invariant metric

The Riemannian Inner Product

A Riemannian metric assigns an inner product to each tangent space. Given two tangent vectors \(u, v \in \mathcal{T}_p\mathcal{M}\) at a point \(p\), the inner product is written

\[\langle u, v \rangle_p,\]

and the induced norm is

\[\|v\|_p = \sqrt{\langle v, v \rangle_p}.\]

On the sphere with the round metric, the inner product is simply the Euclidean dot product of the ambient vectors:

\[\langle u, v \rangle_p = u \cdot v.\]

In code, we call inner() and norm():

geodex::Sphere<> sphere;

Eigen::Vector3d p{0, 0, 1};          // north pole
Eigen::Vector3d u{1, 0, 0};          // tangent vector pointing east
Eigen::Vector3d v{0, 1, 0};          // tangent vector pointing south

double ip = sphere.inner(p, u, v);
std::cout << "ip = " << ip << "\n";  // 0.0 (orthogonal)
double n  = sphere.norm(p, u);
std::cout << "n = " << n << "\n";  // 1.0

sphere = geodex.Sphere()

p = np.array([0., 0., 1.])  # north pole
u = np.array([1., 0., 0.])  # tangent vector pointing east
v = np.array([0., 1., 0.])  # tangent vector pointing south

ip = sphere.inner(p, u, v)  # 0.0 (orthogonal)
print("ip =", ip)  # 0.0 (orthogonal)
n  = sphere.norm(p, u)
print("n =", n)  # 1.0

Notice that both functions take the base point \(p\) as their first argument. For the round metric, the inner product does not actually depend on \(p\), but this is a special case. On manifolds with position-dependent metrics (such as the kinetic energy metric used in robotics), the base point changes the inner product at every configuration.

Tip

The Minimum-Energy Planning on Configuration Manifolds tutorial shows how position-dependent metrics arise naturally when modelling robot inertia.

On Euclidean space, the inner product and norm behave identically to the standard dot product:

geodex::Euclidean<3> R3;

Eigen::Vector3d p{0, 0, 0};
Eigen::Vector3d u{1, 0, 0};
Eigen::Vector3d v{0, 1, 0};

double ip = R3.inner(p, u, v);
std::cout << "ip = " << ip << "\n";  // 0.0
double n  = R3.norm(p, u);
std::cout << "n = " << n << "\n";  // 1.0

euclidean = geodex.Euclidean(3)

p = np.array([0., 0., 0.])
u = np.array([1., 0., 0.])
v = np.array([0., 1., 0.])

ip = euclidean.inner(p, u, v)
print("ip =", ip)  # 0.0
n  = euclidean.norm(p, u)
print("n =", n)  # 1.0

Exponential and Logarithmic Maps

The exponential map \(\exp_p(v)\) starts at a point \(p\) and “walks” along the manifold in the direction of a tangent vector \(v\) for a distance equal to \(\|v\|_p\). The result is a new point on the manifold. The logarithmic map \(\log_p(q)\) is its inverse: given two points \(p\) and \(q\), it returns the tangent vector at \(p\) that points toward \(q\).

On the sphere, the exponential map has a particularly elegant closed form:

\[\exp_p(v) = \cos(\|v\|)\, p + \sin(\|v\|)\, \frac{v}{\|v\|}.\]

This traces a great circle starting at \(p\) in the direction of \(v\). The logarithmic map inverts the relationship, that is, it finds the initial velocity that, followed for unit time, arrives at \(q\):

geodex::Sphere<> sphere;

Eigen::Vector3d p{0, 0, 1};  // north pole
Eigen::Vector3d q{1, 0, 0};  // point on equator

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

// log: tangent vector at p pointing toward q
Eigen::Vector3d v = sphere.log(p, q);
std::cout << "v = " << v.transpose().format(fmt) << "\n"; // ≈ [π/2, 0, 0] - points along the great circle from pole to equator

// exp: follow that tangent vector to recover q
Eigen::Vector3d q_recovered = sphere.exp(p, v);
std::cout << "q_recovered = " << q_recovered.transpose().format(fmt) << "\n"; // ≈ [1, 0, 0]

sphere = geodex.Sphere()

p = np.array([0., 0., 1.])  # north pole
q = np.array([1., 0., 0.])  # point on equator

# log: tangent vector at p pointing toward q
v = sphere.log(p, q)
print("v =", v)  # ≈ [π/2, 0, 0] - points along the great circle from pole to equator

# exp: follow that tangent vector to recover q
q_recovered = sphere.exp(p, v)
print("q_recovered =", q_recovered)  # ≈ [1, 0, 0]

The tangent vector \(v = \log_p(q)\) encodes two things: its direction is the initial heading of the geodesic from \(p\) to \(q\), and its norm \(\|v\|_p\) is the geodesic distance between them. The round trip exp(p, log(p, q)) always recovers \(q\) (up to numerical precision), as long as \(q\) lies within the injectivity radius of \(p\).

Note

On the sphere, the injectivity radius is \(\pi\). Antipodal points (e.g. the north and south poles) lie exactly at distance \(\pi\), and the geodesic between them is not unique, that is, there exist infinitely many great circles passing through both poles. In geodex, calling log on antipodal points returns a zero vector with a warning.

On Euclidean space, the exponential and logarithmic maps reduce to simple addition and subtraction:

\[\exp_p(v) = p + v, \qquad \log_p(q) = q - p.\]

geodex::Euclidean<3> R3;

Eigen::Vector3d p{1, 0, 0};
Eigen::Vector3d q{0, 1, 0};

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

auto v  = R3.log(p, q);
std::cout << "v = " << v.transpose().format(fmt) << "\n";  // [-1, 1, 0]
auto q2 = R3.exp(p, v);
std::cout << "q2 = " << q2.transpose().format(fmt) << "\n";  // [0, 1, 0] == q

euclidean = geodex.Euclidean(3)

p = np.array([1., 0., 0.])
q = np.array([0., 1., 0.])

v  = euclidean.log(p, q)
print("v =", v)  # [-1, 1, 0]
q2 = euclidean.exp(p, v)
print("q2 =", q2)  # [0, 1, 0] == q

Geodesic Distance

The geodesic distance between two points is the length of the shortest path connecting them. It can be expressed in terms of the logarithmic map and the Riemannian norm:

\[d(p, q) = \bigl\|\log_p(q)\bigr\|_p.\]

On the sphere with the round metric, this simplifies to the well-known arc-length formula:

\[d(p, q) = \arccos(p \cdot q).\]

We can verify this in the code. The north pole \((0,0,1)\) and a point on the equator \((1,0,0)\) are separated by a quarter of a great circle, so the distance should be \(\pi/2\):

geodex::Sphere<> sphere;

Eigen::Vector3d p{0, 0, 1};       // north pole
Eigen::Vector3d q{1, 0, 0};       // equator

auto d = sphere.distance(p, q);
std::cout << "d = " << d << "\n";           // 1.5708 ≈ π/2

sphere = geodex.Sphere()

p = np.array([0., 0., 1.])  # north pole
q = np.array([1., 0., 0.])  # equator

d = sphere.distance(p, q)
print("d =", d)  # 1.5708 ≈ π/2

Note

Internally, distance() uses the distance_midpoint algorithm from [Kyaw and Kelly, 2026], which evaluates log maps at the geodesic midpoint to obtain a third-order accurate approximation. For manifolds that provide true (exact) exponential and logarithmic maps, like the sphere with SphereExponentialMap, this formula yields the exact geodesic distance.

On Euclidean space, the geodesic distance is just the standard Euclidean norm of the difference:

geodex::Euclidean<3> R3;

Eigen::Vector3d p{1, 0, 0};
Eigen::Vector3d q{0, 1, 0};

double d = R3.distance(p, q);
std::cout << "d = " << d << "\n";  // 1.41421 ≈ √2

euclidean = geodex.Euclidean(3)

p = np.array([1., 0., 0.])
q = np.array([0., 1., 0.])

d = euclidean.distance(p, q)
print("d =", d)  # 1.41421 ≈ √2

The torus is where distance becomes more interesting, because coordinates wrap around. Consider two angles on a 1-torus (a circle) at \(\theta_1 = 0.1\) and \(\theta_2 = 6.0\). The naive difference is \(5.9\), but the shortest path wraps around through \(2\pi\), giving a distance of approximately \(0.38\):

geodex::Torus<1> S1;

Eigen::Vector<double, 1> p{0.1};
Eigen::Vector<double, 1> q{6.0};

double d = S1.distance(p, q);
std::cout << "d = " << d << "\n";  // 0.383 ≈ 2π - 5.9

S1 = geodex.Torus(1)

p = np.array([0.1])
q = np.array([6.0])

d = S1.distance(p, q)
print("d =", d)  # 0.383 ≈ 2π - 5.9

Geodesic Interpolation

Given two points \(p\) and \(q\), we often want the point a fraction \(t\) of the way along the shortest geodesic between them. The formula is a direct composition of the logarithmic and exponential maps:

\[\gamma(t) = \exp_p\,\bigl(t \cdot \log_p(q)\bigr), \quad t \in [0, 1].\]

At \(t = 0\) we recover \(p\), at \(t = 1\) we recover \(q\), and at \(t = 0.5\) we get the geodesic midpoint.

On the sphere, this traces an arc of the great circle. The midpoint between the north pole \((0,0,1)\) and the equator \((1,0,0)\) lies at 45° latitude:

geodex::Sphere<> sphere;

Eigen::Vector3d p{0, 0, 1};  // north pole
Eigen::Vector3d q{1, 0, 0};  // equator

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

Eigen::Vector3d mid = sphere.geodesic(p, q, 0.5);
std::cout << "mid = " << mid.transpose().format(fmt) << "\n";  // ≈ [0.707, 0, 0.707]

// Trace the full geodesic
for (int i = 0; i <= 10; ++i) {
   double t = i / 10.0;
   Eigen::Vector3d pt = sphere.geodesic(p, q, t);
   std::cout << "t=" << t << ": " << pt.transpose().format(fmt) << "\n";
}

sphere = geodex.Sphere()

p = np.array([0., 0., 1.])  # north pole
q = np.array([1., 0., 0.])  # equator

mid = sphere.geodesic(p, q, 0.5)
print("mid =", mid)  # ≈ [0.707, 0, 0.707]

# Trace the full geodesic
for i in range(11):
    t = i / 10.0
    pt = sphere.geodesic(p, q, t)
    print(f"t={t:.1f}: {pt}")

The midpoint has coordinates approximately \((1/\sqrt{2},\, 0,\, 1/\sqrt{2})\), which is the unit vector at 45° between the pole and the equator — exactly what we expect.

Tip

The formula \(\gamma(t) = \exp_p(t \cdot \log_p(q))\) is universal: it works on any RiemannianManifold. Whether you are interpolating on a sphere, a torus, or SE(2), the same geodesic(p, q, t) call does the right thing.

On Euclidean space, geodesic interpolation reduces to linear interpolation:

\[\gamma(t) = (1 - t)\, p + t\, q.\]

geodex::Euclidean<3> R3;

Eigen::Vector3d p{0, 0, 0};
Eigen::Vector3d q{2, 4, 6};

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

Eigen::Vector3d mid = R3.geodesic(p, q, 0.5);
std::cout << "mid = " << mid.transpose().format(fmt) << "\n";  // [1, 2, 3]

euclidean = geodex.Euclidean(3)

p = np.array([0., 0., 0.])
q = np.array([2., 4., 6.])

mid = euclidean.geodesic(p, q, 0.5)
print("mid =", mid)  # [1, 2, 3]

Random Sampling

Every manifold in geodex provides a random_point() method for sampling. The distribution depends on the manifold:

geodex::Sphere<>     sphere;
geodex::Euclidean<3> R3;
geodex::Torus<2>     T2;
geodex::SE2<>        se2;

auto p1 = sphere.random_point();  // uniform on S²
auto p2 = R3.random_point();      // uniform in [-1, 1]³
auto p3 = T2.random_point();      // uniform in [0, 2π)²
auto p4 = se2.random_point();     // uniform in sampling bounds

sphere    = geodex.Sphere()
euclidean = geodex.Euclidean(3)
torus     = geodex.Torus(2)
se2       = geodex.SE2()

p1 = sphere.random_point()     # uniform on S²
p2 = euclidean.random_point()  # uniform in [-1, 1]³
p3 = torus.random_point()      # uniform in [0, 2π)²
p4 = se2.random_point()        # uniform in sampling bounds

The sphere uses the standard technique of normalizing a Gaussian vector to obtain a uniform distribution on \(\mathbb{S}^n\). Euclidean space draws each coordinate uniformly from \([-1, 1]\); call set_sampling_bounds(lo, hi) to change the box. The torus samples each angle uniformly from \([0, 2\pi)\).

Note

Euclidean, Torus, and SE2 take a SamplerT policy template parameter that drives random_point(). The default is StochasticSampler (mt19937). Swap in HaltonSampler for deterministic low-discrepancy quasi-random sampling — useful for reproducible benchmarks and for planners that benefit from better space coverage than pseudo-random draws.

geodex::Euclidean<3, geodex::EuclideanStandardMetric<3>, geodex::HaltonSampler> R3_halton;

Note

For SE(2), random_point() samples the position \((x, y)\) uniformly within the configured sampling bounds (default: \([0, 10]^2\)) and the heading \(\theta\) uniformly from \([-\pi, \pi)\). The bounds can be configured via the constructor.

The Torus and SE(2)

The sphere and Euclidean space are the simplest manifolds to work with. The torus and SE(2) introduce two important complications: periodic coordinates and Lie group structure.

The Flat Torus

The \(n\)-torus \(\mathbb{T}^n\) is the product of \(n\) circles. Points are represented as angle vectors in \([0, 2\pi)^n\). The key difference from Euclidean space is that coordinates wrap around: the angles \(0\) and \(2\pi\) represent the same point.

The exponential map adds the tangent vector and wraps the result:

\[\exp_p(v) = (p + v) \bmod 2\pi.\]

The logarithmic map computes the shortest signed difference for each coordinate:

\[\bigl(\log_p(q)\bigr)_i = \mathrm{wrap}_{[-\pi,\pi)}\!\bigl(q_i - p_i\bigr).\]

This wrapping ensures that the log always returns the shortest path around each circle. Consider two points on the 2-torus that are close together when wrapping is accounted for but far apart in raw coordinates:

geodex::Torus<2> T2;

Eigen::Vector2d p{0.1, 0.2};
Eigen::Vector2d q{6.1, 0.5};

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

// log wraps to the shortest path
Eigen::Vector2d v = T2.log(p, q);
std::cout << "v = " << v.transpose().format(fmt) << "\n";  // ≈ [-0.283, 0.3]
// The first component wraps: 6.1 - 0.1 = 6.0 → wrap to ≈ -0.283

// exp follows the tangent vector and wraps back to [0, 2π)
Eigen::Vector2d q_recovered = T2.exp(p, v);
std::cout << "q_recovered = " << q_recovered.transpose().format(fmt) << "\n";  // ≈ [6.1, 0.5]

T2 = geodex.Torus(2)

p = np.array([0.1, 0.2])
q = np.array([6.1, 0.5])

# log wraps to the shortest path
v = T2.log(p, q)
print("v =", v)  # ≈ [-0.283, 0.3]
# The first component wraps: 6.1 - 0.1 = 6.0 → wrap to ≈ -0.283

# exp follows the tangent vector and wraps back to [0, 2π)
q_recovered = T2.exp(p, v)
print("q_recovered =", q_recovered)  # ≈ [6.1, 0.5]

SE(2) — Rigid-Body Poses

The special Euclidean group \(\mathrm{SE}(2) = \mathbb{R}^2 \rtimes \mathrm{SO}(2)\) describes rigid-body poses in the plane. A pose is represented as \((x, y, \theta)\) where \((x, y)\) is the position and \(\theta \in [-\pi, \pi)\) is the heading.

Unlike the torus, SE(2) is a Lie group: the coupling between translation and rotation means that moving “forward” depends on your current heading. The exponential map on SE(2) uses the Lie group exponential, which accounts for this coupling:

\[\exp_p(v) = p \cdot \mathrm{Exp}(v),\]

where \(\mathrm{Exp}\) is the matrix exponential at the identity and \(\cdot\) denotes group composition (left translation). Concretely, the tangent vector \(v = (v_x, v_y, \omega)\) represents a body-frame velocity, and the matrix \(V(\omega)\) maps it to a group displacement.

The default metric on SE(2) is the left-invariant metric with diagonal weights \((w_x, w_y, w_\theta)\):

\[\langle u, v \rangle = w_x u_x v_x + w_y u_y v_y + w_\theta u_\theta v_\theta.\]

With unit weights, the metric treats translational and rotational displacements equally. In practice, you may want to penalize rotation more heavily (or vice versa) to reflect your robot’s kinematic preferences.

geodex::SE2<> se2;

Eigen::Vector3d p{1.0, 2.0, 0.0};   // pose at (1,2), heading east
Eigen::Vector3d q{3.0, 4.0, 1.57};  // pose at (3,4), heading north

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

Eigen::Vector3d v = se2.log(p, q);
std::cout << "log: " << v.transpose().format(fmt) << "\n";

Eigen::Vector3d q_recovered = se2.exp(p, v);
std::cout << "recovered: " << q_recovered.transpose().format(fmt) << "\n";

double d = se2.distance(p, q);
std::cout << "distance: " << d << "\n";

se2 = geodex.SE2()

p = np.array([1.0, 2.0, 0.0])   # pose at (1,2), heading east
q = np.array([3.0, 4.0, 1.57])  # pose at (3,4), heading north

v = se2.log(p, q)
print("log:", v)

q_recovered = se2.exp(p, v)
print("recovered:", q_recovered)

d = se2.distance(p, q)
print("distance:", d)

A particularly useful trick is to set a large lateral weight \(w_y\). This heavily penalizes sideways motion in the body frame, mimicking the non-holonomic constraint of a car-like robot that can only drive forward or backward:

// Large w_y penalizes lateral (sideways) motion - car-like behavior
geodex::SE2LeftInvariantMetric car_metric{1.0, 100.0, 1.0};
geodex::SE2<> se2_car{car_metric};

Eigen::Vector3d p{0.0, 0.0, 0.0};  // facing east
Eigen::Vector3d q{2.0, 2.0, 0.0};  // same heading, offset diagonally

// Geodesics now prefer to turn-and-drive rather than slide sideways
double d = se2_car.distance(p, q);
std::cout << "car-like distance: " << d << "\n";

# Large wy penalizes lateral (sideways) motion - car-like behavior
se2_car = geodex.SE2(wx=1.0, wy=100.0, wtheta=1.0)

p = np.array([0.0, 0.0, 0.0])  # facing east
q = np.array([2.0, 2.0, 0.0])  # same heading, offset diagonally

# Geodesics now prefer to turn-and-drive rather than slide sideways
d = se2_car.distance(p, q)
print("car-like distance:", d)

With \(w_y = 100\), a direct sideways displacement is 10x more expensive than driving forward. The geodesic will instead steer and drive, a smooth approximation to Dubins-like paths, without any explicit non-holonomic constraint (see [Belta and Kumar, 2002, Kyaw and Kelly, 2026]).

Changing the Metric

The metric is a swappable policy, i.e., changing it changes what “distance” and “shortest path” mean on the manifold, without touching the underlying manifold structure. This is one of the most powerful ideas in geodex: the same set of points, the same topology, but a completely different geometry.

ConstantSPDMetric

The simplest way to change the metric is with ConstantSPDMetric<N>, which defines a point-independent inner product using a symmetric positive-definite (SPD) matrix \(A\):

\[\langle u, v \rangle = u^\top A\, v.\]

For example, setting \(A = \mathrm{diag}(4, 1, 1)\) makes the first coordinate “count” four times as much as the others when measuring lengths:

Eigen::Matrix3d A = Eigen::Vector3d(4, 1, 1).asDiagonal();
geodex::ConstantSPDMetric<3> weighted{A};

geodex::Euclidean<3, geodex::ConstantSPDMetric<3>> R3_weighted{weighted};

Eigen::Vector3d p{0, 0, 0};
Eigen::Vector3d u{1, 0, 0};
Eigen::Vector3d v{0, 1, 0};

double ip = R3_weighted.inner(p, u, v);  // 0.0  (still orthogonal)
double n  = R3_weighted.norm(p, u);      // 2.0  (scaled by √4)

Eigen::Vector3d q{1, 1, 1};
double d = R3_weighted.distance(p, q);
std::cout << "d = " << d << "\n";  // √(4 + 1 + 1) = √6 ≈ 2.449

A = np.diag([4., 1., 1.])
weighted = geodex.ConstantSPDMetric(A)

R3_weighted = geodex.ConfigurationSpace(geodex.Euclidean(3), weighted)

p = np.array([0., 0., 0.])
u = np.array([1., 0., 0.])
v = np.array([0., 1., 0.])

ip = R3_weighted.inner(p, u, v)  # 0.0  (still orthogonal)
n  = R3_weighted.norm(p, u)      # 2.0  (scaled by √4)

q = np.array([1., 1., 1.])
d = R3_weighted.distance(p, q)
print("d =", d)  # √(4 + 1 + 1) = √6 ≈ 2.449

Compare this to the standard metric, where the distance from the origin to \((1,1,1)\) is \(\sqrt{3} \approx 1.732\). The weighted metric stretches space along the first axis, making that direction “more expensive” to traverse.

Note

In C++, the metric is a compile-time template parameter, that is, Euclidean<3, ConstantSPDMetric<3>> constructs a manifold with the custom metric baked in. In Python, dimension and metric are runtime values, so use ConfigurationSpace(base_manifold, metric) to compose any base manifold’s topology with any metric. The resulting object supports all the same operations (inner, norm, distance, exp, log, geodesic); geometry comes from the metric, topology from the base.

Metrics Are Manifold-Agnostic

Because metrics are standalone policy types, the same ConstantSPDMetric<3> can be used on different manifolds. Here we use it on the sphere:

Eigen::Matrix3d A = Eigen::Vector3d(4, 1, 1).asDiagonal();
geodex::ConstantSPDMetric<3> weighted{A};

geodex::Sphere<2, geodex::ConstantSPDMetric<3>> sphere_weighted{weighted};

Eigen::Vector3d p{0, 0, 1};
Eigen::Vector3d u{1, 0, 0};

double n = sphere_weighted.norm(p, u);
std::cout << "n = " << n << "\n";  // 2.0  (same weighting)

A = np.diag([4., 1., 1.])
weighted = geodex.ConstantSPDMetric(A)

sphere_weighted = geodex.ConfigurationSpace(geodex.Sphere(), weighted)

p = np.array([0., 0., 1.])
u = np.array([1., 0., 0.])

n = sphere_weighted.norm(p, u)
print("n =", n)  # 2.0  (same weighting)

The metric changes how we measure lengths on the sphere, but the sphere’s topology and retraction (how we step along it) remain unchanged.

Tip

The metric determines what we measure (inner products, norms, distances), while the retraction determines how we move (exp/log). These are independent choices. The next section explores swapping the retraction.

Swapping Retractions

The exponential and logarithmic maps can be expensive to compute, especially on manifolds where they involve transcendental functions. A retraction is a cheaper approximation that agrees with the true exponential map to first order (or higher). In geodex, the retraction is a separate policy type that can be swapped independently of the metric.

On the sphere, the projection retraction simply normalizes \(p + v\) instead of tracing a great circle:

\[R_p(v) = \frac{p + v}{\|p + v\|}.\]

This is cheaper than the true exponential map (no trigonometric functions) but only a first-order approximation:

using ProjSphere = geodex::Sphere<2, geodex::SphereRoundMetric,
                                  geodex::SphereProjectionRetraction>;
ProjSphere sphere;

Eigen::Vector3d p{0, 0, 1};
Eigen::Vector3d q{1, 0, 0};

Eigen::IOFormat fmt(4, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

// log + exp with projection retraction: approximate round trip
auto v       = sphere.log(p, q);
auto q_approx = sphere.exp(p, v);
std::cout << "q_approx = " << q_approx.transpose().format(fmt) << "\n";  // ≈ [0.707, 0, 0.707]

sphere = geodex.Sphere(retraction="projection")

p = np.array([0., 0., 1.])
q = np.array([1., 0., 0.])

# log + exp with projection retraction: approximate round trip
v = sphere.log(p, q)
q_approx = sphere.exp(p, v)
print("q_approx =", q_approx)  # ≈ [0.707, 0, 0.707]

The metric is unchanged, only the retraction is swapped. Calls to inner() and norm() still use SphereRoundMetric.

SE(2) Retractions

SE(2) offers two retraction policies:

Retraction	Order	Description
`SE2ExponentialMap`	Exact	True Lie group exponential using \(V(\omega)\) matrix (default)
`SE2EulerRetraction`	1st	Component-wise addition with angle wrapping; treats SE(2) as \(\mathbb{R}^2 \times S^1\)

Creating SE(2) with the Euler retraction follows the same pattern as the sphere:

// SE(2) with Euler retraction (1st order, cheapest)
using SE2Euler = geodex::SE2<geodex::SE2LeftInvariantMetric,
                             geodex::SE2EulerRetraction>;
SE2Euler se2_euler;

# SE(2) with Euler retraction (1st order, cheapest)
se2_euler = geodex.SE2(retraction="euler")

Note

The choice of retraction affects the accuracy of distance() and geodesic() because both operations build on exp/log internally. When high accuracy matters, use the true exponential map. When speed matters more than exactness (e.g. inside a motion planning algorithm that calls distance() millions of times), a cheaper retraction may be preferable.

Summary and Next Steps

We have covered the fundamental operations of geodex:

Creating manifolds — Sphere<>, Euclidean<N>, Torus<N>, and SE2<>
Riemannian inner product — inner(p, u, v) and norm(p, v)
Exponential and logarithmic maps — exp(p, v) and log(p, q)
Geodesic distance — distance(p, q)
Geodesic interpolation — geodesic(p, q, t)
Random sampling — random_point()
Swapping metrics and retractions — policy-based template parameters

The table below collects all operations for quick reference:

Operation	Code	Math
Inner product	`m.inner(p, u, v)`	\(\langle u, v \rangle_p\)
Norm	`m.norm(p, v)`	\(\\|v\\|_p\)
Exponential map	`m.exp(p, v)`	\(\exp_p(v)\)
Logarithmic map	`m.log(p, q)`	\(\log_p(q)\)
Distance	`m.distance(p, q)`	\(d(p, q)\)
Geodesic	`m.geodesic(p, q, t)`	\(\exp_p(t \cdot \log_p(q))\)
Random point	`m.random_point()`
Dimension	`m.dim()`	\(\dim(\mathcal{M})\)

To see these operations in action on a real robotics problem, continue to the Minimum-Energy Planning on Configuration Manifolds tutorial, where we use position-dependent metrics to plan minimum-energy motions for a planar manipulator.

For the full API reference, see API Reference.

References

[BK02]

Calin Belta and Vijay Kumar. Euclidean metrics for motion generation on SE(3). Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, 216(1):47–60, 2002.

[KK26] (1,2)

Phone Thiha Kyaw and Jonathan Kelly. Geometry-aware sampling-based motion planning on Riemannian manifolds. In Proceedings of the 17th World Symposium on the Algorithmic Foundations of Robotics (WAFR). Oulu, Finland, Jun. 15–17 2026. To Appear. URL: https://arxiv.org/abs/2602.00992.