A Roadmap of Mathematical Optimization for Visual SLAM in Dynamic Environments

Abstract

The widespread application of robots in complex and dynamic environments demands that Visual SLAM is both robust and accurate. However, dynamic objects, varying illumination, and environmental complexity fundamentally challenge the static world assumptions underlying traditional SLAM methods. This review provides a comprehensive investigation into the mathematical foundations of V-SLAM and systematically analyzes the key optimization techniques developed for dynamic environments, with particular emphasis on advances since 2020. We begin by rigorously deriving the probabilistic formulation of V-SLAM and its basis in nonlinear optimization, unifying it under a Maximum a Posteriori (MAP) estimation framework. We then propose a taxonomy based on how dynamic elements are handled mathematically, which reflects the historical evolution from robust estimation to semantic modeling and then to deep learning. This framework provides detailed analysis of three main categories: (1) robust estimation theory-based methods for outlier rejection, elaborating on the mathematical models of M-estimators and switch variables; (2) semantic information and factor graph-based methods for explicit dynamic object modeling, deriving the joint optimization formulation for multi-object tracking and SLAM; and (3) deep learning-based end-to-end optimization methods, discussing their mathematical foundations and interpretability challenges. This paper delves into the mathematical principles, performance boundaries, and theoretical controversies underlying these approaches, concluding with a summary of future research directions informed by the latest developments in the field. The review aims to provide both a solid mathematical foundation for understanding current dynamic V-SLAM techniques and inspiration for future algorithmic innovations. By adopting a math-first perspective and organizing the field through its core optimization paradigms, this work offers a clarifying framework for both understanding and advancing dynamic V-SLAM.

1. Introduction

Visual Simultaneous Localization and Mapping (V-SLAM) serves as a foundational capability for autonomous systems, enabling robots to perceive, navigate, and interact within complex environments without relying on external infrastructure [1,2]. Its importance spans a broad spectrum of real-world applications, from autonomous driving and augmented reality to service robotics in public squares, hospitals, and logistics centers. However, despite significant advances in controlled or static settings, a critical bottleneck persists when V-SLAM operates in unstructured, dynamic domains [3,4,5]. In such scenarios, the fundamental assumption of a static world is violated by moving pedestrians, vehicles, and other transient objects. These dynamic elements introduce non-stationary noise that corrupts the measurement model. This challenge is concretely exemplified in widely-used benchmarks such as the KITTI dataset for urban traffic scenarios [6] and the TUM RGB-D dataset for indoor pedestrian environments [7], where moving vehicles and people systematically violate the static-world assumption and generate structured measurement outliers. Such scenarios lead to inconsistent data associations, biased state estimates, and ultimately, catastrophic failures in tracking and mapping. Addressing this robustness gap demands a deep re-examination of the underlying mathematical formulations and optimization frameworks [8]. This review is motivated by the urgent need to consolidate and analyze the latest mathematical optimization strategies that empower V-SLAM to function reliably amid real-world dynamics, thereby bridging the gap between laboratory performance and in-the-wild deployment [9,10].

In the pre-2000s, the filtering-based era, early V-SLAM systems predominantly relied on the Extended Kalman Filter (EKF) framework, whose mathematical foundation lies in first-order linearization of nonlinear systems around the current estimate, recursively updating the state vector and covariance matrix [11,12,13]. While theoretically sound, EKF-SLAM suffered from computational complexity that scaled quadratically with the number of landmarks, cumulative linearization errors, and sensitivity to data association errors. Notably, modern adaptations continue to leverage its efficiency in resource-constrained systems. A recent implementation [14] demonstrates an enhanced EKF-based MonoSLAM framework for UAV navigation in GPS-denied environments. By fusing monocular visual data with range-to-base-station measurements (e.g., UWB or RF), this system achieves full observability and bounded-error localization using only a single fixed base station, proving particularly effective for long-term indoor or urban-canyon drone operations without requiring loop closure.

During the batch optimization era from approximately 2000 to 2010, the limitations of recursive filtering spurred the development of bundle adjustment and pose graph optimization methods [15,16]. The mathematical foundation shifted toward sparse nonlinear least squares optimization over the entire trajectory, dramatically improving accuracy and consistency. The introduction of factor graphs provided a unified probabilistic graphical model for representing diverse sensor constraints, while Lie group theory offered a mathematically sound approach to manifold optimization for pose variables [17]. This period also saw the development of efficient optimization frameworks, such as g2o, which leveraged the problem’s inherent sparsity [18]. This established g2o as the standard optimization framework for major systems like ORB-SLAM3 [19] (for visual and visual-inertial SLAM), with recent extensions adapting it for dynamic environments [20,21]. Modern visual-lidar fusion systems also leverage its factor graph architecture for tightly-coupled sensor fusion [22].

From approximately 2010 to 2020, the robust and semantic era saw SLAM research mature, with its focus expanding to handle challenging environments. Robust estimation theory was incorporated to address outliers, while semantic SLAM emerged by integrating discrete semantic variables into continuous optimization frameworks [3,9,23]. This period also saw the rise of multi-sensor fusion, particularly visual-inertial systems, requiring sophisticated mathematical formulations for state estimation. Representative implementations include VINS-Mono [24], which employed sliding-window, tightly-coupled graph optimization with M-estimators (e.g., Huber loss) to jointly estimate camera poses, IMU states, and landmarks while robustly rejecting visual outliers. Simultaneously, semantic-enhanced systems integrated real-time semantic segmentation to filter dynamic objects, demonstrating how discrete semantic labels could be fused with continuous bundle adjustment for improved robustness in dynamic environments [25].

Beginning in the 2020s and continuing to the present, the learning-integrated era has been marked by the profound influence of deep learning on V-SLAM. This influence manifests through differentiable optimization layers, neural scene representations, and foundation models for zero-shot dynamic object recognition [26,27]. The mathematical foundations are evolving from explicit geometric models toward data-driven implicit representations and end-to-end function approximation. The mathematical foundations are evolving from explicit geometric models toward data-driven implicit representations and end-to-end function approximation. Key algorithmic advances include DROID-SLAM [28], which implements fully differentiable bundle adjustment; NeRF-SLAM [29], combining neural radiance fields with simultaneous localization; and the integration of foundation models like Segment Anything (SAM) [30] for zero-shot dynamic object recognition.

Through the above, it can be concluded that the core mathematical difficulty in dynamic V-SLAM stems from the violation of fundamental assumptions in traditional formulation. In this model, ${\mathbf{z}}_i$ represents the sensor observation, $h(·)$ is the observation function, ${\mathbf{x}}_i$ denotes the robot state, ${\mathbf{m}}_j$ is the static landmark position, and ${\mathbf{n}}_i$ is the observation noise assumed to follow a zero-mean Gaussian distribution with covariance ${\Sigma }_i$. The standard observation model ${\mathbf{z}}_t=h({\mathbf{x}}_t,{\mathbf{m}}_j)+{\mathbf{n}}_t$, where ${\mathbf{n}}_t\sim \mathcal{N}(0,{\Sigma }_t)$, becomes invalid when dynamic objects are present. Observations originating from moving objects constitute structured outliers that violate the Gaussian noise assumption, leading to biased state estimates [31,32,33].

Mathematically, this can be formalized as a mixture model:

$$p\left({\mathbf{z}}_t\right|{\mathbf{x}}_t,\mathcal{M})=(1-ϵ)\mathcal{N}(h({\mathbf{x}}_t,\mathcal{M}),{\Sigma }_t)+ϵp_{\mathrm{dynamic}}\left({\mathbf{z}}_t\right)$$

(1)

where $ϵ$ represents the contamination ratio and $p_{\mathrm{dynamic}}$ characterizes the outlier distribution.

Furthermore, the state estimation problem expands significantly. For a system with N camera poses, M static landmarks, and K dynamic objects observed over T time steps, the state dimension grows as $\mathcal{O}(N+M+KT)$, compared to $\mathcal{O}(N+M)$ in static environments. This combinatorial increase introduces fundamental challenges in computational complexity, identifiability, and robust inference.

This review makes several key contributions to the literature on dynamic environment V-SLAM:

• A comprehensive mathematical treatment of V-SLAM foundations, from probabilistic formulation to Lie group optimization, with detailed derivations.
• A structured taxonomy of dynamic V-SLAM methods centered on their core mathematical optimization strategies, incorporating cutting-edge research.
• In-depth analysis of mathematical challenges, theoretical controversies, and promising future research directions informed by the latest developments.
• Explicit connections between classical geometric methods and emerging learning-based approaches, highlighting their complementary mathematical strengths.

Our taxonomy is designed to organize methods by their core mathematical approach to dynamic elements, while also showing the field’s progression over time. This dual structure helps connect methodological developments with historical trends. The remainder of this review is organized as follows: Section 2 establishes the mathematical foundations of V-SLAM optimization. Section 3 presents our taxonomy and detailed analysis of methods for dynamic environments. Section 4 discusses mathematical challenges and controversies, and Section 5 concludes with future research directions.

2. Mathematical Foundations of V-SLAM Optimization in Static Environments

This section delineates the core mathematical underpinnings required to understand modern V-SLAM optimization. Rather than an exhaustive review of all mathematical tools, we concentrate on three foundational components that are both ubiquitous in the literature and critical for transitioning from problem formulation to numerical solution: probabilistic graphical models (for formulation), Lie group theory (for correct geometric representation), and nonlinear least squares (for efficient computation). The choice of these three is motivated by their representative role in forming the backbone of the majority of contemporary V-SLAM systems discussed subsequently.

A comprehensive summary of the mathematical notation used throughout this review is provided in Appendix A.

2.1. Probabilistic Formulation and Graphical Models

The SLAM problem is inherently probabilistic, concerned with estimating the joint posterior over the robot trajectory ${\mathbf{x}}_{1:t}$ and the map $\mathcal{M}$ given sensor observations ${\mathbf{z}}_{1:t}$ and control inputs ${\mathbf{u}}_{1:t}$:

$$p({\mathbf{x}}_{1:t},\mathcal{M}|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})$$

(2)

The classical filtering approach addresses this through recursive Bayesian estimation [34]. The Extended Kalman Filter (EKF) maintains a Gaussian approximation of the belief state, while the Particle Filter (PF) represents the belief non-parametrically through weighted samples [2,35,36,37,38,39]. However, the modern paradigm formulates SLAM as a full trajectory optimization problem using Maximum a Posteriori (MAP) estimation:

$${\mathcal{X}}^{\ast },{\mathcal{L}}^{\ast }=\arg \underset{\mathcal{X},\mathcal{L}}{max}p(\mathcal{X},\mathcal{L}|\mathcal{Z})=\arg \underset{\mathcal{X},\mathcal{L}}{max}p\left(\mathcal{Z}\right|\mathcal{X},\mathcal{L})p\left(\mathcal{X}\right)p\left(\mathcal{L}\right)$$

(3)

where $\mathcal{X}=\{{\mathbf{x}}_1,\dots ,{\mathbf{x}}_N\}$ represents the robot trajectory, $\mathcal{L}=\{{\mathbf{l}}_1,\dots ,{\mathbf{l}}_M\}$ denotes the set of landmarks, and $\mathcal{Z}$ encompasses all available observations.

The factor graph provides a powerful graphical model representation of this MAP estimation problem. This review focuses on the factor graph and MAP estimation paradigm as the primary probabilistic framework for modern optimization-based V-SLAM. While alternative approaches such as particle filters remain valuable for specific scenarios (e.g., multi-hypothesis tracking), the factor graph’s explicit representation of sparsity and its direct link to efficient nonlinear optimization make it the dominant and most extensible framework for integrating dynamic object states and semantic constraints, which are central themes in subsequent chapters. The joint probability distribution factorizes as:

$$p(\mathcal{X},\mathcal{L}|\mathcal{Z})\propto \prod _{k=1}^K{f}_k({\mathcal{X}}_k,{\mathcal{L}}_k)$$

(4)

where each factor $f_k$ corresponds to a constraint involving subsets of variables ${\mathcal{X}}_k\subseteq \mathcal{X}$ and ${\mathcal{L}}_k\subseteq \mathcal{L}$. This factorization naturally captures the sparse connectivity of the SLAM problem and enables efficient inference.

Within the probabilistic framework, we emphasize factor graphs and maximum a posteriori estimation, as they constitute the prevailing optimization paradigm for handling dynamic and semantic information; the Extended Kalman Filter is introduced as a historical foundation to illustrate the developmental trajectory.

2.2. Lie Group Theory for Pose Representation

The Lie group structure addresses a fundamental challenge in SLAM optimization: performing calculus on the non-Euclidean manifold of rigid-body poses. Standard gradient-based optimization operates in vector spaces, where updates are additive [40,41,42]. However, directly adding an increment to a rotation matrix $\mathbf{R}\leftarrow \mathbf{R}+\Delta \mathbf{R}$ does not guarantee that the result remains in $\mathrm{SO}\left(3\right)$, violating the constraints of the rotation group.

Lie theory resolves this issue by providing a local parameterization via the Lie algebra. The algebra $\mathfrak{se}\left(3\right)$, being a vector space, permits unconstrained optimization. The current estimate $\mathbf{T}$ is updated using a perturbation $\delta \xi$ in the algebra:

$${\mathbf{T}}_{\mathrm{new}}=\mathbf{T}·exp\left(\delta {\xi }^{\wedge }\right)$$

(5)

This update rule ensures that ${\mathbf{T}}_{\mathrm{new}}$ remains in $\mathrm{SE}\left(3\right)$ for any $\delta \xi \in {\mathbb{R}}^6$. The optimization then proceeds by iteratively solving for the optimal perturbation $\delta {\xi }^{\ast }$ in the tangent space.

The Jacobian matrices required for optimization, such as ${\displaystyle \frac{\mathsf{\partial }\mathbf{e}}{\mathsf{\partial }\delta \xi }}$, characterize the sensitivity of the error function to infinitesimal changes on the manifold [43,44,45,46]. This approach, known as on-manifold optimization, transforms a constrained optimization problem on a nonlinear manifold into an unconstrained one in a Euclidean vector space. Consequently, Lie theory provides the mathematical rigor necessary for stable and consistent estimation of pose variables in modern sparse, nonlinear least-squares solvers [28,29].

For a camera at pose ${\mathbf{T}}_i$ observing a landmark ${\mathbf{l}}_j$, the reprojection error is:

$${\mathbf{e}}_{ij}=\pi \left({\mathbf{T}}_i^{-1}{\mathbf{l}}_j\right)-{\mathbf{z}}_{ij}$$

(6)

where $\pi :{\mathbb{R}}^3\to {\mathbb{R}}^2$ is the camera projection function and ${\mathbf{z}}_{ij}$ is the actual image measurement. Proper handling of 3D rigid body transformations is crucial for V-SLAM. The Special Euclidean Group $\mathrm{SE}\left(3\right)$ represents valid rigid body transformations:

$$\mathrm{SE}\left(3\right)=\left\{\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^{\top }& 1\end{array}\right]|\mathbf{R}\in \mathrm{SO}\left(3\right),\mathbf{t}\in {\mathbb{R}}^3\right\}$$

(7)

where $\mathrm{SO}\left(3\right)$ is the Special Orthogonal Group representing rotations.

Lie theory provides the mathematical framework for manipulating elements of $\mathrm{SE}\left(3\right)$ during optimization [47]. The Lie algebra $\mathfrak{se}\left(3\right)$ is the tangent space at the identity of $\mathrm{SE}\left(3\right)$, providing a minimal representation for perturbations. The exponential map $exp:\mathfrak{se}\left(3\right)\to \mathrm{SE}\left(3\right)$ and logarithm map $log:\mathrm{SE}\left(3\right)\to \mathfrak{se}\left(3\right)$ connect the Lie algebra to the Lie group:

$${\mathbf{T}}^{\prime }=\mathbf{T}exp\left({\xi }^{\wedge }\right)$$

(8)

where $\xi \in {\mathbb{R}}^6$ is the twist coordinate vector and ${(·)}^{\wedge }$ is the hat operator mapping ${\mathbb{R}}^6$ to $\mathfrak{se}\left(3\right)$.

Regarding pose representation, we select the Lie groups $\mathrm{SE}\left(3\right)/\mathrm{SO}\left(3\right)$ because they provide the minimal and complete mathematical objects for describing rigid-body motion, serving as the common foundation for the vast majority of V-SLAM systems.

2.3. Nonlinear Least Squares Formulation

Under the assumption of zero-mean Gaussian noise, the MAP estimate in Equation (3) becomes a Nonlinear Least Squares (NLSQ) problem:

$${\mathcal{X}}^{\ast },{\mathcal{L}}^{\ast }=\arg \underset{\mathcal{X},\mathcal{L}}{min}\left(\sum _{i=1}^{N-1}\parallel {\mathbf{e}}_i^{\mathrm{odom}}{\parallel }_{{\Omega }_i^{\mathrm{odom}}}^2+\sum _{i,j}\parallel {\mathbf{e}}_{i,j}{\parallel }_{{\Omega }_{i,j}}^2+\sum _k{\parallel {\mathbf{e}}_k^{\mathrm{loop}}\parallel }_{{\Omega }_k^{\mathrm{loop}}}^2\right)$$

(9)

where ${\parallel \mathbf{e}\parallel }_{\Omega }^2\triangleq {\mathbf{e}}^{\top }\Omega \mathbf{e}$ is the squared Mahalanobis distance and $\Omega$ is the information matrix [48,49].

The Gauss-Newton algorithm solves this iteratively by linearizing around the current estimate. At each iteration, we solve the linear system:

$${\mathbf{J}}^{\top }\Omega \mathbf{J}\delta {Ø}^{\ast }=-{\mathbf{J}}^{\top }\Omega \mathbf{e}$$

(10)

where $\mathbf{J}$ is the Jacobian matrix of the error vector $\mathbf{e}$ with respect to the state perturbation $\delta \mathbf{Ø}$.

The structure of the Hessian approximation $\mathbf{H}={\mathbf{J}}^{\top }\Omega \mathbf{J}$ reflects the connectivity of the factor graph. For pose graph SLAM, it exhibits a block structure:

$$\mathbf{H}=\left[\begin{array}{cc}{\mathbf{H}}_{\mathcal{X}\mathcal{X}}& {\mathbf{H}}_{\mathcal{X}\mathcal{L}}\\ {\mathbf{H}}_{\mathcal{L}\mathcal{X}}& {\mathbf{H}}_{\mathcal{L}\mathcal{L}}\end{array}\right]$$

(11)

The Schur complement trick can be applied to efficiently solve this system by first marginalizing the landmark variables:

$$[{\mathbf{H}}_{\mathcal{X}\mathcal{X}}-{\mathbf{H}}_{\mathcal{X}\mathcal{L}}{\mathbf{H}}_{\mathcal{L}\mathcal{L}}^{-1}{\mathbf{H}}_{\mathcal{L}\mathcal{X}}]\delta {\mathcal{X}}^{\ast }=-{\mathbf{b}}_{\mathcal{X}}+{\mathbf{H}}_{\mathcal{X}\mathcal{L}}{\mathbf{H}}_{\mathcal{L}\mathcal{L}}^{-1}{\mathbf{b}}_{\mathcal{L}}$$

(12)

This dramatically reduces the problem size since the number of pose variables is typically much smaller than the number of landmarks. In terms of optimization solving, we analyze the Schur complement technique in detail, as it represents the key algorithmic core for addressing large-scale sparse problems, and its principle can be directly extended to the marginalization of object states in dynamic scenarios. The primary aim of this chapter is to demonstrate how these foundational tools are extended to address dynamism, rather than to enumerate all possible variants.

2.4. Sparsity, Solver Techniques, and Optimization Foundations

This section elucidates how the inherent sparsity of the SLAM problem is systematically exploited to enable efficient numerical optimization. The techniques discussed herein—particularly those leveraging the block structure of the Hessian and the Schur complement—form the computational backbone for solving large-scale nonlinear least-squares problems. While presented in the context of static environments, these foundational methods are directly extensible to dynamic SLAM, where they underpin advanced strategies for handling moving objects (e.g., through dynamic marginalization or object-aware sparsity exploitation).

As shown by the block structure of the Hessian matrix in Equation (11), the problem is highly sparse. This sparsity can be exploited through the Schur complement trick (also known as landmark marginalization). The core idea is to solve for the landmark variables first in terms of the poses, and then substitute back to obtain a smaller, denser system involving only poses. Formally, this is derived from the linear system $\mathbf{H}\delta {\theta }^{\ast }=-\mathbf{b}$ by block Gaussian elimination.

The computational advantages of this manipulation are threefold. First, the size of the system to be solved is drastically reduced from $dim\left(\mathcal{X}\right)+dim\left(\mathcal{L}\right)$ to just $dim\left(\mathcal{X}\right)$. Since the number of landmarks M often far exceeds the number of poses N (i.e., $M\gg N$), this transforms an intractable $\mathcal{O}\left({(N+M)}^3\right)$ problem into a manageable $\mathcal{O}\left(N^3\right)$ problem [50]. Second, the inversion of ${\mathbf{H}}_{\mathcal{LL}}$, which is the most expensive step, is highly efficient because ${\mathbf{H}}_{\mathcal{LL}}$ is block-diagonal, allowing for its inverse to be computed by inverting each small, individual landmark block independently—an $\mathcal{O}\left(M\right)$ operation. Finally, although the Schur complement $\mathbf{S}$ is denser than the original ${\mathbf{H}}_{\mathcal{XX}}$, it inherits a structured sparsity pattern dictated by the pose graph, which permits the continued use of efficient solvers such as sparse Cholesky factorization or preconditioned conjugate gradient methods [51].

This combination of exploiting the problem’s inherent topological sparsity (from the factor graph) with the algebraic simplification (via the Schur complement) forms the computational backbone of modern, large-scale SLAM systems, making them feasible for real-world applications [52].

3. Mathematical Optimization Methods for Dynamic Environments: A Taxonomy Based on Robust, Semantic, and Learning Paradigms

This section systematically elaborates on the core mathematical optimization methods for visual SLAM in dynamic environments. Starting from the joint state modeling of dynamic SLAM, it highlights three major challenges: identifiability, computational complexity, and robustness. Furthermore, this section evaluates the adaptability of each method under atypical observation scenarios such as extreme illumination, occlusion, transient dynamic objects, and sensor degradation, providing both theoretical foundations and practical guidance for the design and selection of robust dynamic V-SLAM systems. As illustrated in Figure 1, we categorize dynamic V-SLAM methods into three main optimization paradigms: (1) robust estimation, which treats dynamic measurements as statistical outliers; (2) semantic factor graphs, which explicitly model object states and motion; and (3) deep learning end-to-end optimization, which learns implicit representations through differentiable components. The following subsections detail the mathematical foundations of each paradigm.

3.1. Mathematical Formulation of Dynamic SLAM

The transition from static to dynamic environments necessitates a fundamental extension of the classical SLAM state vector to explicitly account for moving objects [53]. This evolution shifts the problem from one of pure geometry and localization to a joint estimation problem involving both the static environment and dynamic agents. Let the full state in a dynamic setting be defined as the tuple [54]:

$$\Theta =(\mathcal{X},\mathcal{M},\mathcal{D}),$$

(13)

where $\mathcal{X}=\{{\mathbf{x}}_1,\dots ,{\mathbf{x}}_N\}$ denotes the robot trajectory, $\mathcal{M}=\{{\mathbf{m}}_1,\dots ,{\mathbf{m}}_M\}$ the static map, and $\mathcal{D}=\{{\mathbf{o}}_1,\dots ,{\mathbf{o}}_K\}$ the temporally evolving states of K dynamic objects. Crucially, the state of a dynamic object ${\mathbf{o}}_k$ is not a single pose but a trajectory itself, often represented as ${\mathbf{o}}_k=\{{\mathbf{o}}_k^1,{\mathbf{o}}_k^2,\dots ,{\mathbf{o}}_k^{T_k}\}$ over the time steps it is observed.

This augmented state vector fundamentally alters the probabilistic structure of the SLAM problem. The joint posterior, factoring in sensor observations $\mathcal{Z}$, now becomes

$$p(\Theta \mid \mathcal{Z})\propto p(\mathcal{Z}\mid \Theta )·p\left(\mathcal{X}\right)·p\left(\mathcal{M}\right)·p(\mathcal{D}\mid \mathcal{X}),$$

(14)

Here, the conditional prior $p(\mathcal{D}\mid \mathcal{X})$ is particularly significant. It encodes the dependence of dynamic object states on the robot’s own trajectory, reflecting the fact that observations of objects are made from the robot’s moving frame of reference. This prior can incorporate dynamical models (e.g., constant velocity or acceleration) to constrain object motion. This formulation introduces a strong coupling between the robot’s path and the objects’ trajectories, as miscalibration in one directly biases the estimate of the other. Furthermore, the resulting objective function, often derived from this posterior, becomes highly non-convex due to the complex interactions and data association ambiguities between the robot, static landmarks, and dynamic objects.

While this formulation elegantly generalizes the static SLAM problem to dynamic settings, it simultaneously raises profound theoretical and practical challenges:

This formulation introduces profound theoretical and practical challenges in three key areas: scalability, identifiability, and robust optimization. First, regarding scalability, the state dimension explodes combinatorially. For a system with N poses, M static landmarks, and K dynamic objects each observed over T time steps, the state dimension grows as $\mathcal{O}(N+M+KT)$, compared to $\mathcal{O}(N+M)$ in static environments. The worst-case computational complexity for a batch solution becomes $\mathcal{O}\left({(N+M+KT)}^3\right)$, posing a severe challenge for long-term operations in crowded environments. In practice, systems like the Komatsu FrontRunner autonomous haulage system [55] address this by employing sliding window approximations with $w=10$, reducing effective complexity to $\mathcal{O}\left(w^3\right)$ while tracking 5–10 dynamic vehicles in kilometer-scale open-pit mines. Second, the problem suffers from inherent identifiability ambiguities. A classic issue is the object-pose ambiguity, where the relative motion between the robot and a dynamic object cannot be uniquely disambiguated if the object is observed from only a limited set of viewpoints. This leads to unobservable directions in the state space, making the Fisher Information Matrix (FIM) rank-deficient. Finally, the presence of dynamic objects acts as a source of structured outliers that violate the standard static-world observation model, raising the challenge of robust optimization. Standard least-squares optimizers are highly sensitive to such outliers, which necessitates the integration of robust estimation techniques (e.g., M-estimators, switchable constraints) or probabilistic data association methods to prevent catastrophic failure, further complicating the optimization landscape.

Consequently, tackling dynamic SLAM requires not only a more complex state representation but also novel algorithmic approaches that address these intertwined challenges of scalability, identifiability, and robustness.

3.2. Robust Estimation Theory for Outlier Rejection

Robustness can be achieved at different levels. At the geometric level, methods such as RANSAC or epipolar geometric checks reject outliers through discrete hypothesis testing during motion estimation or feature matching. While effective for sparse, independent outliers, these approaches may struggle with pervasive or structured dynamic interference. Therefore, this article focuses on optimization-level robust estimators, which integrate robustness directly into the continuous optimization framework in a probabilistic and differentiable manner, offering a more principled solution for complex dynamic scenarios.

3.2.1. M-Estimators and Robust Kernels

The core idea of M-estimation is to replace the quadratic loss with a robust kernel function $\rho (·)$ that reduces the influence of large residuals:

$${\mathcal{X}}^{\ast },{\mathcal{L}}^{\ast }=\arg \underset{\mathcal{X},\mathcal{L}}{min}\sum _{i,j}\rho \left(\parallel {\mathbf{e}}_{i,j}{\parallel }_{{\Omega }_{i,j}}\right)$$

(15)

The robustness properties of M-estimators can be characterized through their influence function $\psi \left(r\right)=\mathsf{\partial }\rho \left(r\right)/\mathsf{\partial }r$, which quantifies the effect of residuals on the parameter estimates. A bounded influence function indicates robustness to outliers. For the Huber loss, the influence function is

$${\psi }_{\mathrm{Huber}}\left(r\right)=\left\{\begin{array}{cc}r\hfill & \mathrm{if}\left|r\right|\le \delta \hfill \\ \delta ·sign\left(r\right)\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(16)

The asymptotic breakdown point ${ϵ}^{\ast }$ provides a theoretical measure of robustness, representing the maximum proportion of outliers an estimator can tolerate before producing arbitrary estimates. For M-estimators with redescending influence functions like the Geman-McClure kernel, the breakdown point can approach 0.5, meaning they can handle up to 50% contamination in the data.

The statistical efficiency of M-estimators under Gaussian noise can be analyzed through their relative efficiency compared to the optimal maximum likelihood estimator. The Huber loss, for instance, maintains 95% efficiency at the Gaussian distribution while providing robustness to heavy-tailed distributions. This capability is leveraged in urban delivery drones like the DJI Matrice 300 series [56], where adaptive M-estimators handle 25–35% dynamic vehicle interference during package delivery, demonstrating practical robustness beyond theoretical guarantees.

The convergence properties of M-estimators in SLAM applications depend on the convexity of the robust kernel. While the Huber loss preserves convexity, redescending M-estimators like Cauchy and Geman-McClure introduce non-convexity, potentially leading to local minima. However, this non-convexity is precisely what provides stronger rejection of gross outliers in dynamic environments. The influence function $\psi \left(r\right)=\mathsf{\partial }\rho \left(r\right)/\mathsf{\partial }r$ characterizes how much each residual contributes to the solution. Common robust kernels include the following:

• Huber: ${\rho }_{\mathrm{Huber}}\left(r\right)=\left\{\begin{array}{cc}{\displaystyle \frac{1}{2}}{r}^2\hfill & \mathrm{if}|r|\le \delta \hfill \\ \delta (|r|-{\displaystyle \frac{1}{2}}\delta )\hfill & \mathrm{otherwise}\hfill \end{array}\right.$
• Cauchy: ${\rho }_{\mathrm{Cauchy}}\left(r\right)={\displaystyle \frac{c^2}{2}}log(1+{(r/c)}^2)$
• Geman-McClure: ${\rho }_{\mathrm{GM}}\left(r\right)={\displaystyle \frac{r^2/2}{1+r^2}}$

For practitioners selecting a robust kernel, key characteristics differ: the Huber kernel has a linear-then-constant influence function and is convex (typical $\delta \approx 1.345$); the Cauchy kernel has a redescending influence function and is non-convex (typical $c\approx 2.385$); the Geman-McClure kernel also redescends and is non-convex (typical scale $=1$). Choice depends on the expected outlier proportion and need for convexity.

Recent work has explored learning adaptive robust kernels directly from data. The researchers of [57] proposed a method to learn kernel parameters within a differentiable optimization framework:

$${\alpha }_{\mathrm{kernel}}^{\ast }=\arg \underset{{\alpha }_{\mathrm{kernel}}}{min}\sum _{\mathrm{validation}}\parallel {\mathcal{X}}^{\ast }\left({\alpha }_{\mathrm{kernel}}\right)-{\mathcal{X}}_{\mathrm{gt}}\parallel$$

(17)

where ${\mathcal{X}}^{\ast }\left({\alpha }_{\mathrm{kernel}}\right)$ is the solution obtained using kernel parameters ${\alpha }_{\mathrm{kernel}}$.

To illustrate how M-estimation integrates into practical SLAM systems, Algorithm 1 presents a simplified pseudocode for robust cost computation using the Huber loss. This algorithm demonstrates the conditional treatment of residuals based on their magnitude, which forms the core of robust outlier rejection in dynamic environments.

Algorithm 1 Robust Cost Computation using M-Estimation
1:	function RobustEstimation($e_{ij},\rho ,\delta$)
2:	$C\leftarrow 0$	▹ Initialize total cost
3:	for each residual e in $e_{ij}$ do
4:	if $\left|e\right|\le \delta$then	▹ Inlier region: quadratic loss
5:	$C\leftarrow C+0.5·e^2$
6:	else	▹ Outlier region: linear loss
7:	$C\leftarrow C+\delta ·\left(\right|e|-0.5\delta )$
8:	end if
9:	end for
10:	return C	▹ Robust cost for optimization
11:	end function

3.2.2. Switch Variables and Probabilistic Data Association

The switch variable approach introduces a continuous variable $s_{i,j}\in [0,1]$ for each potential dynamic constraint, leading to a joint estimation problem:

$${\mathcal{X}}^{\ast },{\mathcal{L}}^{\ast },{\mathcal{S}}^{\ast }=\arg \underset{\mathcal{X},\mathcal{L},\mathcal{S}}{min}\left(\sum _{\langle i,j\rangle }{s}_{i,j}{\parallel {\mathbf{r}}_{i,j}(\mathcal{X},\mathcal{L})\parallel }_{{\Omega }_{i,j}}^2+\sum _{\langle i,j\rangle }\Psi \left(s_{i,j}\right)\right)$$

(18)

where $\Psi \left(s_{i,j}\right)$ is a prior penalty that encourages $s_{i,j}$ toward 0 or 1. This optimization problem is non-convex due to the bilinear terms $s_{ij}{\parallel {\mathbf{r}}_{ij}(\mathcal{X},\mathcal{L})\parallel }^2$. However, under certain conditions, convergence guarantees can be established. When the prior penalty $\Psi \left(s_{i,j}\right)$ is chosen as a concave function, the overall objective becomes difference-of-convex, enabling convergence to a stationary point using block coordinate descent. Specifically, when the prior penalty $\Psi \left(s_{i,j}\right)$ is concave, the objective becomes difference-of-convex, and block coordinate descent can be shown to converge to a stationary point, as analyzed in [58]. The sensitivity to initialization can be mitigated via continuation methods, which start with strong regularization and gradually reduce it.

The convergence rate can be characterized by analyzing the Kurdyka-Łojasiewicz property of the objective function. Recent work by [59] has shown that with proper initialization and regularization, the switch variable formulation converges linearly to a local minimum in practice.

The sensitivity of switch variables to initialization can be mitigated through continuation methods, where the optimization starts with strong regularization on the switch variables and gradually reduces it, allowing for the solver to first establish good pose estimates before determining data association.

From a probabilistic perspective, the switch variable approach can be interpreted as an approximation to marginalization over data association variables. The continuous relaxation $s_{ij}\in [0,1]$ approximates the discrete association probabilities, with the prior $\Psi \left(s_{ij}\right)$ acting as a entropy regularization that encourages binary solutions while maintaining differentiability.

Building on this, a probabilistic data association method that models the correspondence between landmarks and observations as a distribution can be expressed as:

$$p(\mathcal{X},\mathcal{L},\mathcal{A}|\mathcal{Z})\propto p(\mathcal{Z}|\mathcal{X},\mathcal{L},\mathcal{A})p\left(\mathcal{X}\right)p\left(\mathcal{L}\right)p\left(\mathcal{A}\right)$$

(19)

where $\mathcal{A}=\left\{{\alpha }_{i,j}\right\}$ represents the data association variables. They employ variational inference to approximate the posterior:

$$q^{\ast }(\mathcal{X},\mathcal{L},\mathcal{A})=\arg \underset{q}{min}\mathrm{KL}\left(q(\mathcal{X},\mathcal{L},\mathcal{A})\parallel p(\mathcal{X},\mathcal{L},\mathcal{A}|\mathcal{Z})\right)$$

(20)

3.2.3. Robustness to Atypical Measurements

While M-estimators and switch variables provide considerable robustness to outliers under the commonly assumed i.i.d. noise models, their efficacy against structured, non-i.i.d. atypical measurements—such as temporally correlated noise induced by dynamic reflections, persistent sensor malfunctions, or systematic errors from adverse weather conditions—presents a more complex and less understood challenge. The asymptotic breakdown point provides a crucial theoretical upper bound on the proportion of generic outliers an estimator can tolerate before failing catastrophically. However, this classical metric does not fully capture the nuanced vulnerability to structured corruptions that violate its underlying i.i.d. assumption.

Generalization in unseen, highly dynamic real-world scenarios often hinges on a more subtle capability: the estimator’s ability to discriminate between informative geometric perturbations (carrying legitimate signal about robot or object motion) and deceptive perturbations (arising from structured, non-geometric artifacts). For instance, a moving vehicle’s specular highlights may generate a spatially and temporally correlated stream of feature measurements that appear geometrically consistent but are physically uninformative for pose estimation. Standard robust kernels, tuned for isolated large residuals, may fail to reject such coherent deceptive structures, leading to biased estimates.

This limitation motivates the integration of temporal reasoning and semantic-aware robustness frameworks. Recent approaches, such as those incorporating learned, context-dependent kernel parameters or leveraging sequential hypothesis testing within a filtering paradigm [60], aim to endow estimators with the discernment required for these challenging scenarios. The analysis of estimator performance must, therefore, evolve from a purely statistical outlier-rejection perspective to one that also considers the semantic and temporal structure of measurement errors in dynamic environments.

3.3. Optimization with Semantic Priors: A Methodological Bridge

Beyond handling dynamics as outliers, another optimization strategy is to incorporate semantic understanding as structural priors into the estimation framework. This approach, represented by semantic SLAM systems, extends the state vector and factor graph model to jointly reason about geometry and object categories.

3.3.1. Semantic Factor Graphs

In semantic SLAM, the state vector is extended to include object states: $\Theta =\{\mathcal{X},\mathcal{L},\mathcal{O}\}$ where $\mathcal{O}=\{{\mathbf{o}}_1,\dots ,{\mathbf{o}}_K\}$ represents dynamic objects. The factor graph incorporates additional semantic factors:

$$p(\Theta |\mathcal{Z},\mathcal{S})\propto \left[\prod _k{f}_k^{\mathrm{geo}}\right]\left[\prod _m{f}_m^{\mathrm{sem}}(\mathcal{X},{\mathcal{O}}_m)\right]\left[\prod _n{f}_n^{\mathrm{dyn}}\left({\mathcal{O}}_n\right)\right]\left[\prod _p{f}_p^{\mathrm{assoc}}\right]$$

(21)

where $\mathcal{S}$ represents semantic observations, $f^{\mathrm{sem}}$ are semantic observation factors, $f^{\mathrm{dyn}}$ are dynamic model factors, and $f^{\mathrm{assoc}}$ are data association factors. A fundamental theoretical question in semantic SLAM concerns parameter identifiability—whether the true state $\Theta =(\mathcal{X},\mathcal{L},\mathcal{O})$ can be uniquely determined from the observations $\mathcal{Z}$. The Fisher Information Matrix (FIM) provides insights into this question:

$$\mathcal{I}(\Theta )=\mathbb{E}\left[\left({\displaystyle \frac{\mathsf{\partial }logp\left(\mathcal{Z}\right|\Theta )}{\mathsf{\partial }\Theta }}\right){\left({\displaystyle \frac{\mathsf{\partial }logp\left(\mathcal{Z}\right|\Theta )}{\mathsf{\partial }\Theta }}\right)}^{\top }\right]$$

(22)

This ambiguity can be seen from a minimal derivation: observing a point $\mathbf{p}$ on a moving object from a single camera gives a measurement $\mathbf{z}=\pi \left({\mathbf{T}}_c^{-1}{\mathbf{T}}_o\mathbf{p}\right)$, which is invariant under any common rigid transformation $\mathbf{G}$ applied to both ${\mathbf{T}}_c$ and ${\mathbf{T}}_o$, i.e., $\pi ({\left(\mathbf{G}{\mathbf{T}}_c\right)}^{-1}\left(\mathbf{G}{\mathbf{T}}_o\right)\mathbf{p})=\mathbf{z}$. This symmetry induces a nullspace in the Fisher Information Matrix. When $\mathcal{I}(\Theta )$ is rank-deficient, certain state components are unobservable.

In semantic SLAM, common unobservable directions indicated by a rank-deficient Fisher Information Matrix include the object-pose ambiguity, where the relative motion between the camera and a moving object cannot be uniquely disambiguated if the object is observed by only one camera pose, leading to a nullspace in the FIM corresponding to simultaneous transformations of camera and object poses. Furthermore, scale ambiguity arises in monocular semantic SLAM, as the absolute scale of both the camera trajectory and object states may be unobservable without additional scale information from sensors like IMU or object size priors. Finally, object identity ambiguity presents a challenge when multiple objects of the same class have similar appearances, making data association difficult and potentially causing label switching during optimization.

The observability analysis reveals that at least two camera poses must observe the same dynamic object to make its motion observable relative to the static scene. Furthermore, the object’s motion model plays a crucial role in identifiability—constant velocity models provide better observability compared to random walk models.

The work of [61] presented a system that performs object-level SLAM with online object model estimation. Their optimization includes object shape parameters ${\mathbf{s}}_k$:

$${\Theta }^{\ast }=\arg \underset{\Theta }{min}\left(\sum _i\parallel {\mathbf{e}}_i^{\mathrm{odom}}{\parallel }^2+\sum _{i,j}\parallel {\mathbf{e}}_{i,j}^{\mathrm{obs}}{\parallel }^2+\sum _k\parallel {\mathbf{e}}_k^{\mathrm{obj}}{\parallel }^2+\sum _k{\parallel {\mathbf{e}}_k^{\mathrm{shape}}\parallel }^2\right)$$

(23)

The introduction of object states significantly increases the computational complexity of the SLAM problem. For a system with N camera poses, M static landmarks, and K dynamic objects each with $T_k$ temporal states, the state dimension becomes:

$$\dim (\Theta )=\underset{\mathrm{poses}}{\underbrace{6N}}+\underset{\mathrm{landmarks}}{\underbrace{3M}}+\underset{\mathrm{object}\mathrm{states}}{\underbrace{\sum _{k=1}^K{d}_k{T}_k}}$$

(24)

where $d_k$ is the state dimension per object per time step (typically 6 for SE(3) poses or 9 for poses with velocities).

The worst-case complexity for direct solving is $\mathcal{O}({\left(\dim (\Theta )\right)}^3)$. However, the sparsity pattern in semantic factor graphs exhibits a block structure that can be exploited:

$$\mathbf{H}=\left[\begin{array}{ccc}{\mathbf{H}}_{\mathcal{XX}}& {\mathbf{H}}_{\mathcal{XL}}& {\mathbf{H}}_{\mathcal{XO}}\\ {\mathbf{H}}_{\mathcal{LX}}& {\mathbf{H}}_{\mathcal{LL}}& {\mathbf{H}}_{\mathcal{LO}}\\ {\mathbf{H}}_{\mathcal{OX}}& {\mathbf{H}}_{\mathcal{OL}}& {\mathbf{H}}_{\mathcal{OO}}\end{array}\right]$$

(25)

The Schur complement can be applied hierarchically, first marginalizing landmarks and then objects, reducing the effective problem size to $\mathcal{O}\left({(N+\sum T_k)}^3\right)$.

For real-time operation, sliding window approaches limit the number of poses and object states in the optimization window. The complexity then becomes $\mathcal{O}\left(w^3\right)$ where w is the window size, making the approach scalable to long-term operation in dynamic environments. Algorithm 2 presents the procedural steps for constructing and optimizing a semantic factor graph.

Algorithm 2 Semantic Factor Graph Construction and Optimization
1:	function SemanticFactorGraph($\mathcal{X},\mathcal{L},\mathcal{O},\mathcal{Z}$)
2:	Initialize factor graph $G\leftarrow \varnothing$
3:	Add odometry factors between consecutive poses ${\mathbf{x}}_i,{\mathbf{x}}_{i+1}\in \mathcal{X}$
4:	Add landmark observation factors between $\mathcal{X}$ and $\mathcal{L}$ based on $\mathcal{Z}$
5:	for each dynamic object $o_k\in \mathcal{O}$ do
6:	Add object state variables $\left\{o_k^t\right\}$ to G
7:	Add motion model factors between consecutive states $o_k^t,o_k^{t+1}$
8:	Add observation factors linking camera poses ${\mathbf{x}}_i$ to object states $o_k^t$
9:	end for
10:	Construct full state vector $\Theta =\{\mathcal{X},\mathcal{L},\mathcal{O}\}$
11:	${\Theta }^{\ast }\leftarrow \arg {min}_{\Theta }{\sum }_{f\in G}{\parallel e_f(\Theta )\parallel }_{{\Omega }_f}^2$	▹ Joint optimization
12:	return ${\Theta }^{\ast }$	▹ Optimized camera poses, landmarks, and object states
13:	end function

3.3.2. Joint SLAM and Multi-Object Tracking

The tight coupling of SLAM with Multi-Object Tracking (MOT) aims to estimate the posterior:

$$p(\mathcal{X},\mathcal{M},{\mathcal{O}}_{1:t}|{\mathbf{z}}_{1:t})$$

(26)

Some researchers proposed a unified factor graph framework that integrates visual-inertial SLAM with model-based object tracking [62,63]. The motion model for object ${\mathbf{o}}_k$ is:

$${\mathbf{o}}_k^{t+1}={\mathbf{A}}_k{\mathbf{o}}_k^t+{\mathbf{B}}_k{\mathbf{u}}_k^t+{\mathbf{w}}_k^t,{\mathbf{w}}_k^t\sim \mathcal{N}(0,{\mathbf{Q}}_k)$$

(27)

This creates dynamic factors in the factor graph:

$$f_k^{\mathrm{dyn}}\propto exp\left(-{\displaystyle \frac{1}{2}}{\parallel {\mathbf{o}}_k^{t+1}-{\mathbf{A}}_k{\mathbf{o}}_k^t-{\mathbf{B}}_k{\mathbf{u}}_k^t\parallel }_{{\mathbf{Q}}_k^{-1}}^2\right)$$

(28)

Consider a toy example tracking a single object ($k=1$) over three time steps ($t=1,2,3$). The factor graph would include static SLAM factors (odometry between camera poses ${\mathbf{x}}_1,{\mathbf{x}}_2,{\mathbf{x}}_3$ and landmark observations), dynamic object factors (three object states ${\mathbf{o}}_1^1,{\mathbf{o}}_1^2,{\mathbf{o}}_1^3$ connected by two dynamic factors $f_1^{\mathrm{dyn}}$ from Equation (30) representing the constant-velocity motion prior), and association factors (measurement factors linking each object state ${\mathbf{o}}_1^t$ to its corresponding image detection ${\mathbf{z}}_1^t$, obtained from camera pose ${\mathbf{x}}_t$). This minimal example clarifies how dynamic objects introduce additional variables and factors that are jointly optimized with the traditional SLAM graph.

3.4. Deep Learning-Based End-to-End Optimization

These methods represent the latest paradigm shift, extending differentiable optimization through neural networks and completing the evolution from explicit to implicit modeling.

3.4.1. Differentiable Optimization Layers

Differentiable optimization layers enable end-to-end learning of SLAM systems. A differentiable projective geometry module is also proposed [64,65] and the system learns feature extraction and matching through a differentiable correspondence network:

$$\mathbf{C}=\mathrm{SoftMax}\left({\displaystyle \frac{{\mathbf{F}}_s·{\mathbf{F}}_t^{\top }}{\sqrt{d}}}\right)$$

(29)

where ${\mathbf{F}}_s$ and ${\mathbf{F}}_t$ are source and target feature maps, and $\mathbf{C}$ is the correspondence matrix.

The pose is estimated through a differentiable PnP solver:

$${\mathbf{T}}^{\ast }=\arg \underset{\mathbf{T}}{min}\sum _i{\parallel \pi \left(\mathbf{T}{\mathbf{P}}_i\right)-{\mathbf{p}}_i\parallel }^2$$

(30)

with gradients flowing through the iterative optimization process.

3.4.2. Theoretical Foundations of Differentiable Optimization

Differentiable optimization layers introduce new theoretical challenges regarding gradient computation through iterative solvers. For the differentiable PnP problem in Equation (30), the gradient $\mathsf{\partial }{\mathbf{T}}^{\ast }/\mathsf{\partial }\mathbf{p}$ can be computed using the implicit function theorem:

$${\displaystyle \frac{\mathsf{\partial }{\mathbf{T}}^{\ast }}{\mathsf{\partial }\mathbf{p}}}=-{\left({\displaystyle \frac{{\mathsf{\partial }}^{2}E}{\mathsf{\partial }{\mathbf{T}}^2}}\right)}^{-1}{\displaystyle \frac{{\mathsf{\partial }}^{2}E}{\mathsf{\partial }\mathbf{T}\mathsf{\partial }\mathbf{p}}}$$

(31)

where $E(\mathbf{T},\mathbf{p})={\sum }_i{\parallel \pi \left(\mathbf{T}{P}_i\right)-p_i\parallel }^2$ is the PnP energy function. The conditioning of the Hessian ${\mathsf{\partial }}^{2}E/\mathsf{\partial }{\mathbf{T}}^2$ determines the stability of gradient computation. Poor conditioning, which occurs when points are nearly coplanar or have limited parallax, can lead to exploding gradients during training. Algorithm 3 provides the concrete implementation of the differentiable PnP solver described in Equation (31).

Algorithm 3 Differentiable Perspective-n-Point (PnP) Solver
Require: 3D point set $\left\{P_i\right\}\in {\mathbb{R}}^3$, 2D observation set $\left\{p_i\right\}\in {\mathbb{R}}^2$, maximum iterations K,
damping factor $\lambda$
Ensure: Camera pose $\mathbf{T}\in \mathrm{SE}\left(3\right)$ and complete gradient computation graph
1:	Initialize: $\mathbf{T}\leftarrow {\mathbf{I}}_{4\times 4}$	▹ Identity transformation matrix
2:	Initialize computational graph recorder
3:	for $k=1$ to K do
4:	Compute projected points: ${\widehat{p}}_i=\pi \left(\mathbf{T}{P}_i\right)$	▹$\pi$ is the camera projection function
5:	Compute residuals: $r_i={\widehat{p}}_i-p_i$
6:	Compute total error: $E={\displaystyle \frac{1}{2}}{\sum }_i{\parallel r_i\parallel }^2$
7:	if $k=1$ or $k=K$ then
8:	Record current computational graph for gradient backpropagation
9:	end if
10:	Compute Jacobian matrix: $\mathbf{J}={\displaystyle \frac{\mathsf{\partial }r}{\mathsf{\partial }\xi }}$	▹$\xi \in \mathfrak{se}\left(3\right)$ is the Lie algebra perturbation
11:	Compute Gauss-Newton update: $\Delta \xi =-{({\mathbf{J}}^{\top }\mathbf{J}+\lambda \mathbf{I})}^{-1}{\mathbf{J}}^{\top }r$
12:	Apply update on Lie group: $\mathbf{T}\leftarrow \mathbf{T}·exp(\Delta {\xi }^{\wedge })$
13:	if $\parallel \Delta \xi \parallel <ϵ$ then
14:	break	▹ Convergence check
15:	end if
16:	end for
17:	Construct gradient propagation path through all iterative steps
18:	return $\mathbf{T}$	▹ Pose estimate with attached gradient computation graph

Lipschitz continuity of the learned components is crucial for training stability. For the feature extraction network $f_{\theta }$, we require

$$\parallel f_{\theta }\left(I_1\right)-f_{\theta }\left(I_2\right)\parallel \le L\parallel I_1-I_2\parallel$$

(32)

where L is the Lipschitz constant.

The convergence of end-to-end learned SLAM systems lacks the well-established guarantees of traditional geometric methods. However, recent work has shown that under mild assumptions about the network architecture and training procedure, the learned components can converge to approximations of geometric primitives, providing empirical if not theoretical guarantees of performance.

Several empirical heuristics are commonly employed to enforce the required theoretical properties in practice. Spectral normalization can enforce Lipschitz continuity during training by controlling the spectral norm of network layers. Curriculum learning gradually increases optimization complexity during training to avoid poor local minima. Geometry constraints, such as incorporating depth measurements or stereo constraints, help anchor scene geometry. Additionally, pose priors from inertial measurements or odometry are used to constrain camera pose estimation.

3.4.3. Neural Scene Representations

Neural Radiance Fields (NeRF) have revolutionized scene representation [66,67]. A joint optimization of camera poses and scene representation is presented:

$${\Theta }^{\ast },{\mathcal{X}}^{\ast }=\arg \underset{\Theta ,\mathcal{X}}{min}\sum _{t,\mathbf{r}}{\parallel \widehat{C}(\mathbf{r};\Theta ,{\mathbf{T}}_t)-C\left(\mathbf{r}\right)\parallel }^2$$

(33)

where $\widehat{C}\left(\mathbf{r}\right)$ is the volume rendered color for ray $\mathbf{r}$:

$$\widehat{C}\left(\mathbf{r}\right)={\int }_{t_n}^{t_f}T\left(t\right)\sigma \left(\mathbf{r}\left(t\right)\right)\mathbf{c}(\mathbf{r}\left(t\right),\mathbf{d})dt$$

(34)

with $T\left(t\right)=exp\left(-{\int }_{t_n}^t\sigma \left(\mathbf{r}\left(s\right)\right)ds\right)$.

3.4.4. Generalization Theory for Neural Representations

Neural scene representations like NeRF pose unique theoretical challenges due to their over-parameterization. The generalization error can be bounded using the concept of neural tangent kernel (NTK):

$${\mathcal{L}}_{\mathrm{test}}\le {\mathcal{L}}_{\mathrm{train}}+\mathcal{O}\left(\sqrt{{\displaystyle \frac{\mathrm{effective}\mathrm{params}}{N_{\mathrm{rays}}}}}\right)$$

(35)

The implicit regularization of gradient-based optimization in over-parameterized networks tends to find solutions with low complexity, as characterized by the frequency bias of neural networks—they preferentially learn low-frequency functions before fitting high-frequency noise.

For joint optimization of camera poses and scene representation, the identifiability problem becomes more severe. The network can compensate for incorrect camera poses by learning distorted scene geometries, leading to local minima that are photometrically accurate but geometrically inconsistent. In practice, the techniques mentioned in the Implementation Techniques paragraph above are applied to mitigate this ambiguity.

The sample complexity of neural scene representations grows with scene complexity rather than dataset size, making them particularly suitable for SLAM applications where we optimize a single scene with many observations [68,69]. The sample complexity of neural scene representations grows with scene complexity…. Furthermore, their generalization to atypical measurements—such as those arising from adverse weather or sensor failure—remains an open challenge, as the training data distribution may not adequately cover these edge cases.

A direct comparison and summary of these methods is presented in

Table 1. Method comparison summary.

	Robust Est.	Semantic	Deep Learning
Assumptions	Majority inliers	Reliable detection	Training coverage
	Sparse outliers	Simple dynamics	Generalization
	Gaussian noise	Correct association	Sufficient compute
Sensors	Monocular/Stereo	RGB-D/LiDAR	Monocular
Robustness	M-estimators	Explicit modeling	Diff. layers
	Switch variables	Semantic constraints	Learned features
	Prob. DA	Motion models	Implicit rejection
Failures	High outliers	Detection errors	OOD failures
	Structured noise	ID switches	Debug difficulty
	Param. sensitivity	Unobservable	High cost

.

4. Mathematical Challenges and Future Directions

4.1. Foundational and Emerging Challenges in Optimization for Dynamic V-SLAM

The transition from static to dynamic environments does not merely add complexity; it fundamentally invalidates core assumptions of traditional V-SLAM and introduces new layers to the optimization problem. The challenges selected for discussion here are not an exhaustive list but represent the most critical bottlenecks that arise directly from extending the standard optimization framework to the dynamic formulation. They can be categorized into foundational challenges, which are inherent to the problem definition, and emerging challenges, which are amplified or created by modern solution paradigms.

The foundational challenges form an interdependent triad that defines the essential difficulty of dynamic V-SLAM. They arise directly from augmenting the state vector and corrupting the observation model with structured outliers.

First, scalability and computational tractability represent the cardinal challenge due to the combinatorial explosion of the state dimension. The real challenge lies in managing the effective complexity within real-time constraints. This necessitates approximate methods like sliding windows or incremental solvers, which trade optimality guarantees for tractability. Consequently, the scalability challenge is not just about processing larger problems but about designing optimization algorithms whose complexity grows gracefully with scene dynamics.

Second, parameter identifiability and observability present a key difficulty because dynamic SLAM is a severely under-constrained problem. A primary manifestation is the object-camera motion ambiguity, where the motion of an object observed from a limited set of viewpoints relative to the camera is not uniquely determinable. Mathematically, this induces a null space in the Fisher Information Matrix, making certain state components unobservable. This lack of identifiability leads to ill-posed optimization problems where the cost function has flat directions or multiple local minima that are geometrically distinct but photometrically similar. Ensuring identifiability therefore requires either sufficient viewpoint diversity, strong dynamical priors, or additional semantic or scale constraints.

Third, robustness to structured outliers is critical as dynamic objects generate measurements that are not random noise but structured outliers, spatially and temporally coherent signals that systematically violate the static observation model. Traditional M-estimators, designed for i.i.d. outliers, can fail against such structured corruption. Thus, the challenge evolves from simple outlier rejection to the more difficult task of inlier and outlier model selection within the optimization loop. This demands robust kernels or probabilistic frameworks that can discern geometrically consistent but physically invalid data associations.

In addition to these foundational issues, the adoption of advanced methods to solve them introduces new, secondary layers of difficulty, constituting the emerging challenges.

Specifically, the integration of deep learning into optimization pipelines, through learned features, differentiable layers, or implicit neural representations, shifts challenges from explicit modeling to generalization and stability. Key issues here include the Lipschitz continuity of learned components to guarantee stable gradients during optimization, the lack of convergence guarantees compared to geometric methods, and the identifiability problem in joint pose-NeRF optimization, where a neural radiance field can compensate for incorrect poses, leading to plausible yet geometrically inaccurate solutions.

Furthermore, theoretical guarantees for data-driven methods remain a significant gap. While empirical success is evident, providing certificates for learned models is essential. This includes understanding the sample complexity of neural scene representations, bounding their generalization error in unseen environments, and providing robustness guarantees against adversarial or out-of-distribution inputs. This challenge is particularly crucial for deploying learning-based SLAM in safety-critical applications.

Finally, the scalability of implicit representations themselves poses an open problem. Neural scene representations such as NeRF or Gaussian Splatting, while powerful, face scalability issues where their training and inference cost scales with scene complexity and required rendering resolution, posing a challenge for long-term, large-scale SLAM. Optimizing compact and efficient neural architectures for mapping therefore remains an active area of research.

In summary, the challenges highlighted here are selected because they represent fundamental limits arising from the core mathematical formulation of dynamic SLAM, namely scalability, identifiability, and robustness, and pivotal hurdles introduced by the field’s current trajectory towards learned, implicit models. Addressing them is synonymous with advancing the state of the art in robust, real-time perception for autonomous systems.

4.2. Future Research Directions

As for lifelong SLAM and continual learning, mathematical frameworks for lifelong SLAM must address catastrophic forgetting. Bayesian nonparametrics offer promising foundations:

$$G\sim \mathrm{DP}(\alpha ,H),{\theta }_i|G\sim G$$

(36)

where the Dirichlet Process (DP) prior allows for unbounded complexity growing with data.

For compositional scene understanding, future systems should reason about object relationships:

$$p\left(\mathcal{S}\right|\mathcal{O})=\prod _{\mathrm{relations}r}p\left(r({\mathbf{o}}_i,{\mathbf{o}}_j)\right)$$

(37)

where $\mathcal{S}$ represents scene structure and r are spatial, semantic, or functional relations.

5. Conclusions

This review has systematically reviewed mathematical optimization methods and their progress in visual SLAM for robots operating in complex dynamic environments. By establishing a taxonomy centered on mathematical optimization paradigms, we have organized existing approaches into three main categories: robust estimation theory-based methods for outlier rejection, semantic information and factor graph-based methods for explicit dynamic object modeling, and deep learning-based end-to-end optimization methods.

Our analysis reveals that the field of dynamic V-SLAM is evolving along three distinct yet interconnected trajectories. First, tighter Integration of Geometry and Semantics: Mathematical frameworks are transitioning from purely geometric optimization to unified formulations that simultaneously handle discrete semantic variables and continuous geometric states. This enables robots to achieve more reliable localization and mapping with contextual scene understanding. Second, paradigm Shift from Explicit Modeling to Implicit Learning: Optimization methods traditionally relying on precise geometric models and hand-crafted features are being complemented by data-driven implicit representations and differentiable optimization. This shift offers new possibilities for robots to adapt to unseen, unstructured dynamic environments. Third, algorithmic Scalability for Long-term Autonomy: As robots operate for extended periods in complex environments, mathematical optimization algorithms must balance computational complexity, memory efficiency, and long-term consistency. Techniques such as incremental optimization and continual learning provide promising pathways to address this challenge.

Looking forward, we identify several critical research directions for robotic dynamic V-SLAM:

Development of Unified Mathematical Frameworks: Creating frameworks that seamlessly integrate geometry, semantics, physical constraints, and task objectives to enable more comprehensive and robust environmental understanding by robots.

Theoretical Guarantees for Learning-Based Approaches: While data-driven methods demonstrate strong empirical performance in handling dynamic environments, their stability, convergence, and generalization capabilities require more rigorous theoretical analysis—particularly crucial for safety-critical robotic applications.

Scalable Optimization Algorithms for Lifelong Operation: Developing theoretically sound incremental and scalable optimization algorithms that address the needs of robots operating long-term in large-scale, unstructured, and dynamically changing environments.

Exploration of Compositional Scene Understanding Models: Investigating spatial, semantic, and functional relationships among objects in scenes to develop mathematical representations and reasoning methods that support advanced robotic tasks such as manipulation, navigation, and human-robot interaction.

As robots transition from controlled laboratory settings to unstructured real-world environments, the mathematical sophistication of V-SLAM systems must correspondingly increase. We believe the methods and perspectives surveyed in this work provide a solid foundation for these future developments and will inspire continued innovation at the intersection of robotic perception and mathematical optimization.