TRLO: An Efficient LiDAR Odometry With 3-D Dynamic Object Tracking and Removal

Yichong Jia, Ting Wang, Fengkui Cao, Xieyuanli Chen, Shiliang Shao, Lianqing LiuIEEE Trans. Instrumentation & Measurement 2025 · DOI

TL;DR

This paper introduces TRLO, a LiDAR odometry system that tackles the challenge of dynamic environments by effectively detecting and removing moving objects. By integrating a 3D object tracker and a novel bounding box consistency constraint, TRLO significantly improves localization accuracy and generates cleaner 3D maps compared to existing methods.

What Problem Does This Paper Address?

Autonomous robots, especially self-driving cars, rely heavily on Simultaneous Localization and Mapping (SLAM) to understand their surroundings and navigate. Traditional SLAM systems often assume a static environment, which breaks down in busy urban settings with moving vehicles and pedestrians. This static assumption leads to inaccurate robot localization and distorted maps, hindering reliable navigation. While some methods try to mitigate this, they often focus on general outlier rejection or require additional sensors, increasing cost and complexity. TRLO aims to provide a robust and accurate LiDAR-only odometry solution that specifically addresses the impact of dynamic objects.

Methods

TRLO's core innovation lies in its multi-stage approach to handling dynamic elements:

  • 3D Object Detection: The system first employs a deep learning-based method (PointPillars with Center-based detection) to identify dynamic and semi-static objects in the raw LiDAR point cloud, outputting 3D bounding boxes.
  • 3D Multi-Object Tracker: A novel tracker, built upon the Unscented Kalman Filter (UKF) and a nearest neighbor (NN) strategy, is used to reliably track these detected objects across frames. This tracker is designed to be more robust to non-linearities than traditional Extended Kalman Filters (EKF).
  • Dynamic Object Removal: Based on the tracker's output, dynamic objects are identified and removed from the point cloud, creating a "clean" static point cloud.
  • Efficient LiDAR Odometry: A fast, two-stage iterative closest point (ICP) solver (Fast GICP) is then applied to the cleaned static point clouds for scan-to-scan and scan-to-map transformations, enabling accurate state estimation.
  • Bounding Box Consistency Constraint: A key novel contribution is the use of the detected bounding boxes to impose a "posture consistency constraint." This leverages the assumption that objects rest on a flat ground plane to refine the robot's pose estimation, particularly its roll and pitch. This constraint is applied over a sliding window to ensure robustness.
  • Hash-Based Keyframe Management: To accelerate map registration, a hash-based keyframe database is implemented for efficient retrieval of relevant keyframes.

Datasets

The paper evaluates TRLO on two well-established datasets:

  • KITTI: A widely used dataset for autonomous driving research, providing various sequences with different levels of dynamic content.
  • UrbanLoco: A dataset specifically designed for urban localization challenges, often featuring more complex and dynamic scenarios.

Key Insights and Ideas

  • Dynamic Objects are a Major Bottleneck: The paper strongly emphasizes that ignoring dynamic objects is a significant limitation for current LiDAR odometry systems, leading to substantial errors in both localization and mapping.
  • UKF for Robust Tracking: Using the Unscented Kalman Filter (UKF) for object tracking proves more effective than EKF in handling the non-linearities inherent in dynamic object motion, leading to more accurate identification and removal.
  • Bounding Box Consistency is Powerful: The novel bounding box consistency constraint is a clever way to leverage semantic information (even without full semantic segmentation) to improve pose estimation, especially for the vertical (z-axis) and rotational components. This constraint helps "ground" the robot's pose.
  • Efficiency Through Specialization: By combining a fast deep learning detector, an efficient tracker, and a refined ICP, TRLO achieves a good balance between accuracy and real-time performance, outperforming many baselines.
  • LiDAR-Only Advantage: The system's ability to achieve competitive results using only LiDAR data is a significant advantage, reducing system complexity and cost compared to methods requiring IMUs or other sensors.

Key Results

  • Improved Localization Accuracy: TRLO consistently outperforms traditional LiDAR odometry methods and even some dynamic SLAM approaches on both KITTI and UrbanLoco datasets, showing lower Relative Pose Error (RPE) and Absolute Pose Error (APE). For instance, on UrbanLoco03, it achieves an APE of 1.23, outperforming several baselines.
  • Cleaner Maps: Qualitatively, TRLO generates significantly cleaner 3D maps with reduced "ghosting" artifacts caused by moving objects, as demonstrated in Figure 4.
  • Competitive Mapping Quality: Quantitatively, TRLO achieves high preserved rates (PR) of static points and competitive F1-scores for map quality.
  • Real-time Performance: The system can run at over 20Hz, meeting real-time operation requirements, with a total processing time of approximately 24.28 ms per scan.

Limitations and Future Work

  • Object Detection Focus: The authors acknowledge that they do not focus on further improving the object detection and tracking algorithms themselves, suggesting this as a potential area for future enhancement.
  • IMU Integration: While TRLO is LiDAR-only, the authors express interest in integrating IMU data in the future to further enhance robustness, especially for scenarios where LiDAR data might be sparse or noisy.
  • Loop Closure: The paper mentions that loop closure detection was disabled for some baselines during evaluation. Integrating loop closure into TRLO could further improve long-term localization accuracy and global map consistency.

Summary updated

Abstract

Simultaneous state estimation and mapping is an essential capability for mobile robots working in dynamic urban environment. The majority of existing SLAM solutions heavily rely on a primarily static assumption. However, due to the presence of moving vehicles and pedestrians, this assumption does not always hold, leading to localization accuracy decreased and maps distorted. To address this challenge, we propose TRLO, a dynamic LiDAR odometry that efficiently improves the accuracy of state estimation and generates a cleaner point cloud map. To efficiently detect dynamic objects in the surrounding environment, a deep learning-based method is applied, generating detection bounding boxes. We then design a 3D multi-object tracker based on Unscented Kalman Filter (UKF) and nearest neighbor (NN) strategy to reliably identify and remove dynamic objects. Subsequently, a fast two-stage iterative nearest point solver is employed to solve the state estimation using cleaned static point cloud. Note that a novel hash-based keyframe database management is proposed for fast access to search keyframes. Furthermore, all the detected object bounding boxes are leveraged to impose posture consistency constraint to further refine the final state estimation. Extensive evaluations and ablation studies conducted on the KITTI, UrbanLoco, and self-collected datasets demonstrate that our approach not only achieves more accurate state estimation but also generates cleaner maps, compared with baselines.
BibTeX
@article{jia2025trlo,
  author = {Yichong Jia and Ting Wang and Fengkui Cao and Xieyuanli Chen and Shiliang Shao and Lianqing Liu},
  title = {{TRLO: An Efficient LiDAR Odometry With 3-D Dynamic Object Tracking and Removal}},
  journal = {IEEE Transactions on Instrumentation and Measurement},
  year = {2025},
  doi = {10.1109/tim.2025.3561381},
}
Figure