Discrete Geodesic Interpolation

discrete_geodesic is the core interpolation function in geodex. Given a manifold \(\mathcal{M}\), a start point \(q_s\), and a target \(q_t\), it returns a sequence of points on \(\mathcal{M}\) approximating a length-minimizing geodesic between them. The same subroutine drives midpoint distance estimation, edge interpolation in motion planners, and any code path that needs a geodesic on a manifold whose true logarithm is unavailable, expensive, or inconsistent with the metric in use. This page explains how the algorithm works, the design decisions behind it, how to tune its parameters, and how to call it from both C++ and Python.

Problem statement

Discrete geodesic iterates on a convex manifold with tangent spaces and descent directions.

We minimize the squared Riemannian distance to the target,

\[\varphi(x) \;=\; \tfrac{1}{2}\, d_g^2(x,\, q_t),\]

starting from \(x_0 = q_s\) and taking Riemannian gradient steps. Every successful iterate is recorded as a waypoint, so on convergence the returned path traces a discrete approximation of the geodesic from \(q_s\) to \(q_t\).

At each iterate \(x_k\), the descent direction lives in the tangent space \(\mathcal{T}_{x_k}\mathcal{M}\) and points along \(-\log_{x_k}(q_t)\). The next iterate \(x_{k+1}\) is obtained by retracting back to \(\mathcal{M}\) along that direction, capped at the current step length.

How the algorithm works

Each iteration computes a descent direction in \(\mathcal{T}_x\mathcal{M}\), caps its Riemannian length at the current step budget, and moves to the next iterate through the manifold’s retraction.

Fast path

When the manifold’s log is the Riemannian logarithm of the metric in use, the gradient of \(\varphi\) has a closed form:

\[\nabla_g \!\left[ \tfrac{1}{2}\, d_g^2(\cdot,\, q_t) \right]\!(x) \;=\; -\log_x(q_t).\]

The algorithm uses this direction directly, normalizes it, scales by the current step cap, and retracts. This is by far the cheapest path: one log, one retract, and a progress check per iteration.

Whether this fast path executes is decided by the resolver is_riemannian_log(m), which collapses a compile-time trait (M::has_riemannian_log) and a runtime hook (m.has_riemannian_log_runtime()) into a single boolean. Built-in manifolds like Sphere, Euclidean, Torus, and SE2 under their standard metrics return true at compile time, while ConfigurationSpace and callable metrics resolve the flag at runtime by asking whether the attached metric matches the base manifold’s log. Manifolds that do not opt in always fall through to the finite-difference path below.

There are cases where the closed-form direction is still geometrically useful even when the chosen metric is not the one implied by log. Setting force_log_direction = true skips this resolver and always uses \(-\log_x(q_t)\) as the descent direction; the metric’s norm and distance continue to drive step sizing and convergence, but the resulting path follows the base retraction’s geodesic rather than the Riemannian geodesic of the configured metric.

Note

The identity \(\nabla_g \tfrac{1}{2} d_g^2 = -\log\) is standard; see [Lee, 2018].

Finite-difference fallback

When log is not the Riemannian logarithm of the chosen metric (anything built on ConstantSPDMetric, KineticEnergyMetric, JacobiMetric, PullbackMetric, or any callable metric), or when the closed-form direction from fast path fails the progress check, the iteration falls back to a finite-difference natural gradient computed from the metric’s inner product:

Build an orthonormal tangent basis \(\{e_i\}\) at the current point. If the manifold exposes a project method, ambient seed vectors are projected to \(\mathcal{T}_x\mathcal{M}\) before Gram-Schmidt orthonormalization.
Assemble the metric tensor in this basis, \(G_{ij} = g_x(e_i, e_j)\). When the metric provides a batched inner_matrix, the whole Gram matrix is filled in one call.
Estimate the coordinate-space gradient \(g_i = \partial_{e_i} \varphi(x)\) by central finite differences along each basis direction. The \(d_g^2\) samples use a third-order-accurate midpoint surrogate guarded by the Riemannian-midpoint identity \(\log_m(a) = -\log_m(b)\); when the relative deviation exceeds fd_midpoint_guard_tau, the sample falls back to \(\|\log_a(b)\|_g\) for that basis direction and the count is reported on InterpolationResult::fd_midpoint_fallbacks.
Solve \(G\,\alpha = -g\) via Cholesky. The natural gradient in ambient coordinates is \(v = \sum_i \alpha_i\, e_i\).

The fallback engages on a per-step basis, so a single walk can mix fast-path and finite-difference iterations as the geometry demands.

Adaptive step control and termination

A retraction is only an approximation of the exponential map, and on a curved manifold an aggressive step can either overshoot the target or land somewhere whose realized length differs noticeably from the requested length. After each candidate step, the algorithm measures \(\|\log_x(x_{\text{next}})\|_g\) and compares it to the requested step length. The fallback is two-stage: when the fast-path candidate fails either due to the progress check or the distortion ratio, the iteration falls through to the finite-difference path at the same step cap, without counting a halving. When the FD candidate also fails, the step cap is halved, the iteration retries, and distortion_halvings increments. After a successful step, the cap regrows by growth_factor until it reaches step_size again. This trust-region behavior keeps the walk stable under heavy curvature without forcing the user to pick a tiny global step size.

The loop ends with one of the following statuses:

Converged: The Riemannian distance to the target dropped below convergence_tol or below convergence_rel * initial_distance. The returned path ends at, or very close to, target.
MaxStepsReached: The iteration budget was exhausted. The path is still a valid descent sequence, but it has not reached the target. Inspect final_distance to decide whether it is good enough.
GradientVanished: The Riemannian gradient norm collapsed at a non-target point. This is rare in practice and usually indicates that the metric or finite-difference step is misconfigured.
CutLocus: log returned (numerically) zero while the ambient gap to the target is nonzero. The classic example is exact antipodal points on the sphere, where the logarithm is multivalued. This is the correct response, not a bug.
StepShrunkToZero: The distortion guard halved the step cap below min_step_size. This usually means the retraction is incompatible with the metric in this neighborhood.
DegenerateInput: start and target were the same point (within tolerance) at entry.

Beyond the status, the result carries diagnostic counters that are useful for understanding how the walk went. iterations is the number of accepted steps, equal to the returned path length minus one. distortion_halvings counts how many times the FD path forced a step-cap halving, and a nonzero value under an otherwise fast-path-eligible manifold signals a poor retraction-metric match in this neighbourhood. fd_midpoint_fallbacks counts how many FD basis samples the Riemannian-midpoint guard rejected, which flags non-Riemannian retractions, cut-locus crossings, or non-smooth metric features within the finite-difference neighbourhood.

Tuning the parameters

Every parameter lives on InterpolationSettings. Defaults are sensible for moderate problems on the unit sphere; you should expect to revisit step_size and max_steps for tighter state spaces or heavier metrics.

Parameter	Default	Effect
`step_size`	0.5	Maximum Riemannian step per iteration. Also the effective path resolution: consecutive returned points are at most `step_size` apart in the metric. Smaller values give a denser path and a smoother walk under aggressive curvature, at the cost of more iterations.
`convergence_tol`	1e-4	Absolute stop threshold on the Riemannian distance to the target.
`convergence_rel`	1e-3	Relative stop threshold; the walk also stops when the distance drops below `convergence_rel * initial_distance`. Useful when the working scale of the problem is much larger or smaller than the absolute tolerance.
`max_steps`	100	Successful gradient steps before giving up. Distortion retries do not count.
`force_log_direction`	false	Force the log-based descent direction even when `is_riemannian_log(m)` would return false. The metric still drives norm, distance, and convergence, but the path follows the base retraction’s geodesic rather than the Riemannian geodesic of the configured metric. Use when the FD fallback’s natural oscillation is visible and a smooth path matters more than strict metric fidelity.
`fd_epsilon`	0.0	Central finite-difference step. Zero auto-selects \(\max(10^{-8},\, 10^{-5} \cdot \max(1, d_0))\) from the initial distance, which is the right choice in nearly all cases.
`fd_midpoint_guard_tau`	0.25	Relative-error threshold above which the midpoint-distance surrogate used inside the FD gradient is rejected and the sample falls back to \(\\|\log\\|_g\) for that basis direction. Lower values are stricter; set to 0 to force the log-based sample on every basis direction.
`distortion_ratio`	1.5	How much the realized step length is allowed to exceed the requested length before the retraction is considered to have overshot. Lower this for retractions that drift visibly from the exponential map.
`growth_factor`	1.5	How quickly the step cap regrows after a successful iteration. Set to `1.0` to disable growth and keep the cap fixed at whatever the distortion guard last permitted.
`min_step_size`	1e-12	Floor on the step cap. The walk fails with `StepShrunkToZero` once it is crossed.
`gradient_eps`	1e-12	Riemannian-norm threshold below which the gradient is considered vanished.
`cut_locus_eps`	1e-10	Threshold on \(\\|\log_x(q_t)\\|_g\) that, combined with a nonzero ambient gap, flags a cut-locus situation.

In day-to-day use, the three parameters worth reaching for first are step_size, convergence_tol, and distortion_ratio. step_size has the most impact: halving it doubles both the path resolution and the iteration count, but it also makes the walk far more tolerant of curvature and metric anisotropy. convergence_tol and convergence_rel together set how tightly the final iterate must approach the target; loosen them when the downstream consumer only needs a coarse path. distortion_ratio is the safety valve for retractions that are not isometries, such as SphereProjectionRetraction under an anisotropic metric or SE2EulerRetraction away from \(\theta = 0\).

For hot loops, pass an InterpolationCache to reuse the basis matrix, Gram matrix, and gradient buffers across calls. The cache is resized once on first use and then avoids all per-iteration heap allocations.

Worked example: \(\mathbf{S}^2\) with an anisotropic metric

The example below runs discrete_geodesic twice on the unit 2-sphere between the north pole and a target in the upper hemisphere. The first call uses the default round metric, so the fast path executes and the walk traces the great circle exactly. The second call swaps in a constant SPD metric \(A = \mathrm{diag}(25, 1, 1)\) that heavily penalizes motion in the \(x\) direction; the finite-difference fallback runs, and the resulting path bends visibly away from the great circle in order to spend less length along the penalized axis.

#include <Eigen/Core>
#include <geodex/geodex.hpp>

using namespace geodex;

int main() {
  const Eigen::Vector3d start(0.0, 0.0, 1.0);
  const Eigen::Vector3d target(
      std::sin(1.3) * std::cos(0.5),
      std::sin(1.3) * std::sin(0.5),
      std::cos(1.3));

  InterpolationSettings s;
  s.step_size = 0.05;
  s.max_steps = 500;

  // 1. Round sphere — fast path, traces the great circle.
  Sphere<> round_sphere;
  auto great = discrete_geodesic(round_sphere, start, target, s);

  // 2. Anisotropic constant-SPD metric — finite-difference fallback.
  Eigen::Matrix3d A = Eigen::Matrix3d::Identity();
  A(0, 0) = 25.0;
  Sphere<2, ConstantSPDMetric<3>> stretched{ConstantSPDMetric<3>{A}};
  auto bent = discrete_geodesic(stretched, start, target, s);

  // great.path and bent.path are std::vector<Eigen::Vector3d> waypoints.
}

import numpy as np
import geodex

start  = np.array([0.0, 0.0, 1.0])
target = np.array([
    np.sin(1.3) * np.cos(0.5),
    np.sin(1.3) * np.sin(0.5),
    np.cos(1.3),
])

settings = geodex.InterpolationSettings(step_size=0.05, max_steps=500)

# 1. Round sphere — fast path, traces the great circle.
round_sphere = geodex.Sphere()
great = geodex.discrete_geodesic(round_sphere, start, target, settings)

# 2. Anisotropic constant-SPD metric, attached via ConfigurationSpace.
A = np.diag([25.0, 1.0, 1.0])
stretched = geodex.ConfigurationSpace(round_sphere, geodex.ConstantSPDMetric(A))
bent = geodex.discrete_geodesic(stretched, start, target, settings)

The two paths visualised on \(\mathbb{S}^2\):

Discrete geodesic walks on the 2-sphere under round and anisotropic metrics.

The blue curve is the great circle path returned by the fast path under the round metric. The orange curve is the natural-gradient walk under \(A = \mathrm{diag}(25, 1, 1)\); both endpoints are identical, but the second path leaves the great circle to favour motion along \(y\) and \(z\), where the metric is cheaper.

Common pitfalls

Warning

Anisotropic metrics combined with first-order retractions such as SphereProjectionRetraction rely on the distortion guard to stay stable. Do not raise distortion_ratio past 2 unless you have measured what the retraction actually does in your neighborhood.
Near-antipodal inputs on the sphere may legitimately terminate with CutLocus. The logarithm is multivalued there, and no descent direction is well defined. Pre-split the problem if you need to traverse the cut.
The default step_size = 0.5 is large for tight state spaces or heavy metrics. If you see MaxStepsReached or many distortion halvings in distortion_halvings, halve step_size first and re-run.
Always check result.status before consuming result.path. A walk that stopped on MaxStepsReached still returns a valid descent sequence, but its last point is not the target.
SE(2) under an anisotropic or clearance-based metric tends to produce a visibly bumpy path because the FD natural gradient reacts to small-scale metric variation between samples. When a smooth path matters more than strict Riemannian fidelity, set force_log_direction = true and inspect fd_midpoint_fallbacks to confirm the FD guard was indeed tripping on the original run.

References

Full details are in our WAFR 2026 paper [Kyaw and Kelly, 2026]. This page is a usage-oriented summary of the same algorithm.

[KK26]

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.

[Lee18]

John M. Lee. Introduction to Riemannian Manifolds. Springer, 2nd edition, 2018.