Robust Autonomous Perception for Indoor Service Machines via Geometry-Aware RGB-D SLAM and Probabilistic Dynamic Modeling

Abstract

Reliable autonomous perception is essential for indoor service machines operating in human-centered environments, where weak textures, repetitive structures, and frequent dynamic interference often degrade localization stability. Conventional RGB-D SLAM systems typically rely on static-scene assumptions or binary semantic masking, which are insufficient for handling persistent and fine-grained environmental dynamics. This paper presents a robust autonomous perception framework based on geometry-aware RGB-D SLAM, with a particular emphasis on probabilistic dynamic modeling at the feature level. The proposed system integrates multi-granularity geometric representations, including point features, parallel-line structures, and planar regions, to enhance geometric observability in low-texture indoor environments. On this basis, a probabilistic dynamic model is introduced to explicitly characterize feature reliability under motion, where dynamic probabilities are initialized by object detection and continuously updated through temporal consistency, spatial propagation, and multi-view geometric verification. Large-scale planar structures further serve as stable anchors to support robust pose estimation. Experimental results on the TUM RGB-D dynamic benchmark demonstrate that the proposed method significantly improves localization robustness, reducing the average ATE RMSE by approximately 66% compared with representative dynamic SLAM baselines. Additional evaluations on a real-world indoor dataset further validate its effectiveness for long-term autonomous perception under dense motion and frequent occlusions.

1. Introduction

With the increasing deployment of indoor service machines in human-centered environments such as homes, offices, hospitals, and commercial facilities, ensuring reliable autonomous perception has become a fundamental requirement for long-term autonomy and operational safety. Real-world indoor environments are often characterized by weak visual textures, repetitive human-made layouts, and frequent human-induced dynamics, which jointly challenge the stability of perception systems and may lead to localization degradation or failure. Developing perception frameworks that can maintain reliable localization under such complex and dynamic conditions therefore remains a critical problem for intelligent service machines [1].

Visual simultaneous localization and mapping (SLAM) serves as a core perception component for indoor service machines by enabling continuous pose estimation and spatial awareness. Among existing solutions, RGB-D SLAM is particularly attractive due to its ability to provide metric depth information and dense geometric cues with moderate hardware cost and power consumption [2,3]. However, most conventional RGB-D SLAM systems rely on static-scene assumptions and sparse point features, making them vulnerable to weak-texture regions, repetitive structural patterns, and dynamic elements commonly encountered in indoor service environments [4].

To improve robustness under such conditions, a variety of approaches have been proposed. Classical visual SLAM systems, including the ORB-SLAM family [5,6,7] and visual–inertial extensions [8], perform well in static and well-textured scenes but remain fragile in low-texture or repetitive environments. Structure-aware SLAM methods enhance geometric observability by exploiting human-made regularities such as line segments [9,10,11,12,13,14,15,16,17] and planar surfaces [18,19,20,21,22,23,24], while dynamic SLAM approaches typically rely on semantic segmentation or object detection to suppress the influence of moving objects [25,26,27,28,29,30,31,32,33]. Recent end-to-end methods attempt to model dynamics implicitly through learned photometric and geometric consistency [34,35,36,37].

Despite these advances, existing methods remain limited in realistic indoor service environments. Structure-aware approaches are often developed under static assumptions, whereas most dynamic SLAM methods handle scene dynamics in a binary manner by discarding features associated with detected objects. In practice, environmental dynamics are continuous and fine-grained: not all features on dynamic objects are unreliable, and many static features may be temporarily corrupted by occlusion or motion. Moreover, dynamic objects frequently appear near dominant structural elements, making it difficult to assess feature reliability using semantic cues alone. This reveals a fundamental limitation of existing approaches and highlights the need for feature-level uncertainty modeling in dynamic indoor scenes.

To address this issue, the aim of this work is to improve the robustness of autonomous perception for indoor service machines by jointly considering structural ambiguity and feature-level dynamic uncertainty. This paper proposes a geometry-aware RGB-D SLAM framework with probabilistic dynamic modeling in which multi-granularity geometric structures are integrated with feature-level dynamic probability estimation. Experimental results demonstrate that the proposed framework achieves robust and stable localization under dense motion and frequent occlusions, supporting long-term autonomous perception in complex indoor environments. The main contributions of this paper are summarized as follows:

• A geometry-aware perception backbone for indoor service machines. A unified RGB-D perception framework is developed by jointly exploiting point features, parallel-line structures, and planar regions. By integrating multi-granularity geometric representations, the proposed backbone improves geometric observability and pose stability in weak-texture and structurally repetitive indoor environments.
• A feature-level probabilistic dynamic modeling mechanism for reliability-aware perception. A probabilistic dynamic model is introduced to explicitly characterize the reliability of point and line features under environmental motion. Dynamic probabilities are initialized using object detection and continuously updated through temporal consistency analysis, spatial neighborhood propagation, and multi-view geometric verification, while large-scale planar structures serve as stable anchors in dynamic scenes.
• A semantic–geometric dynamic observation handling strategy for back-end optimization. A structure-aware observation handling mechanism is designed by jointly considering near-frame motion residuals, multi-keyframe projection consistency, and epipolar-geometry-based constraints. This strategy enables adaptive weighting and suppression of unreliable observations caused by motion and occlusion during pose optimization.

The remainder of this paper is organized as follows. Section 2 reviews related work on the subject of robust indoor perception under conditions of weak texture and dynamic interference. In Section 3, the proposed method is described in detail, along with an examination of its fundamental components. Section 4 presents the findings of experimental investigations conducted on benchmark and real-world indoor datasets. Section 5 concludes the paper and discusses future research directions.

2. Related Work

2.1. Geometry-Aware SLAM for Robust Indoor Perception

Conventional point-based SLAM systems often suffer from degraded robustness in indoor environments with weak textures or repetitive structures, where sparse or ambiguously distributed keypoints lead to unreliable data association and unstable pose estimation. To alleviate these limitations, a substantial body of research has explored the incorporation of higher-level geometric primitives, most notably line segments and planar surfaces, giving rise to point-line, point-plane, and unified point-line-plane SLAM frameworks.

Early point-line SLAM systems introduced line features as complementary constraints to ORB-SLAM-style pipelines. Representative PL-SLAM approaches [9,10] incorporated line endpoint reprojection residuals into pose optimization and improved robustness under partial occlusions and texture-deficient conditions. Subsequent work refined line representation and residual design to improve optimization stability. Structure PLP-SLAM [11] adopted Plücker coordinates with orthonormal parameterization to reduce redundancy in nonlinear optimization, while EPLF-VINS [12] reformulated line constraints using angular residuals to strengthen geometric consistency. Nevertheless, line segments are often short and fragmented in real images, making them sensitive to noise when used in isolation.

To further improve line reliability, later studies moved from individual segments to more coherent structural cues. PLFG-SLAM [13] employed clustering and graph-based modeling to capture relationships between point and line primitives, and DPLVO [14] introduced pixel-level collinearity residuals with uncertainty-aware weighting to suppress spurious line detections. In addition, UV-SLAM [15] and UL-SLAM [16] exploited vanishing directions to stabilize orientation estimation in structured scenes, while PPL-SLAM [17] explicitly modeled parallel-line groups to reinforce structural coherence and improve pose stability in indoor environments with repetitive layouts.

Planar primitives provide more stable and globally consistent geometric support in human-made environments. Point–plane SLAM methods leverage coplanarity, parallelism, and orthogonality constraints to reduce pose drift and improve long-term stability [18,19]. Building on these insights, unified point-line-plane SLAM frameworks have been proposed to jointly exploit multi-granularity geometric features within a single optimization formulation and to incorporate structural priors such as Manhattan-world constraints [20,21,22,23,24]. Although these geometry-aware approaches significantly enhance geometric observability in weak-texture indoor scenes, most of them are developed under static-scene assumptions and do not explicitly address feature reliability under dynamic interference, which limits their applicability in real-world indoor service environments.

2.2. Dynamic Scene Handling in Visual SLAM

Dynamic environments pose a fundamental challenge to visual SLAM, as moving objects violate the static-scene assumption and introduce erroneous observations that degrade data association and state estimation. In indoor service environments, pedestrians, movable furniture, and frequent occlusions further exacerbate this problem, making robust dynamic handling a critical component of practical SLAM systems. Existing dynamic SLAM approaches can be broadly categorized into geometry-based, learning-based, and hybrid semantic–geometric methods.

Geometry-based dynamic SLAM methods attempt to distinguish static and dynamic features using multi-view geometry, motion consistency, or depth stability without relying on semantic priors. Representative approaches include motion-statistics-based foreground modeling and adaptive filtering strategies, which estimate feature reliability from reprojection residuals or temporal motion patterns [25,26]. Other methods suppress dynamic interference by assigning static weights based on depth consistency or edge stability [27,28]. Although computationally efficient, these approaches rely on hand-crafted thresholds and heuristic rules, making them sensitive to parameter tuning in complex indoor scenes.

Learning-based methods explicitly introduce semantic information to detect potentially dynamic objects at the pixel or instance level. Systems such as DS-SLAM [29] and StaticFusion [30] integrate semantic segmentation or dense reconstruction into the SLAM pipeline to identify moving objects and recover static background structures. While these approaches provide intuitive and effective cues in cluttered environments, their performance is constrained by predefined semantic categories and the accuracy of segmentation masks. Moreover, their reliance on GPU resources and dense inference pipelines often limits deployability on lightweight service machines.

To balance robustness and computational efficiency, hybrid semantic–geometric methods combine semantic priors with geometric consistency constraints. Typical systems integrate object detection with optical flow, depth consistency, or motion clustering to refine dynamic region estimation while maintaining real-time performance [31,32,33]. For instance, SG-SLAM [31] enhances RGB-D SLAM by verifying semantic segmentation results using geometric consistency, whereas Dynamic-VINS [32] fuses RGB-D and inertial measurements to improve tracking robustness under motion disturbances. DRG-SLAM [33] jointly exploits semantic cues and geometric features to filter dynamic observations. These methods demonstrate that coupling semantics with geometry can significantly improve stability in dynamic indoor environments. However, most existing approaches adopt a binary object-level dynamic masking strategy in which all observations associated with detected objects are either entirely discarded or fully retained. Such hard decisions ignore the temporal evolution and geometric reliability of individual features, making the system vulnerable to over-suppression of informative constraints or insufficient rejection of transient dynamic interference.

More recently, neural implicit representation-based SLAM frameworks have emerged as an alternative paradigm for scene modeling and camera tracking, where the environment is represented using continuous neural fields optimized under photometric consistency constraints. Representative systems such as NICE-SLAM [34] and ESLAM [35] employ hierarchical feature grids or multi-scale tensor representations to reduce implicit query cost and enable dense reconstruction in static indoor scenes. These approaches achieve impressive mapping quality and geometric completeness under static assumptions. To extend neural implicit SLAM to dynamic environments, several works have incorporated semantic and geometric cues to explicitly handle motion. NID-SLAM [36] refines semantic masks using depth-based geometric information to improve robustness near object boundaries, while DN-SLAM [37] combines ORB feature tracking with neural implicit mapping and progressively removes dynamic observations using semantic segmentation and optical flow constraints. RoDyn-SLAM [38] further fuses semantic masks with optical flow to generate reliable motion masks and introduces a divide-and-conquer pose optimization strategy with edge-based geometric constraints to enhance inter-frame consistency. In contrast, DDN-SLAM [39] incorporates explicit probabilistic modeling by combining semantic features with a mixture Gaussian formulation and dynamic semantic loss functions to mitigate error propagation caused by occlusions. Although these neural implicit SLAM variants demonstrate strong capability in dense reconstruction and dynamic detail modeling, they fundamentally rely on dense photometric consistency and iterative implicit field optimization. This results in high computational cost, substantial memory overhead, and strong dependence on illumination stability and semantic prediction quality, often requiring GPU acceleration to maintain acceptable performance. Such characteristics limit their real-time localization robustness and long-term stability in practical indoor service machine scenarios, where computational resources are constrained and dynamic interference is persistent.

In contrast to the above methods, which treat dynamics primarily as a discrete classification problem, this work models dynamic behavior as a continuously evolving reliability probability at the feature level. By jointly exploiting semantic cues and geometric consistency across both temporal and spatial domains, the proposed probabilistic dynamic modeling framework enables soft reliability weighting rather than hard exclusion of observations. This formulation fundamentally differs from traditional binary semantic masking strategies and provides a more flexible and stable mechanism for handling dense dynamic interference while preserving informative geometric constraints in real indoor service environments.

3. Algorithm

3.1. System Overview

The proposed system, termed MGD-SLAM (Multi-Granularity Geometry-Aware Dynamic SLAM), is developed on top of the ORB-SLAM2 [6] framework to support robust autonomous perception for indoor service machines operating in environments with weak texture and dynamic interference. ORB-SLAM2 offers a reliable point-based optimization baseline, but its performance degrades in scenes where point features become sparse or unstable. To overcome this limitation at the perception level, MGD-SLAM extends the original framework by incorporating multi-granularity geometric representations and dynamic-aware feature processing. Specifically, point features are complemented by parallel line structures and planar regions to enhance geometric observability in low-texture and repetitive indoor environments. In addition, a probabilistic dynamic modeling mechanism is introduced to characterize feature reliability under motion, avoiding binary static-dynamic decisions.

As illustrated in Figure 1, MGD-SLAM is organized into three tightly coupled stages. For each incoming RGB-D frame, the front-end performs camera tracking and extracts multi-granularity geometric features, including ORB point features, parallel line structures reconstructed from two-dimensional line segments, and planar regions obtained through depth-based segmentation. In parallel, a YOLOv11-based object detector runs as an independent ROS node and provides semantic bounding boxes for potentially dynamic objects, which are associated with point and line features to initialize dynamic probabilities. Based on this initialization, the probabilistic dynamic modeling module estimates the motion likelihood of point and line features. In the back-end, observations are adaptively weighted according to their estimated dynamic probabilities, and point-parallel line-plane constraints are jointly optimized to refine the camera trajectory while preserving the structural consistency of the indoor map.

3.2. Multi-Granularity Feature Extraction

To enhance perception robustness in indoor service environments, the proposed system employs a multi-granularity geometric feature extraction strategy that integrates point features, parallel-line structures, and planar regions into a unified representation. By combining geometric cues at different spatial scales, the system maintains reliable data association under weak texture, repetitive patterns, and partial occlusion.

Point: Point features are extracted using the ORB operator due to its favorable balance between computational efficiency and robustness to viewpoint and illumination changes. For each RGB-D frame, detected keypoints $u_i^k$ are back-projected using depth measurements to obtain their corresponding 3D positions $X_i^k$. These point features provide appearance-based correspondences over short baselines and form the foundation of frame-to-frame tracking.

Parallel Line: To capture structural regularities beyond isolated line segments, MGD-SLAM represents line features at the level of parallel-line groups. Instead of relying on conventional detectors such as LSD [40] or EDLines [41], which output unorganized segments, an orientation-consistent grouping strategy is adopted to identify sets of line segments sharing similar directions [42].

Specifically, line segments are grouped based on the angular difference between their 2D orientations. Two segments are considered parallel candidates if the absolute orientation difference is smaller than a predefined threshold ${\tau }_{\theta }$. In our implementation, ${\tau }_{\theta }$ is set to 3°, following the default configuration adopted in PPL-SLAM [17], which has been validated for structured indoor environments. This threshold provides a good balance between grouping robustness and sensitivity to local structural variations.

Each group reflects a coherent structural element commonly observed in human-made indoor environments. Within each group, an individual line segment is parameterized using three sampled image points corresponding to its start point $u_{n,q,s}^k=(u_s,v_s)$, midpoint $u_{n,q,m}^k=(u_m,v_m)$, and endpoint $u_{n,q,e}^k=(u_e,v_e)$:

$${\mathit{s}}_{n,q}^k=\left(u_{n,q,s}^k,u_{n,q,m}^k,u_{n,q,e}^k\right),$$

(1)

where the midpoint $u_{n,q,m}^k$ provides an additional geometric sample along the segment. This three-point representation reduces depth back-projection ambiguity and improves the stability of inter-frame association compared with endpoint-only parameterizations. Using the depth map, the sampled points are back-projected into 3D space as

$$X_{n,q}^k=(X_{n,q,s}^k,X_{n,q,m}^k,X_{n,q,e}^k),$$

(2)

yielding a compact spatial representation that preserves the segment geometry while remaining efficient for optimization. To quantify the structural significance of each segment, its pixel length $D_{n,q}^k$ is defined as

$$D_{n,q}^k=‖u_{n,q,e}^k-u_{n,q,s}^k‖,$$

(3)

and a normalized salience score ${\gamma }_{n,q}^k$ is introduced as

$${\gamma }_{n,q}^k={\displaystyle \frac{L_{n,q}^k}{L_{\max }}},$$

(4)

where $L_{\max }=\max (H,W)$ denotes the dominant image dimension. This salience measure favors long, visually supported segments that are more likely to belong to persistent structural elements such as walls, shelves, or corridors.

Each segment is further associated with a set of parallel companions ${\widehat{S}}_n^k$, determined using the same orientation-consistency criterion. Following PPL-SLAM [17], no additional hierarchical merging is applied across groups; instead, grouping is performed locally within each frame to maintain robustness under viewpoint changes and partial occlusions. The complete representation of a line segment is summarized as

$${\mathcal{K}}_{n,q}^k=\left\{{\theta }_{n,q}^k,S{L}_{n,q}^k,D_{n,q}^k,{\gamma }_{n,q}^k,{\widehat{S}}_n^k\right\},$$

(5)

where ${\theta }_{n,q}^k$ denotes the segment orientation and $S{L}_{n,q}^k$ is the metric length derived from depth. These geometric attributes are integrated into an LBD-based descriptor, enabling appearance and structural cues to jointly support robust matching.

Plane: Planar regions are extracted from the depth-induced point clouds using an adaptive agglomerative hierarchical clustering (AHC) method, following the plane segmentation framework of Feng et al. [43], which provides a good balance between detection accuracy and computational efficiency for real-time indoor SLAM. The detected planes are subsequently parameterized using the minimal angular representation proposed by Zhang et al. [18] for pose optimization.

In AHC-based segmentation, locally fitted planar cells are iteratively merged based on spatial adjacency and normal consistency. Region merging is terminated when the plane fitting error exceeds a predefined threshold, as specified in [43]. To ensure geometric reliability, only planar regions with sufficient support are retained; in our implementation, planes with fewer than $T_{\mathrm{plane}}$ supporting points (approximately $0.05{ \mathrm{m}}^2$ in image space) are discarded. During segmentation, neighboring planar regions with consistent parameters are automatically merged, preventing over-segmentation and preserving dominant indoor structures such as walls, floors, and tabletops.

Each detected plane is represented by its unit normal vector $\mathit{n}=\left(n_x,n_y,n_z\right)$ and signed distance $d_{\Pi }$ to the origin. To improve numerical stability during pose optimization, the plane normal is further reparameterized using a minimal angular representation following [18]. Since the unit normal lies on the sphere and thus contains redundancy, its orientation is expressed by

$$\left\{\begin{array}{c}\alpha =\arctan 2\left(n_y,n_x\right)\\ \beta =\arcsin \left(n_z\right)\end{array}\right.,$$

(6)

where $\alpha$ denotes the azimuth of the normal projected on the horizontal plane and $\beta$ specifies its elevation. The plane is thus represented in the compact form:

$$q\left(\Pi \right)=\left(\alpha ,\beta ,d_{\Pi }\right),$$

(7)

which avoids the over-parameterization of raw normal vectors and provides a numerically well-conditioned representation for optimization.

Through the integration of point features, parallel-line structures and planar regions, the feature extraction module yields a structurally coherent description of each frame. This multi-granularity representation provides a robust geometric foundation for the probabilistic modeling and dynamic filtering mechanisms introduced in subsequent sections, enabling stable tracking in environments with weak texture, repeated patterns and heterogeneous dynamic interference.

3.3. Dynamic Object Detection

In indoor service environments, dynamic objects such as pedestrians, movable furniture, and carried items represent a major source of geometric inconsistency for visual SLAM systems. Their motion introduces transient observations that violate the static-scene assumption and can severely corrupt feature association and pose estimation if treated indiscriminately. Rather than directly discarding observations based on semantic labels, MGD-SLAM employs object detection as a semantic prior to initializing feature-level dynamic uncertainty.

A YOLOv11-based detector [44] is adopted to identify potentially dynamic objects in real time. The detector is initialized with COCO [45] pretrained weights and fine-tuned on scene-specific samples to ensure reliable detection of common indoor dynamic categories, including pedestrians, backpacks, chairs, and other movable objects. Bounding-box predictions with a confidence score higher than 0.5 are retained as valid detections. Importantly, the detected bounding boxes are not used to directly discard all enclosed observations. Instead, they serve as coarse semantic indicators of regions that are likely to contain independent motion. Point and line features falling inside such regions are assigned an initial dynamic probability, which is subsequently refined through temporal consistency, spatial propagation, and geometric verification, as described in Section 3.4. This probabilistic treatment avoids over-aggressive feature rejection and allows the system to handle partial motion, intermittent dynamics, and detection uncertainty in a principled manner.

To maintain modularity and computational efficiency, the detector runs as an independent ROS node and communicates with the SLAM system via lightweight message passing. Bounding-box predictions are synchronized with incoming RGB frames and associated with extracted geometric features without interfering with the core estimation pipeline. Representative detection results and the corresponding dynamic feature initialization are illustrated in Figure 2.

3.4. Probabilistic Dynamic Modeling

In realistic indoor environments, dynamic behavior cannot be reliably characterized using instantaneous semantic cues alone. Apparent motion induced by camera ego-motion, intermittent occlusions, and imperfect object detection often lead to ambiguous observations in which static and dynamic features are difficult to separate using binary decisions. To address this challenge, MGD-SLAM introduces a feature-level probabilistic dynamic model that treats motion state as a continuously evolving uncertainty rather than a discrete label.

Each geometric feature is associated with a dynamic probability that represents its likelihood of being influenced by independent motion. This probability is maintained and updated along the SLAM tracking thread, allowing dynamic behavior to be inferred from both current observations and historical evidence. While a unified probabilistic formulation is adopted, different geometric primitives exhibit distinct motion characteristics and are therefore modeled with tailored strategies within the same framework.

Point features are sensitive to both camera motion and object motion, making them the primary carriers of fine-grained dynamic information. For a 3D map point $X_i$ observed in frame ${\mathcal{F}}_k$, its dynamic probability is recursively updated using an exponential smoothing formulation:

$$P_k^{init}\left(X_i\right)=\left(1-\partial \right)P_{k-1}\left(X_i\right)+\partial S_k\left(x_i\right),$$

(8)

where $P_{k-1}\left(X_i\right)$ denotes the propagated probability from the previous frame ${\mathcal{F}}_{k-1}$, $S_k\left(x_i\right)$ is the observation-based likelihood derived from semantic–geometric cues (Section 3.5), and $\partial$ controls the balance between historical consistency and new evidence.

Temporal coherence is explicitly enforced through feature association. When a feature in frame ${\mathcal{F}}_k$ is successfully matched to an existing map point with a descriptor distance below a predefined threshold, its dynamic probability is inherited from the corresponding map representation. For newly observed features $x_i^k$, the initial probability is determined based on whether the feature lies within a region indicated as potentially dynamic by object detection:

$${\stackrel{⌢}{P}}_k\left(x_i^k\right)=\left\{\begin{array}{c}{P}_{k-1}\left(x_i^{k-1}\right), {‖B\left(x_i^k\right)-B\left(x_i^{k-1}\right)‖}_{\mathrm{H}}<\delta \hfill \\ P\left(X_i^k\right), {‖B\left(x_i^k\right)-B\left(x_i^{k-1}\right)‖}_{\mathrm{H}}<\delta \hfill \\ P_{\mathrm{init}}=0, k=1, Fla{g}_d=0\hfill \\ P_{\mathrm{init}}=0.5, k=1, Fla{g}_d=1\hfill \end{array}\right.,$$

(9)

where $B\left(\cdot \right)$ denotes the binary descriptor, ${‖\cdot ‖}_{\mathrm{H}}$ is the Hamming distance, $\delta$ is a matching threshold, and $Fla{g}_d$ indicates whether the feature lies inside a detection box associated with a dynamic object class. This strategy assigns conservative priors in dynamic regions while avoiding premature rejection of newly emerging static features.

Beyond temporal propagation, dynamic behavior also exhibits strong spatial coherence, as neighboring features often undergo similar motion patterns. To exploit this property, a neighborhood-based probability diffusion mechanism is introduced. Let $x_i^k$ denote a feature in frame ${\mathcal{F}}_k$ and $HP\left(x^k\right)$ the set of neighboring features within a radius r. The refined dynamic probability is computed as

$$P_k\left(x_i^k\right)={\stackrel{⌢}{P}}_k\left(x_i^k\right)+{\displaystyle \sum _{x_j^k\in HP\left(x^k\right)}\lambda \left(d_{ij}\right)}\left({\stackrel{⌢}{P}}_k\left(x_j^k\right)-{\stackrel{⌢}{P}}_k\left(x_i^k\right)\right),$$

(10)

where $d_{ij}$ measures the Euclidean distance between features $x_i^k$ and $x_j^k$, and $\lambda \left(\cdot \right)$ is a distance-dependent attenuation function defined as

$$\lambda \left(d_{ij}\right)=C{e}^{-d_{ij}/r},d_{ij}\le r,$$

(11)

where $C$ is a normalization constant. This formulation encourages locally consistent probability fields while preserving sharp transitions at motion boundaries. The overall procedure of frame-by-frame temporal propagation and neighborhood-based diffusion is summarized in Figure 3, which illustrates how feature-level dynamic probabilities evolve coherently across consecutive frames and spatial neighborhoods.

Parallel-line structures provide medium-scale geometric constraints and inherently exert a global influence on pose estimation. To capture partial motion corruption along a line segment, the probabilistic modeling principle is extended by evaluating the dynamic probability at the start point, midpoint, and endpoint of each segment. If any representative sample exhibits a high dynamic likelihood, the entire line segment is treated as unreliable. This design choice is intentionally conservative. Unlike point features, structural line constraints contribute globally to pose optimization, and even partial corruption may introduce severe geometric inconsistency. Therefore, conservative suppression prioritizes robustness and consistency over constraint density, effectively preventing unstable structural constraints from dominating the optimization in highly dynamic environments.

Planar features in indoor environments predominantly correspond to large static structures such as walls, floors, and ceilings. Their spatial extent and semantic meaning make them significantly less susceptible to independent motion. Accordingly, planar features are treated as static anchors in the proposed framework and are directly incorporated as reliable constraints in pose optimization without dynamic probability propagation.

3.5. Semantic–Geometric Observation Likelihood Construction

The probabilistic dynamic model introduced in Section 3.4 relies on observation likelihoods to incorporate new evidence from incoming frames. In dynamic indoor environments, no single cue is sufficient to reliably distinguish static and dynamic features. Semantic information may be incomplete, while purely geometric cues are susceptible to camera ego-motion and noise. To address this challenge, the observation likelihood $S_k\left(x_i\right)$ is constructed by jointly integrating semantic priors with geometric consistency constraints at multiple temporal scales.

At the short-term temporal scale, local motion consistency is evaluated based on image-plane velocities. Let $x_i^k={\left[u_i^k,v_i^k\right]}^{\mathrm{T}}$ denote the image coordinates of a feature in frame ${\mathcal{F}}_k$, and its matched location in frame ${\mathcal{F}}_{k-1}$. The instantaneous velocity is defined as

$${\mathit{v}}_i^k=\left[\begin{array}{c}{u}_i^k-u_i^{k-1}\\ v_i^k-v_i^{k-1}\end{array}\right],$$

(12)

To obtain a reference motion pattern for static structures, the mean velocity is computed using features located outside all detection bounding boxes:

$${\overline{\mathit{v}}}^k={\displaystyle \frac{1}{N^k-M^k}}{\displaystyle \sum _{i\in p_{n-box}}{\mathit{v}}_i^k},$$

(13)

where $N^k$ is the total number of features in frame ${\mathcal{F}}_k$, $M^k$ is the number of features inside detection boxes, and $p_{n-box}$ denotes the set of features outside these regions. For each feature, the deviation from the static background motion is quantified by the magnitude and directional residuals:

$$\left\{\begin{array}{c}{d}_k=‖{\mathit{v}}_i^k-{\overline{\mathit{v}}}^k‖\\ \delta {\theta }_k=\arctan \left({\displaystyle \frac{\delta v}{\delta u}}\right)\end{array}\right.,$$

(14)

where $\left(\delta u,\delta v\right)={\mathit{v}}_i^k-{\overline{\mathit{v}}}^k$. These residuals are mapped to a near-frame dynamic likelihood through a sigmoid function:

$$P_{LS}={\displaystyle \frac{1}{1+\exp \left(-\left[{\alpha }_{LS}\left(d_k^{LS}-d_{th}^{LS}\right)+\left(1-{\alpha }_{LS}\right)\left(\delta {\theta }_k-{\theta }_{th}^{LS}\right)\right]\right)}},$$

(15)

where ${\alpha }_{LS}$ balances magnitude and angular deviations, $d_{th}^{LS}$ and ${\theta }_{th}^{LS}$ are velocity thresholds. This term captures abrupt or inconsistent motion that cannot be explained by camera movement alone.

Short-term analysis is complemented by a long-term geometric consistency check across multiple keyframes. Let $\mathcal{K}{\mathcal{F}}_k$ denote the current keyframe, and let $\mathcal{C}(\mathcal{K}{\mathcal{F}}_k)$ denote the set of its first- and second-order covisible keyframes. For a map point observed in $\mathcal{K}{\mathcal{F}}_k$, its 3D position is projected into each covisible keyframe and compared with the corresponding matched feature $P_j^k$. The forward reprojection error is defined as

$$d_{jk}=‖P_j^k-P_j‖,$$

(16)

and its mean over all covisible keyframes as

$${\overline{d}}_{jk}={\displaystyle \frac{1}{\left|{\mathcal{K}}_n\right|}}{\displaystyle \sum _{j\in {\mathcal{K}}_n}‖P_j^k-P_j‖},$$

(17)

A backward projection error ${\overline{d}}_{jk}$ is computed analogously by projecting points from covisible keyframes back into the current keyframe. The averaged bidirectional reprojection error is then converted into a long-term consistency likelihood:

$$P_{pro}={\displaystyle \frac{1}{1+\exp \left(-{\alpha }_{pro}\left(\frac{{\overline{d}}_{jk}+{\overline{d}}_{kj}}{2}-d_{th}^{pro}\right)\right)}},$$

(18)

where ${\alpha }_{pro}$ controls the sensitivity and $d_{th}^{pro}$ is a reprojection error threshold. This term effectively captures slow-moving or intermittently moving objects that may not trigger strong short-term motion cues.

Semantic detection may still miss dynamic objects whose categories are not covered by the detector. To handle such cases, a structural geometric likelihood based on epipolar constraints is introduced. An overview of the epipolar geometry employed for this structural constraint is shown in Figure 4, where the geometric relationship between point correspondences, epipolar lines, and the associated residuals is illustrated.

Specifically, using features outside all detected dynamic regions, the relative pose between frame ${\mathcal{F}}_{k-1}$ and ${\mathcal{F}}_k$ is first estimated, from which the fundamental matrix is constructed as

$$\mathit{F}={\mathit{K}}^{-\mathrm{T}}{\left[{\mathit{t}}_{k,k-1}\right]}^{\vee }{\mathit{R}}_{k,k-1}{\mathit{K}}^{-1},$$

(19)

where $\mathit{K}$ is the intrinsic calibration matrix, ${\mathit{R}}_{k,k-1}$ and ${\mathit{t}}_{k,k-1}$ denote the relative rotation and translation, and ${\left[\cdot \right]}^{\vee }$ is the skew-symmetric operator. For a feature $x_i^k$ in frame ${\mathcal{F}}_{k-1}$, its corresponding epipolar line in frame ${\mathcal{F}}_k$ is given by

$${\mathit{l}}_i={\mathit{F}}^{\mathrm{T}}{x}_i^{k-1}={\left(l_1,l_2,l_3\right)}^{\mathrm{T}},$$

(20)

and the distance from the observed feature $x_i^k$ to this line is computed as

$$d_i={\displaystyle \frac{\left|{\left(p_i^k\right)}^{\mathrm{T}}{\mathit{l}}_i\right|}{\sqrt{l_1^2+l_2^2}}}.$$

(21)

Static features are expected to satisfy the epipolar constraint with small residuals. Therefore, an epipolar-based dynamic probability is defined as

$$P_{mf}={\displaystyle \frac{1}{1+\exp \left(-{\alpha }_{mf}\left(d_i-d_{th}^{mf}\right)\right)}},$$

(22)

where ${\alpha }_{mf}$ regulates the influence of the residual and $d_{th}^{mf}$ is a distance threshold. Features with large deviations from the epipolar geometry receive higher dynamic likelihood even in the absence of semantic cues.

Finally, the observation likelihood $S_k\left(x_i\right)$ is obtained by fusing the near-frame motion likelihood $P_{LS}$, long-term geometric consistency likelihood $P_{pro}$, and epipolar-based structural likelihood $P_{mf}$ using a weighted log-odds formulation:

$$S_k\left(x_i\right)={\displaystyle \frac{1}{1+e^{-\left({\omega }_{LS}\mathrm{logit}\left(P_{LS}\right)+{\omega }_{pro}\mathrm{logit}\left(P_{pro}\right)+{\omega }_{mf}\mathrm{logit}\left(P_{mf}\right)\right)}}},$$

(23)

where $\mathrm{logit}\left(z\right)=\ln \left(z/\left(1-z\right)\right)$ and ${\omega }_{LS}$, ${\omega }_{pro}$, ${\omega }_{mf}$ are non-negative weights. These weights can be adaptively modulated according to detector confidence, keyframe connectivity, and epipolar inlier ratios.

Through this semantic–geometric integration, the observation likelihood $S_k\left(x_i\right)$ becomes sensitive to both short-term and long-term dynamic behavior, as well as structural inconsistencies that are invisible to purely semantic or purely geometric approaches. Combined with the probabilistic propagation mechanism in Section 3.4, it provides a principled basis for dynamic feature suppression and adaptive weighting in back-end optimization.

The proposed probabilistic dynamic modeling involves several hyperparameters controlling temporal smoothing, spatial diffusion, and observation likelihood fusion. The specific values used in all experiments are summarized in

Table 1. Hyperparameters used in probabilistic dynamic modeling.

Parameter	Description	Value
$\partial$	Temporal smoothing factor	0.7
$\delta$	Descriptor matching threshold	0.6
r	Diffusion radius	0.15 m
$d_{th}^{LS}$	Velocity magnitude threshold	3
${\theta }_{th}^{LS}$	Velocity direction threshold	2
$d_{th}^{pro}$	Reprojection error threshold	2
$d_{th}^{mf}$	Epipolar distance threshold	2
${\omega }_{LS}$$,{\omega }_{pro}$$,{\omega }_{mf}$	Likelihood fusion weights	0.4, 0.4, 0.2

. All hyperparameters are fixed across datasets and scenes to avoid sequence-specific tuning. Their values were determined through empirical stability analysis on representative sequences and prior experience with indoor RGB-D SLAM systems. We observed that moderate variations around these values do not lead to significant performance degradation, indicating that the proposed model is not overly sensitive to hyperparameter selection.

3.6. Tightly Coupled Pose Optimization with Multi-Granularity Geometric Fusion

Following the probabilistic dynamic modeling and observation likelihood evaluation in Section 3.4 and Section 3.5, features with high dynamic probability are suppressed, while the remaining features constitute a geometrically coherent and reliability-aware support set for pose estimation. Unlike hard feature removal strategies, the proposed framework retains partially reliable observations through adaptive weighting, enabling robust optimization under intermittent and partial dynamics.

The filtered point features, parallel-line structures, and planar elements are jointly incorporated into a tightly coupled optimization framework that integrates geometry-aware initialization with multi-constraint bundle adjustment. The overall factor graph structure is illustrated in Figure 5, where different geometric primitives contribute complementary constraints to the camera pose.

For point features, a 3D map point $X_j$ observed in frame ${\mathcal{F}}_j$ is reprojected into frame ${\mathcal{F}}_k$ according to

$$u_{j\to k}=\prod \left({\mathit{R}}_{k,j}{X}_j+t_{k,j},\mathit{K}\right),$$

(24)

where $\prod \left(\cdot \right)$ denotes the projection function, $\mathit{K}$ is the intrinsic matrix, and $\left(\mathit{R}|\mathit{t}\right)$ represents the relative pose. The point reprojection residual is then defined as

$$e_{j\to k}^p={\widehat{u}}_k-u_{k,j},$$

(25)

that is, the Euclidean difference between the observed pixel location ${\widehat{u}}_k$ and its reprojection into frame ${\mathcal{F}}_k$.

Parallel line residuals incorporate both collinearity and parallelism constraints. Each structural line $s_{n,q}$ induces a normalized image line ${\mathcal{l}}_{n,q}^k$ computed from its two endpoints:

$${\mathcal{l}}_{n,q}^k=\eta {\mathcal{l}}_{n,q}^{k^{\prime }},$$

(26)

where ${\mathcal{l}}_{n,q}^{k^{\prime }}$ is unnormalized and $\eta$ is a scale factor. Given the 3D representatives $X_{n,q,s}^j$, $X_{n,q,m}^j$, and $X_{n,q,e}^j$ in frame ${\mathcal{F}}_j$, their projections onto frame ${\mathcal{F}}_k$ must lie on ${\mathcal{l}}_{n,q}^k$. The point-to-line for each representative point is computed as:

$$d_{n,q,x}^k={\displaystyle \frac{\left|{\left({\mathcal{l}}_{n,q}^k\right)}^T{u}_{n,q,x}^k\right|}{\sqrt{{\mathcal{l}}_1^2+{\mathcal{l}}_2^2}}},x\in \left\{s,m,e\right\}.$$

(27)

These distances constitute the point-to-line residuals used to enforce the collinearity of structural lines in frame ${\mathcal{F}}_k$. The Jacobian of this distance with respect to the pose ${\xi }_k$ follows from the chain rule:

$${\displaystyle \frac{\partial d_{n,q,x}^k}{\partial {\mathbf{\xi }}_k}}={\displaystyle \frac{\partial d_{n,q,x}^k}{\partial u_{j\to k}}}·{\displaystyle \frac{\partial u_{j\to k}}{\partial {\mathbf{\xi }}_k}},x\in \left\{s,m,e\right\}.$$

(28)

Collinearity is enforced by requiring that all three representative points $\left(s,m,e\right)$ remain aligned:

$$e_{n,q}^{col}=d_{n,q,s}^k+d_{n,q,e}^k-2\times d_{n,q,m}^k.$$

(29)

Parallelism constraints use the most structurally consistent parallel line ${\mathcal{l}}_{n,q,x}^{k,\parallel }$ discovered in Section 3.2. The corresponding residual is

$$e_{n,q}^{par}=d_{n,q,s}^{k,\parallel }+d_{n,q,e}^{k,\parallel }-2\times d_{n,q,m}^{k,\parallel }.$$

(30)

The full structural-line residual aggregates these constraints:

$$e_{n,q}^{\mathcal{l}}=e_{n,q}^{par}+e_{n,q}^{col}.$$

(31)

Planar features offer strong mid- and high-level geometric support to pose estimation, particularly in indoor environments dominated by walls, floors and ceilings. The reprojection error of a planar feature is constructed by comparing the plane ${\Pi }_k$ observed in the current frame ${\mathcal{F}}_k$ with its corresponding map plane ${\Pi }_m$. Both planes are expressed using the angular parameterization introduced in Section 3.2, from which their unit normals $n({\Pi }_k)$ and $n({\Pi }_m)$ are reconstructed as

$$n(\Pi )={\left(\cos \beta \cos \alpha ,\cos \beta \sin \alpha ,\sin \beta \right)}^T.$$

(32)

To enforce geometric consistency, three relationships are considered implicitly within a unified residual. The first term encourages coplanarity by aligning both the normal direction and the plane-to-origin distance under the estimated pose. The second term promotes parallel consistency when two planar regions are expected to share a common dominant orientation, while the third term penalizes deviations from expected orthogonality relations between major structural surfaces. Combining these components, the planar residual is formulated compactly as

$$e_m^{\Pi }=\left[\begin{array}{c}\theta \left({\mathit{R}}_{\mathcal{W}\to k}n\left({\Pi }_m\right),n\left({\Pi }_k\right)\right)\\ \left(d_{{\Pi }_m}+n{\left({\Pi }_m\right)}^{\mathrm{T}}{\mathit{t}}_{\mathcal{W}\to k}\right)-d_{{\Pi }_k}\end{array}\right],$$

(33)

where $R_{k\leftarrow \mathcal{W}}$ and $t_{k\leftarrow \mathcal{W}}$ are the rotation and translation from the world map to the current frame, and $\theta \left(\cdot ,\cdot \right)$ measures the angular discrepancy between two unit normals. This formulation allows coplanar, parallel and perpendicular structural relations to be handled naturally within the same optimization framework without introducing separate case-specific constraints.

By embedding point, line, and plane residuals into a unified optimization framework, the solver exploits both fine-grained local correspondences and high-level structural regularities. Planar features act as stable anchors, parallel-line structures provide medium-scale directional constraints, and point features capture local geometric details. Under Gaussian noise assumptions, the final optimization problem is solved using the Levenberg–Marquardt algorithm with robust loss functions:

$$\mathit{\xi }⁎=argmin\left[{\displaystyle \sum _i{\rho }_p\left({e_{i,j\to k}^p}^T{\sum }_p^{-1}{e}_{i,j\to k}^p\right)}+{\displaystyle \sum _{n,q}{\rho }_{\mathcal{l}}\left({e_{n,q}^{\mathcal{l}}}^T{\sum }_{\mathcal{l}}^{-1}{e}_{n,q}^{\mathcal{l}}\right)}+{\displaystyle \sum _m{\rho }_{\Pi }\left({e_m^{\Pi }}^T{\sum }_{\Pi }^{-1}{e}_m^{\Pi }\right)}\right],$$

(34)

where ${\Sigma }_p$, ${\Sigma }_{\mathcal{l}}$ and ${\Sigma }_{\Pi }$ are inverse covariance matrices, and $\rho \left(\cdot \right)$ is the Huber loss. The Levenberg–Marquardt algorithm is used to obtain the final camera pose.

Through this tightly coupled formulation, the pose solver explicitly accounts for feature reliability inferred from probabilistic dynamic modeling, resulting in robust and stable localization in weak-texture, repetitive, and partially dynamic indoor environments.

4. Results

To verify the performance of the proposed algorithm, a series of experimental validations were conducted. All experiments were conducted on a laptop configured with an AMD Ryzen 7 5800H processor (3.2 GHz) and 16 GB RAM, running Ubuntu 18.04.

4.1. Experimental Setup

To comprehensively evaluate the effectiveness of the proposed dynamic multi-feature fusion SLAM system, an experimental framework was established that covers both standard benchmark datasets and real-world service machine scenarios.

The TUM RGB-D dataset [46] is selected as the primary benchmark for quantitative evaluation. This dataset provides synchronized RGB–D observations acquired using real sensing platforms operating in real indoor environments, rather than simulated or synthetic settings. As illustrated in Figure 6a, the RGB-D sensor is mounted on a handheld or mobile platform and captures natural indoor scenes with real human motion, occlusions, and viewpoint changes. The dynamic sequences include two representative motion patterns. The sitting sequences (“s-”) mainly contain static backgrounds with localized hand movements, whereas the walking sequences (“w-”) involve continuous human motion, frequent occlusions, and large-scale dynamic interference. Owing to these characteristics, the TUM RGB-D dataset has become a widely adopted benchmark for assessing the robustness of SLAM systems in dynamic indoor environments.

The proposed method is compared against several representative dynamic SLAM systems, including DS-SLAM [29], SG-SLAM [31], Dynamic-VINS [32], and DRG-SLAM [33]. Since the TUM RGB-D dataset does not provide inertial measurements, all compared systems are evaluated in RGB-D mode to ensure a fair and consistent comparison. To further position the proposed approach within the broader SLAM landscape, several state-of-the-art end-to-end neural SLAM methods, including NICE-SLAM [34], ESLAM [35], NID-SLAM [36], RoDyn-SLAM [37], and DDN-SLAM [38], are additionally included. These methods emphasize neural implicit scene modeling and dense reconstruction under dynamic conditions, providing a complementary perspective to geometry-driven approaches.

To analyze the independent contribution of probabilistic dynamic modeling, an ablation study is conducted by comparing the full system with a variant in which dynamic probability estimation and filtering are disabled, leaving only multi-granularity geometric features for pose estimation. ORB-SLAM3 operating in RGB-D mode is further included as a classical geometry-based baseline. This configuration allows a direct assessment of how probabilistic dynamic modeling improves robustness and suppresses drift in highly dynamic indoor scenes.

Beyond benchmark evaluation, the THUD dataset [47] is employed to assess the proposed system in realistic service machine scenarios. As shown in Figure 6b, THUD is collected using a mobile service robot platform operating in real indoor environments over long durations. The dataset features dense pedestrian flows, severe occlusions, abrupt viewpoint changes, and extended trajectories, providing a challenging testbed for evaluating long-term localization stability and map consistency in service-oriented applications.

In all experiments reported in this paper, loop closure is intentionally disabled for all evaluated methods. This design choice aims to focus the evaluation on front-end tracking robustness and the effectiveness of dynamic observation handling, without the influence of global pose graph optimization or loop-induced error redistribution. Disabling loop closure also reflects practical service machine scenarios, where long-term loop opportunities may be sparse or unreliable in highly dynamic indoor environments.

For quantitative evaluation using ATE and RPE, only trajectory segments with successful and continuous tracking are considered.

For quantitative evaluation, Absolute Trajectory Error (ATE) is adopted as the primary metric to measure global trajectory consistency. Following the standard protocol, the estimated trajectory is first aligned to the ground-truth trajectory by a single rigid-body transformation $\mathit{S}\in SE(3)$, where $SE(3)$ denotes the group of 3D rigid transformations (rotation and translation). Given the estimated pose ${\mathit{T}}_t^e\in SE\left(3\right)$ and the ground-truth pose ${\mathit{T}}_t^g\in SE\left(3\right)$, ATE is computed as the root-mean-square (RMS) translational error of the aligned pose discrepancy:

$${\mathrm{ATE}}_{RMSE}=\sqrt{{\displaystyle \frac{1}{N}}{\displaystyle \sum _{t=1}^N{‖trans\left({\left({\mathit{T}}_t^g\right)}^{-1}\mathit{S}{\mathit{T}}_t^e\right)‖}^2}}.$$

(35)

To complement global accuracy, Relative Pose Error (RPE) is used to quantify short-term drift and local motion accuracy, particularly in sequences with rapid movements or dense dynamic interference. For a time interval $\Delta$, RPE is defined as

$${\mathrm{RPE}}_{RMSE}=\sqrt{{\displaystyle \frac{1}{N-\Delta }}{\displaystyle \sum _{t=1}^{N-\Delta }{‖trans\left({\left({\left(T_t^g\right)}^{-1}{T}_{t+\Delta }^g\right)}^{-1}\left({\left(T_t^e\right)}^{-1}{T}_{t+\Delta }^e\right)\right)‖}^2}}$$

(36)

While ATE reflects long-term drift accumulation, RPE characterizes frame-to-frame consistency and sensitivity to dynamic disturbances. When a method fails to maintain tracking and cannot re-initialize, the evaluation is terminated at the failure point, and no extrapolation or post-recovery alignment is applied. This protocol ensures a fair comparison of robustness under dynamic interference, where early tracking failure directly reflects the system’s inability to cope with challenging conditions rather than being masked by back-end optimization.

For sequences without ground-truth trajectories, qualitative evaluation is conducted through visual inspection of reconstructed point clouds and dense maps, focusing on the structural integrity of walls, floors, and furniture, as well as the continuity of the estimated camera trajectory.

Through the integration of benchmark comparisons, ablation studies, and real-world service machine validation, the experimental setup provides a comprehensive assessment of the proposed SLAM system in terms of trajectory accuracy, local stability, robustness to dynamic interference, and long-term mapping consistency.

4.2. Quantitative Evaluation of Pose Accuracy on Dynamic RGB-D Sequences

Building upon the experimental setup described in Section 4.1, this subsection reports the quantitative evaluation results on the dynamic sequences of the TUM RGB-D dataset. ATE is adopted as the primary evaluation metric, and all RMSE values are computed using the EVO toolkit [48]. Table 1 compares the proposed MGD-SLAM with representative geometry-based dynamic SLAM systems, while

Table 2. ATE RMSE Comparison between Traditional Dynamic SLAM and the Proposed MGD-SLAM on the TUM RGB-D Dynamic Dataset (Unit: m).

Sequence	ORB-SLAM3	SG-SLAM	DRG-SLAM	Dynamic-VINS	DS-SLAM	MGD-SLAM
s_half	0.019	0.357	0.073	0.079	0.015	0.034
s_rpy	0.031	0.454	0.032	0.098	0.029	0.042
s_static	0.009	0.163	0.007	0.011	0.012	0.009
s_xyz	0.031	0.316	0.014	0.033	0.031	0.021
w_half	0.427	0.521	0.567	0.059	0.032	0.022
w_rpy	0.829	0.641	0.042	0.167	0.446	0.038
w_static	0.144	0.349	0.011	0.049	0.007	0.007
w_xyz	0.501	0.551	0.022	0.050	0.032	0.013
Avg.	0.249	0.419	0.096	0.068	0.076	0.023

Bold values indicate the best performance; underlined values indicate the second-best performance.

reports results against state-of-the-art end-to-end neural SLAM approaches. For clarity, the best and second-best results are highlighted accordingly.

As shown in Table 2, the proposed MGD-SLAM demonstrates a clear and consistent performance advantage across most dynamic sequences. On average, MGD-SLAM achieves an ATE RMSE of 0.023 m, representing an improvement of approximately 66% over the best-performing traditional dynamic SLAM baseline, Dynamic-VINS (0.068 m). This result indicates that the integration of multi-granularity geometric features with probabilistic dynamic modeling substantially enhances trajectory accuracy in dynamic indoor environments. In nearly static sequences such as “s_static”, all methods exhibit relatively low errors. Even in this favorable scenario, MGD-SLAM achieves the best or near-best performance, benefiting from the additional geometric constraints introduced by point–parallel-line–plane fusion. In moderately dynamic sequences (“s_half” and “s_xyz”), where local motion and partial occlusions are present, the probabilistic dynamic modeling mechanism effectively suppresses inconsistent observations, enabling stable tracking and low drift. The performance gap becomes more pronounced in highly dynamic walking sequences (“w_half”, “w_xyz”, and “w_rpy”). These sequences involve wide camera motion and dense human activity, where dynamic interference severely degrades traditional SLAM pipelines. MGD-SLAM consistently achieves the lowest ATE across all w-type sequences, demonstrating its robustness under strong dynamic disturbances. In contrast, DS-SLAM suffers noticeable degradation in “w_rpy”, where epipolar-geometry-based filtering becomes unreliable under rapid camera rotation. Dynamic-VINS, which operates without inertial measurements on this dataset, shows increased sensitivity to human motion and viewpoint changes. SG-SLAM performs reasonably when foreground motion dominates but becomes unstable in cluttered environments with complex spatial layouts. DRG-SLAM reaches competitive accuracy in isolated cases, yet its overall performance remains clearly inferior. It is worth noting that certain sequences, such as “w_static”, exhibit limited geometric diversity, where only a single dominant planar structure (e.g., a tabletop) is visible while larger structural elements such as the floor are partially occluded. In such cases, global structural regularities are weakly activated. Despite this limitation, MGD-SLAM maintains high accuracy, indicating that the proposed point-parallel line-plane fusion framework contributes to robustness even when strong global structural cues are unavailable.

Table 3. ATE RMSE comparison between state-of-the-art end-to-end dynamic SLAM methods and the proposed method on the TUM RGB-D dynamic dataset (Unit: m).

Sequence	NICE-SLAM	ESLAM	NID-SLAM	Rodyn-SLAM	DDN-SLAM	MGD-SLAM
w_half	0.134	0.617	0.070	0.056	0.023	0.022
w_rpy	0.734	0.573	0.678	0.045	0.039	0.038
w_static	0.491	0.075	0.063	0.017	0.010	0.007
w_xyz	0.421	0.435	0.072	0.083	0.013	0.013
Avg.	0.445	0.425	0.221	0.050	0.021	0.020

Bold values indicate the best performance.

further compares MGD-SLAM with recent end-to-end neural SLAM approaches. Overall, the proposed method achieves accuracy comparable to or better than state-of-the-art neural SLAM systems across all evaluated sequences, attaining the lowest or near-lowest ATE in most cases. In terms of average performance, MGD-SLAM slightly outperforms the best competing neural method, DDN-SLAM, by approximately 5%. Neural implicit SLAM methods typically rely on dense photometric consistency, making them sensitive to weak-texture regions, transient occlusions, and dynamic appearance changes. This limitation is particularly evident in “w_rpy”, where NICE-SLAM and ESLAM suffer from severe drift. Although NID-SLAM and RoDyn-SLAM incorporate enhanced dynamic modeling strategies, their long-term stability remains limited. DDN-SLAM performs competitively on several sequences but still falls slightly short in overall accuracy.

Overall, these quantitative results demonstrate that MGD-SLAM achieves strong robustness across a wide range of dynamic intensities, motion patterns, and geometric configurations. Compared with traditional geometry-based SLAM methods, it delivers substantially improved accuracy in dynamic environments. Compared with end-to-end neural SLAM systems, its explicit geometric modeling and feature-level probabilistic reasoning enable more stable localization under weak texture, heavy occlusion, and rapid appearance variation. These results validate the effectiveness of integrating multi-granularity geometric features with probabilistic dynamic modeling for robust indoor RGB-D SLAM

4.3. Effectiveness of the Dynamic Feature Removal Strategy

To evaluate the effectiveness of the proposed dynamic feature removal strategy, experiments are conducted on the highly dynamic walking sequences of the TUM RGB-D dataset. Three representative baselines are selected for comparison. ORB-SLAM3 serves as a classical point-based SLAM system without explicit dynamic handling. MWSLAM incorporates point, line, and plane features and therefore shares the multi-granularity geometric representation adopted in this work, but it does not include any dynamic modeling mechanism. MGD-SLAM-nD is an ablation variant of the proposed system, in which the probabilistic dynamic modeling and feature removal modules are disabled while all other components remain unchanged. This setup enables a direct and controlled assessment of the contribution introduced by the dynamic feature removal strategy.

The quantitative results reported in

Table 4. Comparison of ATE RMSE on TUM RGBD dynamic dataset (Unit: m).

Sequence	ORB-SLAM3	MWSLAM	MGD-SLAM-nD	MGD-SLAM
w_half	0.427	0.293	0.340	0.022
w_rpy	0.829	0.164	0.148	0.038
w_static	0.144	0.019	0.020	0.007
w_xyz	0.501	0.291	0.279	0.013
Avg.	0.475	0.192	0.197	0.020

Bold values indicate the best performance.

and

Table 5. Comparison of RPE RMSE on TUM RGBD dynamic dataset (Unit: rad).

Sequence	ORB-SLAM3	MWSLAM	MGD-SLAM-nD	MGD-SLAM
w_half	0.521	1.268	0.705	0.424
w_rpy	0.641	0.878	0.832	0.595
w_static	0.349	0.585	0.551	0.184
w_xyz	0.551	0.813	0.612	0.396
Avg.	0.516	0.886	0.675	0.400

Bold values indicate the best performance.

demonstrate the substantial impact of dynamic feature removal on both translational and rotational accuracy. In terms of ATE RMSE, ORB-SLAM3 exhibits severe degradation in dynamic scenes, with an average error of 0.475 m across the four walking sequences. Introducing multi-granularity geometric constraints significantly improves performance, as MWSLAM and MGD-SLAM-nD reduce the average error to 0.192 m and 0.197 m, respectively. However, both methods remain sensitive to dense foreground motion, since dynamic observations are still treated as valid constraints during optimization. By explicitly modeling feature-level dynamic probability and removing motion-inconsistent observations, MGD-SLAM further reduces the average ATE RMSE to 0.020 m, corresponding to a reduction of over 95% compared with ORB-SLAM3 and nearly 90% relative to both MWSLAM and MGD-SLAM-nD. The improvement is particularly evident in sequences such as “w_half” and “w_xyz”, where dense pedestrian motion causes significant drift in methods without dynamic filtering. In these cases, eliminating dynamic features effectively prevents moving objects from corrupting global trajectory estimation.

A similar trend is observed for rotational accuracy. Although MGD-SLAM-nD benefits from point-parallel line-plane fusion, its RPE RMSE increases noticeably in sequences involving rapid rotation or heavy occlusion, where dynamic features and structural cues may simultaneously degrade. In contrast, the full MGD-SLAM consistently achieves the lowest rotational errors across all evaluated sequences, as shown in Table 5. This improvement indicates that dynamic probability estimation and multi-frame geometric consistency checks not only suppress translation drift but also stabilize orientation estimation under complex motion patterns.

To further illustrate the effect of dynamic feature removal on global trajectory consistency, Figure 7 presents qualitative comparisons of estimated trajectories against ground truth for four representative walking sequences. While ORB-SLAM3 and MGD-SLAM-nD exhibit visible divergence and accumulated drift, the trajectory produced by MGD-SLAM closely follows the ground truth, maintaining a smooth and consistent shape throughout the sequence. The reduced deviation in regions with strong dynamic interference confirms that the proposed dynamic filtering mechanism effectively preserves reliable geometric constraints for pose estimation.

Overall, these results demonstrate that multi-granularity geometric fusion alone is insufficient to ensure robustness in highly dynamic indoor environments. Explicit dynamic feature removal plays a critical role in preventing motion-induced corruption of the optimization process. By combining probabilistic dynamic modeling with geometric consistency across multiple frames, MGD-SLAM achieves significantly improved translational and rotational accuracy, enabling stable and reliable localization in challenging dynamic scenarios.

4.4. Evaluation in Complex Large-Scale Dynamic Indoor Environments

To further evaluate the robustness and practical applicability of the proposed MGD-SLAM in real-world service machine scenarios, experiments are conducted on two large-scale indoor sequences from the THUD dataset: the “store-c1” supermarket scene and the “canteen-p1-c1” cafeteria scene. These environments exhibit rich human-made structures together with different levels of dynamic interference, ranging from intermittent pedestrian motion to extremely dense and highly unpredictable human activity. MGD-SLAM is compared with ORB-SLAM3, MWSLAM, and the ablation variant MGD-SLAM-nD to analyze the role of dynamic modeling in large-scale deployments.

In large-scale indoor service scenarios, the evaluation protocol differs from that of short benchmark sequences, as tracking continuity and mapping stability become the primary concerns. For the “store-c1” sequence, all evaluated methods experience tracking interruptions caused by dynamic occlusions and repeated structural layouts, preventing the completion of a continuous trajectory over the entire sequence. Under such conditions, trajectory-based quantitative metrics are less informative. Therefore, performance is assessed through visual comparison of estimated trajectories and mapping results, with particular attention to trajectory smoothness, consistency, and failure behavior.

For the “canteen-p1-c1” sequence, the environment is dominated by extremely dense and irregular human motion. In this case, evaluation focuses on qualitative mapping results and trajectory continuity, emphasizing the geometric coherence of reconstructed structures and the absence of abnormal distortions, such as bent corridors, broken planar surfaces, or abrupt trajectory jumps. This evaluation strategy reflects practical service machine deployment scenarios, where maintaining stable localization and structurally consistent maps under persistent dynamic interference is of greater importance than frame-wise trajectory accuracy.

The “store-c1” sequence represents a typical service environment with narrow aisles, densely arranged shelves, and intermittent pedestrian motion, as shown in Figure 8a. From a perception perspective, such environments simultaneously challenge SLAM systems in two aspects: (i) repeated structural patterns that increase the ambiguity of data association, and (ii) dynamic occlusions that intermittently corrupt feature observations, especially during initialization. Figure 8b and Figure 9 present the estimated trajectories and mapping results produced by different systems. ORB-SLAM3 fails to maintain stable tracking and terminates early, indicating that purely point-based representations are insufficient to cope with dense occlusions in cluttered indoor layouts. MWSLAM benefits from point–line–plane fusion and remains operational for a short period; however, without explicit dynamic feature handling, motion-inconsistent observations accumulate and lead to noticeable trajectory oscillations. MGD-SLAM-nD further improves short-term stability by exploiting more coherent multi-granularity geometric constraints, yet it still drifts and eventually loses tracking as dynamic foreground features persistently enter the optimization. In contrast, MGD-SLAM exhibits substantially enhanced robustness. By explicitly modeling feature-level dynamic probabilities and suppressing motion-inconsistent observations, the system maintains stable tracking for the longest duration among all compared methods. Although the full sequence is not completed, the resulting trajectory remains smooth and structurally consistent, and the reconstructed map preserves clear aisle and shelf layouts, as illustrated in Figure 9d. This behavior confirms that probabilistic dynamic modeling is essential for preventing dynamic clutter from degrading global pose estimation in structurally repetitive service environments.

The “canteen-p1-c1” sequence constitutes an extreme dynamic scenario captured during peak dining hours, featuring dense crowds, frequent occlusions, and rapid, irregular human motion (Figure 10). In such conditions, dynamic objects dominate the visual field and severely violate the static-scene assumption underlying classical SLAM systems. As shown in Figure 11, ORB-SLAM3 fails to initialize due to overwhelming dynamic interference, while MWSLAM also breaks down at an early stage despite its use of structural features. MGD-SLAM-nD manages a brief period of operation by relying on large static elements such as ceilings and upper walls; however, without dynamic probability filtering, the continuous influx of moving features quickly destabilizes pose estimation. MGD-SLAM demonstrates markedly superior performance in this highly dynamic environment. The system selectively preserves reliable planar and structural constraints while aggressively down-weighting or removing dynamic features associated with human motion. As a result, MGD-SLAM maintains the longest continuous trajectory and produces the most coherent map among all evaluated methods, even under extreme crowd density. These results highlight the importance of combining structural geometry with probabilistic feature-level dynamic reasoning for maintaining localization stability in real-world service machine scenarios.

The experiments on the THUD dataset reveal that multi-granularity geometric fusion alone is insufficient for robust SLAM in large-scale, crowded indoor environments. While MWSLAM and MGD-SLAM-nD benefit from richer geometric constraints, their lack of explicit dynamic discrimination renders them vulnerable to persistent motion interference. In contrast, MGD-SLAM consistently achieves superior stability and mapping quality by integrating probabilistic dynamic modeling with structure-aware optimization. This capability is particularly critical for service machines operating in public indoor spaces, where long-term autonomy requires resilience to dense crowds, frequent occlusions, and continuously changing scene dynamics.

4.5. Algorithm Runtime Analysis

To evaluate the computational efficiency of MGD-SLAM, we report the average runtime of its main modules on the dynamic sequences of the TUM RGB-D dataset, measured on the same platform described in Section 4.1.

Table 6. Average runtime of main modules in MGD-SLAM on the TUM RGB-D dataset.

Module	Average Time (ms)
Multi-granularity feature extraction	31
Dynamic feature removal	9
Semantic detection (asynchronous)	12
Total tracking time	46

reports the average per-frame time consumption of the major modules in the proposed system. The multi-granularity geometric feature extraction module, including point, parallel-line, and planar features, requires approximately 31 ms per frame. Although multiple feature types are extracted, they are processed in parallel, resulting in moderate overhead compared to point-only pipelines.

The proposed dynamic feature removal module consumes approximately 9 ms per frame, benefiting from lightweight feature-level probabilistic updates and efficient geometric consistency checks. Semantic object detection is performed using a YOLO-based detector running as an independent ROS node. On the test platform, the detector processes approximately 83 frames per second (≈12 ms per frame) and operates asynchronously, thus not blocking the main SLAM tracking thread.

Overall, MGD-SLAM achieves an average tracking time of approximately 46 ms per frame, corresponding to about 22 FPS, which demonstrates near real-time performance while significantly improving robustness in dynamic indoor environments.

5. Conclusions

This paper presented MGD-SLAM, a geometry-aware RGB-D SLAM framework designed for robust autonomous perception in dynamic indoor service machine environments. The proposed method integrates multi-granularity geometric features with a feature-level probabilistic dynamic modeling mechanism, enabling reliable suppression of motion-inconsistent observations while preserving stable structural constraints from points, parallel-line structures, and planar regions. Extensive experiments on public benchmarks and real-world service environments demonstrate that MGD-SLAM significantly improves localization robustness and trajectory consistency under dynamic interference, weak texture, and crowded conditions. Compared with traditional dynamic SLAM systems, the proposed framework achieves substantially lower trajectory errors, while maintaining performance comparable to recent neural SLAM approaches without relying on dense photometric optimization or heavy computational resources.

Despite these advantages, the performance of MGD-SLAM may degrade in challenging scenarios where RGB-D sensing itself becomes unreliable, such as environments with strong specular reflections (e.g., mirrors or glass surfaces), poor depth quality, or extreme illumination changes. In such cases, inaccurate depth measurements and unstable geometric observations can limit the effectiveness of geometry-based constraints and probabilistic feature modeling. Future work will therefore focus on incorporating more robust multi-modal sensing strategies, such as tighter fusion with inertial or polarization cues, as well as higher-level semantic reasoning to better handle reflective and visually degraded environments. These extensions are expected to further enhance robustness and support long-term deployment of service machines in diverse and complex indoor scenarios.