Technical Summary: Smoothing Out the Edges

Problem Statement

The paper addresses the challenge of continuous-time state estimation, where the goal is to estimate a trajectory $x(t)$ as a continuous function of time based on discrete, potentially asynchronous measurements $y_k$ at times $t_k$. While discrete-time estimation requires explicitly defining a state variable for every measurement or time step, this approach struggles with high-rate sensors (e.g., LiDAR, IMU), asynchronous data, and the need to query the system state at arbitrary times for downstream tasks like control or mapping.

Existing continuous-time approaches generally fall into two paradigms:

1. Parametric: Representing the trajectory using basis functions (e.g., splines). These require careful selection of basis functions to avoid underfitting or overfitting.
2. Nonparametric: Using Gaussian Process (GP) regression. While technically advantageous for its inherent regularization and flexibility, GP-based continuous-time estimation has seen less adoption in robotics, partly due to a lack of intuitive integration with the dominant factor graph estimation paradigm.

Methodology

The authors propose a framework that unifies GP regression with factor graphs, specifically tailored for robotics applications involving Lie groups (e.g., SE(3)).

1. Factor Graph Perspective on GP Regression

The core theoretical contribution is reframing GP continuous-time estimation through the lens of factor graphs.

• Decomposition: The estimation problem is decomposed into two distinct steps:
  • Main Solve: Solving for the states at the discrete measurement times ($t_k$) using standard discrete-time factor graph tools.
  • Interpolation (Querying): Computing the state at arbitrary query times ($\tau$) after the main solve is complete.
• Elimination Algorithm: The authors demonstrate that querying the trajectory at any time $\tau$ is equivalent to applying the factor-graph elimination algorithm to remove the query state $x_\tau$. This reveals that the query depends only on the marginal distribution of the "separator" states (the measurement states immediately preceding and following $\tau$).
• Computational Efficiency: By selecting a kernel function derived from a linear time-invariant (LTI) stochastic differential equation (SDE), such as the White Noise on Acceleration (WNOA) model, the inverse kernel matrix becomes exactly block-tridiagonal. This allows the main solve to be performed in $O(K)$ time (linear in the number of measurements) rather than $O(K^3)$. Furthermore, the querying step becomes an $O(1)$ operation, requiring only the marginal statistics of the neighboring states.

2. Extension to Lie Groups

To handle robot poses (rotations and translations) which reside on manifolds (Lie groups like SE(3)), the authors adapt the linear GP approach:

• Local GP Variables: Instead of a single global GP, they define local variables in the Lie algebra around each measurement time. Specifically, $\xi_k(t) = \text{Log}(T_k^{-1}T(t))$, where $T_k$ is the pose at measurement $k$.
• Linearization: A linear motion model (e.g., WNOA) is applied to these local variables. The non-linear relationship between the Lie group elements and the local variables is handled via standard linearization techniques (perturbation models) within an iterative solver (e.g., Gauss-Newton or Levenberg-Marquardt).
• Covariance Interpolation: A novel method is introduced for interpolating covariances on Lie groups that is simpler than previous methods (e.g., Anderson, 2016) and more efficient than others (e.g., Barfoot, 2024).

3. Handling Measurements on Interpolated States

A significant practical challenge is handling high-rate measurements that occur at times where no state is explicitly optimized.

• State Elimination: The authors show how to eliminate states associated with measurements from the optimization graph entirely.
• Residual Factors: Eliminating an interpolated state with a measurement results in two factors between the bordering states: a motion prior factor and a new measurement factor.
• Independence Approximation: To maintain a sparse graph structure, the authors introduce an approximation where cross-covariances between multiple interpolated states are dropped. This allows multiple measurements to be treated as independent factors on the bordering states, significantly reducing the optimization problem size. They also discuss a "simplified noise model" that omits certain covariance terms for further speed, noting the trade-off in accuracy and confidence.

Implementation

The methodology is implemented in GTSAM (Georgia Tech Smoothing and Mapping), a popular C++ library for factor graph optimization.

• New Factors: The authors introduce WnoaMotionFactor (for motion priors) and WnoaInterpFactor (a wrapper that converts factors defined on interpolated states to factors on bordering states).
• Workflow: Users can define a graph with "border states" (optimized) and "interpolated states" (eliminated). The library automatically constructs the equivalent graph, solves it, and provides a post-processing step (updateInterpValues) to recover the full continuous trajectory and covariances.
• Optimization: A specialized WnoaFactorGraph class is implemented to cache interpolated state means and Jacobians, preventing redundant computation during iterations.

Results and Evaluation

The framework is evaluated on three datasets ranging from 1D to 3D:

1. 1D (Giant Glass of Milk): A robot moving linearly with laser range and odometry. Results show that the continuous-time approach produces smooth, accurate trajectories even with sparse measurements (up to 7.5s intervals), and that the "reduced" solve (with interpolation) yields identical results to the "full" solve in linear cases.
2. 2D (Lost in the Woods): A SLAM problem with a wheeled robot and landmarks. The study demonstrates that optimizing only 1 in 30 states (interpolating the rest) recovers a trajectory nearly identical to the full solve. Quantitative analysis shows a trade-off: while the "simplified noise model" offers significant speedups, it can lead to overconfident estimates and slightly degraded mean accuracy compared to the "full noise model."
3. 3D (Starry Night): A localization problem in SE(3) using stereo cameras and IMU data. Even with 80% fewer linearization points (optimizing every 5th state), the interpolated solution maintains accuracy and consistency comparable to the full solve, despite the non-linearities involved.

Significance and Claims

The paper claims to rectify the situation where GP-based continuous-time estimation is underutilized in robotics by providing:

• New Insights: A clear connection between GP regression and factor graphs, showing that trajectory querying is simply an application of the elimination algorithm. This provides a novel, intuitive way to perform covariance interpolation.
• Practical Accessibility: A working implementation in GTSAM that allows practitioners to easily adopt continuous-time estimation without needing to derive complex kernel matrices or interpolation equations from scratch.
• Efficiency: A demonstration that continuous-time estimation can handle high-rate, asynchronous sensors efficiently by reducing the number of optimization variables without sacrificing trajectory smoothness or accuracy.

The authors position this work as a tutorial and a practical guide, aiming to lower the barrier to entry for continuous-time estimation in robotics. They acknowledge limitations, such as the need for heuristic selection of estimation times and the use of local GPs on Lie groups (which can introduce artifacts), but argue that the approach offers a robust and flexible alternative to parametric methods.