Dynamic Direct LiDAR Odometry: Dynamic LiDAR Odometry for Mobile Robots in Urban Search and Rescue Environments

[ArXiv]

This repository contains the source code to the IROS paper Dynamic LiDAR Odometry for Mobile Robots in Urban Search and Rescue Environments. It extends the Direct LiDAR Odometry framework to detect dynamic objects in the LiDAR scans, track them individually and remove them from the map.

Instructions

As input, structured point clouds are required (sensor_msgs::PointCloud2). We recommend to use a LiDAR scanner with at least 64 scan lines. IMU data can be used optionally for initial ground alignment.

System

We tested our system on Ubuntu 20.04 with ROS Noetic.

Compiling

Clone the repository into the src folder of your catkin workspace and build it with catkin.

git clone https://github.com/tu-darmstadt-ros-pkg/dynamic_direct_lidar_odometry.git
catkin build

Datasets

You can find the kantplatz dataset presented in the paper here.

The small_town_simulation sequence is part of the DOALS dataset.

Running

Run a launch file:

• ddlo.launch Launches DDLO, the trajectory server node, rosbag node and Rviz. Default playback and config for the kantplatz dataset.
• DOALS.launch Same as above, default playback and config for the DOALS dataset.

The datasets can also be started individually by the respective launch files. All algorithm-related parameters are set in the cfg/*.yaml files. You will have to play around with the parameters to find out what fits best to your own data.

Services

The following services are available:

• save_pcd: Saves the map into a .pcd file

  rosservice call /ddlo/save_pcd LEAF_SIZE SAVE_PATH

• save_trajectories: Saves all recorded object trajectories with ID in the format x y z stamp.sec stamp.nsec to a text file

  rosservice call /ddlo/save_trajectories SAVE_PATH

• clear_trajectories: Resets all trajectories from the trajectory_server node. This can also be used to clear the Rviz display.

  rosservice call /ddlo/clear_trajectories

Citation

If you find this work useful, please cite:

@article{lichtenfeld2024dynamic,
    author={},
    TODO...
}

Acknowledgments

• DLO - Kenny Chen, Brett T. Lopez, Ali-akbar Agha-mohammadi, and Ankur Mehta, “Direct LiDAR Odometry: Fast Localization With Dense Point Clouds,” in IEEE Robotics and Automation Letters, 2022.

• FastGICP - Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration,” in IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2021, pp. 11 054–11 059.

• NanoFLANN - Jose Luis Blanco and Pranjal Kumar Rai, “NanoFLANN: a C++ Header-Only Fork of FLANN, A Library for Nearest Neighbor (NN) with KD-Trees,” 2014.

• LeGO-LOAM: Implementation of range image segmentation - Shan et al., "LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain", in IROS 2018

• Kalman Filter Implementation - Hayk Martiros

• Hungarian Algorithm Implementation