Factor Graph Optimization

Follow this topic Ask about this topic

Momentum

4 papers in the last four weeks, against 1 the four weeks before. 0.0% of all new papers.

Jul 13Week of Sep 28

Latest papers 21

Oct 8, 2026cs.RO

GLIO2: A GPU-Parallelized Tightly-Coupled LiDAR-Inertial-GNSS System for Robust and Real-Time Global Localization and Mapping

Globally consistent, real-time state estimation in large-scale, perceptually degraded environments is essential for autonomous vehicles and aerial robots, and requires fusing LiDAR, inertial, and GNSS measurements. Existing fusion methods, however, share a scan-to-map front-end with two failure modes. First, each scan is aligned to an incrementally built map that drifts under degeneracy, and once the estimate diverges the error is irrecoverable. Second, even without divergence, a registration biased by dynamic objects or wrong correspondences is propagated as a single pose constraint with an over-confident covariance, leaving its correspondences unavailable for GNSS to re-weight or relinearize. We propose GLIO2, a tightly-coupled LiDAR-Inertial-GNSS system whose GPU-parallel front-end jointly optimizes scan-to-multiscan LiDAR, IMU pre-integration, and raw GNSS measurements in a single sliding-window factor graph, sustaining real-time operation on edge hardware. A complementary offline back-end reuses the same cached factors to refine the entire trajectory in batch, completing the 30-min, 4.51-km UrbanNav Whampoa sequence in about 24 s. Across three public benchmarks (UrbanNav, MARS-LVIG, M3DGR) and self-collected UAV and vehicle data, GLIO2 attains the best overall accuracy among evaluated systems. On a 5.66-km bridge traversed at up to 96 km/h, where every competing baseline diverges under LiDAR degeneracy, it maintains 1.6 m horizontal accuracy. On an NVIDIA Jetson Orin NX, the full pipeline runs at about 25 Hz (39.60 ms per scan). The source code and datasets will be released.

Qi Zhang, Xikun Liu, Qijun Qin +5

Department of Aeronautical and Aviation Engineering, The Hong Kong Polytechnic University, Hong Kong SAR, China

Simultaneous Localization and Mapping LiDAR Localization Multimodal Sensor Fusion Factor Graph Optimization

Oct 4, 2026cs.CV

F $^2$ SLAM: Turning Feed-Forward Geometry into Persistent Factors for SLAM

Feed-forward 3D models provide strong multi-view geometric priors, while on- line simultaneous localization and mapping (SLAM) relies mainly on local mea- surements and can accumulate drift over long sequences. Existing attempts to combine the two typically treat feed-forward predictions as an external geomet- ric state that is aligned or fused with the online estimate after the fact, which keeps broader multi-view evidence outside the optimizer that refines the SLAM state. We present F2SLAM, which instead converts feed-forward geometry di- rectly into optimization-native target-weight measurements attached to a persis- tent dense factor graph. A high-frequency stream maintains local tracking con- straints and graph connectivity, while a low-frequency stream uses wider multi- view context to selectively refresh existing measurements after a state-consistency check. Both streams constrain the same poses, inverse depths, and optional cam- era intrinsics through a single dense bundle adjustment. Experiments on multiple benchmarks demonstrate consistently strong trajectory estimation and improved dense reconstruction in both calibrated and uncalibrated settings. Notably, the uncalibrated configuration reduces the average ATE RMSE from 0.030 m for the strongest feed-forward baseline to 0.002 m on the Replica dataset.

Zhisong Xu, Fan Zhu, Jiawei Qian +3

The University of Tokyo · Tohoku University · University of Science and Technology of China +1

Simultaneous Localization and Mapping Factor Graph Optimization

Sep 17, 2026cs.RO

Bayesian Continuum Robot Dynamics and State Estimation

Recent factor graph approaches to continuum robot state estimation have been successful for quasi-static applications and spatiotemporal estimation using white-noise kinematic motion priors. However, when inertial effects are significant, these approximations may fail to capture the underlying physics, limiting accuracy during dynamic motions. In contrast, our approach approximates the Cosserat rod dynamics of continuum robots. We write inertia and damping as equivalent applied loads, so that the dynamic balance retains the algebraic form of the static one from prior work with quasi-static robots. Without backbone observations, the framework reduces to a stochastic forward simulation of the robot's motion. Given observations, it jointly refines kinematic and dynamic states and infers external loads, among other states. We validate the approach through simulation and experiments, demonstrating stochastic forward simulation as well as state estimation on tendon-driven continuum robots.

James M. Ferguson, Tucker Hermans, Alan Kuntz

Department of Electrical and Computer Engineering and Department of Computer Science, Vanderbilt University, TN, USA. · Kahlert School of Computing and Robotics Center, University of Utah, UT, USA.

Factor Graph Optimization Robotics Robot State Estimation

Sep 15, 2026cs.RO

Multi-Session Multimodal Underwater Mapping with Acoustic and Optical Imaging

Accurate seafloor mapping is essential for marine science, archaeology, and environmental monitoring. However, integrating data from different sensors, such as side-scan sonar and optical cameras, collected across separate survey sessions, remains challenging due to positioning drift and sensor offsets. This paper presents a multi-session, multimodal underwater mapping framework based on factor graph optimization. The method jointly optimizes vehicle trajectories, 3D landmark positions, sensor extrinsics, and per-session global alignment transformations. By combining rigid inter-session corrections with local trajectory deformations, it compensates for both inter-session offsets and intra-session distortions from accumulated navigation errors. The proposed methodology was validated on real-world datasets collected along the Catalan coast. Results show measurable improvements in map consistency over both unoptimized and rigid-alignment baselines across all metrics, including Pixel Accuracy and mean Intersection over Union. The method achieves a 3.4% improvement in pixel accuracy over the unoptimized baseline, corresponding to improved semantic labelling across approximately 14700

\text{m}^2

of mapped area. Qualitative results further show consistent co-registration between sonar and optical maps, even in the presence of significant trajectory distortions and inter-session misalignments. These findings demonstrate the potential of the proposed framework to generate coherent multimodal seafloor maps from heterogeneous underwater surveys.

Precious Philip-Ifabiyi, Valerio Franchi, Fausto Ferreira +1

Computer Vision and Robotics Research Institute (ViCOROB), University of Girona, Campus Montilivi, Edifici P4, Girona, 17003, Catalonia, Spain · University of Zagreb Faculty of Electrical Engineering and Computing, Zagreb, Croatia · University of Zagreb Faculty of Electrical Engineering and Computing, Laboratory for Underwater Systems and Technologies (LABUST), Zagreb, Croatia +1

Factor Graph Optimization Cross-Modal Image Registration Multimodal Sensor Fusion

Sep 15, 2026cs.RO

Differentiable Mesh State Estimation via Factor Graph Inference for Deformable Object Reconstruction

Estimating deformable object states remains a fundamental challenge in robotics and simulation. We propose a novel factor graph-based framework for probabilistic mesh state estimation of deformable objects. The method directly updates a tetrahedral mesh, a rich and physically-grounded representation of an environment, by combining physics priors, noisy sensor measurements, and temporal smoothness constraints within a unified probabilistic formulation. The estimation problem is posed as a nonlinear least-squares optimization and solved using Levenberg-Marquardt. Ex vivo central-airway obstruction experiments and simulations on deforming cube models demonstrate reliable and accurate reconstruction under both rigid motion and deformation, highlighting the potential of this probabilistic approach for principled, measurement-driven mesh state estimation in deformable object reconstruction.

Lidia Al-Zogbi, Fangjie Li, Samuel Tobin +13

Department of Electrical and Computer Engineering, Tufts University, Medford, MA 02155, USA

Factor Graph Optimization Deformable Shape Reconstruction 3D Mesh Reconstruction

Aug 10, 2026cs.LG

Beyond Binary: Continuous State Optimization with Graph-Structured Objectives

Large-scale learning systems often face the challenge of balancing multiple, potentially competing objectives, such as fairness, accuracy, and latency. While recent work has formalized this as an optimization problem over binary states, many real-world control parameters, such as fairness thresholds, diversity mixing rates, or resource budgets, are continuous. In this work, we extend the framework to \emph{continuous state spaces}. We model the problem as minimizing a sum of linear objectives subject to \emph{movement costs} that penalize system instability. We capture the local structure of the objectives using a \emph{dependency graph} (or factor graph), where each objective is determined by a subset of the state attributes. To address the tension between exploration and stability, we propose \emph{Lazy Graph-LinUCB}, an algorithm that performs lazy updates to minimize switching costs while maintaining near-optimal regret. Beyond stability, we introduce three advanced mechanisms to exploit the underlying graph structure: (1) an \emph{asynchronous} update schedule that eliminates synchronization overhead in sparse graphs; (2) an \emph{adaptive} algorithm that learns the graph structure from data; and (3) a \emph{joint estimator} that leverages data sharing among correlated objectives to significantly tighten regret bounds. Empirically, we demonstrate that these structural exploitations reduce movement costs by more than a factor of three in heterogeneous systems while maintaining similar cumulative losses.

Corinna Cortes, Yishay Mansour, Mehryar Mohri

Google Research · Tel Aviv University · Courant Institute

Factor Graph Optimization Multi-Armed Bandits Graph Bandits

Jul 30, 2026stat.ML

Accelerated Random-Sweep Gibbs Sampling for Gaussian Graphical Models via Dual Normal Factor Graphs

We study the convergence properties of the random-sweep Gibbs sampler for Gaussian graphical models with a thin-membrane prior. We demonstrate that the convergence rate of the Gibbs sampler is significantly accelerated in the dual model, which is obtained by applying the Fourier transform to the local factors of the normal factor graph representing the original model. In both domains, we derive the exact convergence rates for homogeneous

k

-regular graphs. We prove that, for all homogeneous models whose graphical representations contain cycles, the convergence rate in the dual domain is universal and independent of the underlying graph topology. Moreover, we show that the effective convergence rate in the dual domain is governed by the algebraic connectivity of the graph, providing an additional acceleration without increasing the computational complexity per sweep. We further establish an explicit algebraic relation between the covariance structures of the primal and dual models, enabling marginal statistics of the primal model to be recovered directly from those of the dual model. Finally, numerical experiments on several graph families confirm our theoretical results and demonstrate substantial improvements in the convergence rates in various settings.

Borna Khodabandeh, Mehdi Molkaraie

Department of Statistics, University of Oxford, Oxford OX1 3LB, UK · Department of Signal Theory and Communications, UPC, 08034 Barcelona, Spain · Department of Statistical Sciences, University of Toronto, ON M5G 1X6, Canada

Factor Graph Optimization Markov Chain Monte Carlo Probabilistic Graphical Models

Jul 9, 2026cs.RO

Learning Adaptive Solvers for Distributed Factor Graph Optimization on Matrix Lie Groups

Modern robotic perception increasingly involves large-scale geometric optimization problems distributed across multiple robots or sessions. However, existing distributed solvers often depend on brittle hand tuning and primarily target rigid body pose graphs. To address this, we present DeepCORD, a learning-augmented framework for distributed factor graph optimization on general matrix Lie groups. By unfolding a parallel and accelerated Riemannian optimizer into differentiable iterations, DeepCORD learns a self-supervised feedback policy that dynamically adapts solver parameters according to the optimization phase and communication status. The resulting method enables adaptive distributed optimization over matrix Lie groups under both synchronous and asynchronous communication regimes. Extensive experiments on real-world

\mathrm{SE}

(3) pose graph optimization and

\mathrm{SL}

(4) projective submap alignment show that our method achieves lower objective values than existing distributed baselines on most benchmarks across realistic operating scenarios.

Jaeho Shin, Maani Ghaffari, Yulun Tian

University of Michigan

Factor Graph Optimization Riemannian Optimization Learning to Optimize Pose Graph Optimization Lie Group Methods

Jun 27, 2026cs.RO

J-LAW: Joint Localization and Action-Conditioned World Modeling via Coupled Latent Factor Graphs

Classical simultaneous localization and mapping (SLAM) estimates metric poses and a geometric map but does not provide an action-conditioned predictive state. Action-conditioned world models learn compact latent dynamics but ignore global metric consistency and accumulate drift under open-loop rollout. We introduce J-LAW (Joint Localization and Action-Conditioned World Modeling), a unified factor-graph formulation that connects metric pose variables, predictive latent states, and persistent latent landmarks in this letter.J-LAW represents each image as a compact predictive state and combines it with pose or motion measurements through a separately learned mapping. Its maximum a posteriori (MAP) factor graph enforces consistency between these complementary sources of information over time. Experiments on PushT and WildGS show that J-LAW's factor-graph representation can improve long-horizon latent consistency and recover more reliable predictive states under partial observations, forming a foundation for future integrated localization and planning systems.

Guanqun Cao, Liang Chen

Geely Technology Europe, Pumpgatan 1, 417 55 Gothenburg Sweden · State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing (LIESMARS), Wuhan University, China

Simultaneous Localization and Mapping Factor Graph Optimization World Model Learning World Models Action-Conditioned World Models

Jun 14, 2026cs.RO

A Smart-Scheduled Hybrid (SSH) EKF-FGO State Estimation

Reliable state estimation in robotics and control re quires balancing estimation accuracy against computational cost. While filtering-based methods such as the Extended Kalman Filter (EKF) provide efficient real-time updates, and optimisation based formulations using factor graphs improve global consistency, the role of optimisation scheduling is often treated implicitly rather than examined as an explicit design variable. This paper presents an experimental study that explicitly isolates optimisation scheduling using a Smart Scheduled Hybrid (SSH) EKF-FGO framework as a controlled testbed. By combining EKF-based state propagation with periodically invoked batch optimisation and holding solver structure and effort fixed, the main contribution of this work is the experimental characterisation of optimisation scheduling as an independent design variable governing the trade-off between intermediate estimation accuracy and computational cost. Simulation results in a planar SLAM environment show that scheduling strongly influences pre optimisation drift, transient error behaviour, and runtime. In particular, the results identify operating regimes in which most of the benefit of global optimisation can be retained at a fraction of the computational cost, highlighting optimisation scheduling as an under-explored yet critical consideration in hybrid state estimation systems.

Eric Levy, Soosan Beheshti

Electrical, Computer, and Biomedical Engineering Toronto Metropolitan University Toronto, ON, Canada

Factor Graph Optimization Robot State Estimation

Jun 4, 2026cs.LG

Equivariant Neural Belief Propagation

Probabilistic inference over spatially embedded variables requires beliefs that respect

SE(3)

symmetry, yet existing equivariant networks produce only scalars and vectors -- not the rank-2 precision tensors needed for anisotropic uncertainty, and single-component messages collapse multi-modal energy landscapes to physically meaningless averages. We introduce Equivariant Neural Belief Propagation (ENBP), a factor-graph framework whose messages are equivariant Gaussian mixture models with sufficient statistics that transform exactly under

SE(3)

. Rank-2 precision matrices are synthesised via equivariant outer products, ingested through differentiable spectral decomposition, and kept tractable by a greedy KL-based mixture reduction that provably commutes with

SE(3)

. On GEOM-QM9 and GEOM-Drugs, ENBP achieves 98.9% conformational coverage at 0.090

\mathring{A}

error with sub-second latency -- over

100\times

faster than diffusion baselines at higher accuracy. On multi-body robotic inference, vanilla loopy BP diverges at 15+ agents while ENBP converges with near-zero collision rates and machine-precision equivariance error (

{\sim}10^{-7}

vs.\

10^{-1}

for augmented baselines).

Zehua Cheng, Wei Dai, Jiahao Sun

Department of Computer Science, University of Oxford, Oxford, United Kingdom · FLock.io, London, United Kingdom

Factor Graph Optimization Gaussian Mixture Models Bayesian Inference Equivariant Neural Networks Probabilistic Graphical Models Equivariant Representation Learning

May 28, 2026cs.RO

Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs

Robust and efficient state estimation is crucial for perception, navigation, and control in robotics. State estimation problems are conveniently modeled using the factor-graph framework as enabled by modern software packages such as GTSAM or g2o. However, the standard solvers included in such frameworks are local and may converge to poor local minima, posing significant safety concerns. Conversely, techniques based on convex relaxations have been shown to provide a means of globally solving or certifying many state estimation problems. However, these relaxations 1) often require substantial effort to formulate, and 2) may incur significantly higher cost compared to efficient local solvers, as they require solving a large semidefinite program (SDP). In this work, we address both shortcomings by 1) creating a new procedure within the GTSAM framework for automatically constructing convex SDP relaxations for any factor graphs with common factor and variable types, and by 2) exploiting the Bayes tree constructions native to GTSAM to decompose the SDP problem, leading to significant speedup in solver time for chordally sparse problems. We demonstrate the favorable scaling of this structure-exploiting global estimator compared to standard local solvers for two case studies: A 3D pose-graph SLAM problem with a ring factor graph and a 2D localization problem with a chain factor graph. The software framework is available at https://github.com/borglab/gtsam.

Avinash Subramanian, Connor Holmes, Timothy D. Barfoot +2

College of Computing, Georgia Institute of Technology, Atlanta, GA, USA · Robotics Institute, University of Toronto, Toronto, ON, Canada · Department of Mechanical Engineering, Carnegie Mellon University, Pittsburgh, PA, USA

Semidefinite Programming Factor Graph Optimization Constrained Optimization

May 28, 2026cs.LG

Composing Non-Conjugate Factor Graphs with Closed-Form Variational Inference

Stacking probabilistic building blocks into deeper architectures typically breaks closed-form inference. We show that closed-form inference can be preserved. We identify five factor-graph primitives: a bilinear factor, an exponential link, a Gamma prior, a Gaussian likelihood, and an equality node, and prove that any model composed from them admits closed-form variational message passing. The construction works because each primitive preserves a small set of message families: under mean-field factorization, messages on Gaussian variables remain Gaussian and messages on precision variables remain Gamma, while the only non-conjugate interface, the exponential link, remains tractable through the Gaussian moment-generating function and the sufficient statistics of the Gamma family. We demonstrate composition at increasing depth, from static ensembles through input-dependent gating to split-branch routing, and show that stacking routing layers encodes arbitrary decision trees, establishing universal function approximation with closed-form inference. Applied to ensemble time-series forecasting, the framework yields a Bayesian mixture of experts in which gating functions are inferred rather than learned, providing calibrated uncertainty over expert selection across five benchmark datasets.

Mykola Lukashchuk, Kyrylo Yemets, Wouter M. Kouw +4

Eindhoven University of Technology, the Netherlands · Lviv Polytechnic National University, Lviv, Ukraine · Lazy Dynamics, Utrecht, the Netherlands +1

Factor Graph Optimization Mixture-of-Experts Inference Probabilistic Graphical Models Variational Inference

May 24, 2026cs.RO

Loosely Coupled Factor Graph Optimization for Pseudolite-Augmented Navigation

In Global Navigation Satellite System (GNSS)-degraded environments, pseudolites (PLs) provide additional signal sources to enhance positioning performance, but their integration in optimization-based frameworks remains limited. This paper presents a loosely coupled factor graph optimization (FGO) framework that fuses the GNSS/PL least-squares (LS) solutions with inertial measurement unit (IMU) data. The evaluation considers low GNSS visibility scenarios with four high-elevation GNSS satellites and up to two PL transmitters over an 80~s window. FGO achieves a 22.8% to 41.3% reduction in mean 3D error compared to standard LS methods. Compared to a GNSS-IMU baseline, incorporating PL transmitters further improves positioning accuracy, with performance depending on geometry.

Chih-Chun Chen, Lipeng Tan, Shiyu Bai +1

Factor Graph Optimization GNSS-Denied Localization

May 19, 2026cs.RO

Enhancing Graph-Based SLAM in GNSS-Denied environments by leveraging leg odometry

Autonomous navigation in GNSS-denied environments remains a core challenge for legged robots, where exteroceptive sensors such as LiDAR are prone to elevation drift in geometrically sparse or repetitive scenes. We present a factor graph architecture that augments the LIO-SAM framework with a parallel kinematic lane driven by proprioceptive leg odometry, coupled to the main LiDAR-inertial lane via an identity relative pose constraint with a selective noise model. Applied to a Linxai D50 quadruped platform across two outdoor loops totaling over one kilometer, our approach reduces elevation drift from over 30m to under 30cm and enables convergence in a scene where the baseline pipeline fails entirely. These results suggest that proprioceptive data, already computed onboard for gait control, constitutes a lightweight and effective vertical anchor for SLAM in GNSS-denied settings.

Léon Perruchot-Triboulet, Luc Jaulin, Kai Xiao

Lab-STICC, ENSTA Campus Brest, France · LinXai Tech. Co., China

Simultaneous Localization and Mapping Factor Graph Optimization GNSS-Denied Localization Robot State Estimation

May 13, 2026cs.RO

DynoJEPP: Joint Estimation, Prediction and Planning in Dynamic Environments

DynoJEPP is a factor-graph-based framework that jointly formulates and simultaneously optimizes estimation, prediction, and planning in dynamic environments. In conventional factor-graph-based approaches that jointly formulate estimation, prediction, and planning, information from prediction and planning feeds back into state estimation, yielding corrupted estimates, undesired behaviors, and unsafe plans. To address this, DynoJEPP introduces a novel directed factor that enforces directional information flow within the factor graph, preventing prediction and planning from corrupting state estimation. We evaluate the impact of directed factors on inter-module interactions during navigation in both static and dynamic environments. Our results demonstrate that these factors are critical for safe operation, as without them, the robot collides in the majority of experiments. Building on this, we further introduce Cooperative DynoJEPP, which enables the ego robot to incorporate cooperative object behavior into its prediction and trajectory planning.

Mikolaj Kliniewski, Jesse Morris, Yiduo Wang +2

Australian Centre For Robotics (ACFR), School of Aerospace, Mechanical and Mechatronic Engineering (AMME), University of Sydney, 2006 Sydney, Australia.

Factor Graph Optimization Safe Robot Navigation Robot State Estimation Trajectory Prediction

May 7, 2026eess.SP

CredibleDFGO: Differentiable Factor Graph Optimization with Credibility Supervision

Global navigation satellite system (GNSS) positioning is widely used for urban navigation, but the covariance reported by the GNSS solver is often unreliable in urban canyons. Existing differentiable factor graph optimization (DFGO) methods learn measurement weighting through the solver, but they still use position-only objectives. As a result, the position estimate may improve while the reported covariance remains too small, too large, or incorrectly oriented. We propose CredibleDFGO (CDFGO), a differentiable GNSS factor graph framework that makes covariance credibility an explicit training target. A Weighting Generation Network (WGN) predicts per-satellite reliability weights, and a differentiable Gauss-Newton solver maps these weights to a position estimate and a Hessian-derived posterior covariance. We use proper scoring rules to supervise the East-North predictive distribution end to end. We study negative log-likelihood (NLL), the energy score (ES), and their combination. Results on three UrbanNav test scenes show consistent gains in covariance credibility. Positioning accuracy also improves on the medium-urban and harsh-urban scenes; on the deep-urban scene, both the mean horizontal error and the 95th-percentile error improve. On the harsh-urban Mong Kok (MK) scene, CDFGO-Combined reduces the mean horizontal error from 13.77 m to 11.68 m, reduces NLL from 40.63 to 6.59, and reduces ES from 12.31 to 9.05 relative to DFGO (MAE). Case studies link the MK improvement to better axis-wise consistency, more credible local covariance ellipses, and satellite-level reweighting.

Liang Qian, Penggao Yan, Penghui Xu +1

Department of Aeronautical and Aviation Engineering The Hong Kong Polytechnic University, Hong Kong, China

Factor Graph Optimization Wireless Localization Differentiable Optimization Uncertainty Calibration

May 4, 2026cs.RO

Temporally Consistent Object 6D Pose Estimation for Robot Control

Single-view RGB object pose estimators have reached a level of precision and efficiency that makes them good candidates for vision-based robot control. However, off-the-shelf methods lack temporal consistency and robustness that are mandatory for a stable feedback control. In this work, we develop a factor graph approach to enforce temporal consistency of the object pose estimates. In particular, the proposed approach: (i) incorporates object motion models, (ii) explicitly estimates the object pose measurement uncertainty, and (iii) integrates the above two components in an online optimization-based estimator. We demonstrate that with appropriate outlier rejection and smoothing using the proposed factor graph approach, we can significantly improve the results on standardized pose estimation benchmarks. We experimentally validate the stability of the proposed approach for a feedback-based robot control task in which the object is tracked by the camera attached to a torque controlled manipulator.

Kateryna Zorina, Vojtech Priban, Mederic Fourmy +2

Czech Institute of Informatics, Robotics and Cybernetics, Czech Technical University in Prague

Factor Graph Optimization Object Pose Estimation Vision-Based Robot Control

Apr 23, 2026cs.RO

SNGR: Selective Non-Gaussian Refinement for Ambiguous SLAM Factor Graphs

We present Selective Non-Gaussian Refinement (SNGR), a SLAM framework that augments iSAM2 with targeted nested sampling on windows where Gaussian approximations are likely to fail. We detect such regions using the condition number of joint marginal covariances and selectively refine them using the full nonlinear factor graph likelihood, with a gating mechanism to avoid degradation in multimodal cases. Experiments on range-only SLAM with wrong data association show that SNGR achieves high-precision failure detection and consistent local likelihood improvements while reducing computational cost relative to exhaustive non-Gaussian inference. These results highlight both the promise and the limitations of selective refinement for approximate SLAM posteriors.

Anushka Kulkarni, Sarthak Dubey

Northeastern University, Boston, MA, USA

Simultaneous Localization and Mapping Factor Graph Optimization Bayesian Filtering Robot Localization

Apr 17, 2026cs.RO

Factor Graph-Based Shape Estimation for Continuum Robots via Magnus Expansion

Reconstructing the shape of continuum manipulators from sparse, noisy sensor data is a challenging task, owing to the infinite-dimensional nature of such systems. Existing approaches broadly trade off between parametric methods that yield compact state representations but lack probabilistic structure, and Cosserat rod inference on factor graphs, which provides principled uncertainty quantification at the cost of a state dimension that grows with the spatial discretization. This letter combines the strength of both paradigms by estimating the coefficients of a low-dimensional Geometric Variable Strain (GVS) parameterization within a factor graph framework. A novel kinematic factor, derived from the Magnus expansion of the strain field, encodes the closed-form rod geometry as a prior constraint linking the GVS strain coefficients to the backbone pose variables. The resulting formulation yields a compact state vector directly amenable to model-based control, while retaining the modularity, probabilistic treatment and computational efficiency of factor graph inference. The proposed method is evaluated in simulation on a 0.4 m long tendon-driven continuum robot under three measurement configurations, achieving mean position errors below 2 mm for all three scenarios and demonstrating a sixfold reduction in orientation error compared to a Gaussian process regression baseline when only position measurements are available.

Lorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras

School of Aerospace Engineering · School of Electrical and Computer Engineering

Factor Graph Optimization Robotics Robot State Estimation

Mar 14, 2026cs.RO

H-RINS: Hierarchical Tightly-coupled Radar-Inertial State Estimation via Smoothing and Mapping

Millimeter-wave radar enables robust perception in visually degraded environments, yet radar-inertial estimation remains prone to drift: sparse body-frame velocity measurements weakly constrain absolute orientation, leaving IMU biases poorly observable over the short horizons of sliding-window estimators. We propose a tightly coupled, hierarchical radar-inertial factor graph that decouples estimation into a high-rate resetting graph and a persistent global graph. The resetting graph fuses IMU preintegration, radar velocities, and adaptive ZUPT to produce smooth, low-latency odometry for real-time control. The persistent graph maintains a full state (poses, velocities, and biases) via keyframe-based geometric mapping and loop closures. Fully observable biases and their exact covariances are continuously injected from the persistent graph as priors into the resetting graph, anchoring the high-rate estimator against integration drift. Extensive evaluations demonstrate high accuracy and drift-reduced estimation at faster than real-time speeds. Code and datasets will be released upon paper acceptance.

Ali Alridha Abdulkarim, Mikhail Litvinov, Dzmitry Tsetserukou

Intelligent Space Robotics Laboratory, Center for Digital Engineering, Skolkovo Institute of Science and Technology

Simultaneous Localization and Mapping Factor Graph Optimization Inertial Odometry Radar-Inertial Odometry Robot State Estimation Inertial Navigation