People tracking and pose estimation using a 2D LIDAR
This project aims at implementing the 2D LiDAR people-tracking algorithm proposed in this paper by Zhenyu Gao et al. for multiple people. The original source code, which can be found here under the MIT license, implements a single-target frontal human following system using a 2D LIDAR. The goal of this project is to extend this implementation to track multiple people in the 2D LIDAR point cloud.
Note: This is still a work in progress and does not yet provide perfect/well-functioning results.
To achieve multi-person tracking, the given algorithm was separated into three main components:
- Leg Detection: This module provides coordinates of detected legs in the LiDAR data
- For simplicity, this uses the linear optimization approach as provided by Zhenyu Gao et al.
- TODO: This algorithm needs to be adapted to more complex environments, and the parameters adjusted to also fit leg detection further away from the sensor
- human_tracking_module: This module implements measurement-to-track association by matching detected legs to given targets. Additionally, it initiates new targets and destroys targets without measurements for 3 seconds
- this component is the main controller of the tracking algorithm
- TODO: this needs to be adapted for more complicated associations and delayed instanciation
- TargetTracking: this module encapsulates most of the pose estimation and tracking algorithm provided by Zhenyu Gao et al.
- During the update phase, it is fed with 0-2 leg measurements and performs the tracking as described in the paper
The algorithm was changed only partly to fit the multi person tracking scenario. Mainly the changes were made in the human_tracking_module to allow multiple targets and a different measurement to track association. The following changes were made:
- Ported the original code from ROS to ROS2
- Remove not needed elements like the frontal human following controller and dead-man's switch
- Separated the leg detection into its own module
- Separated target tracking into its own module
- Changed the measurement to track association to a nearest neighbor approach based on the distance between predicted target position and detected leg positions
- Changed interaction with RViz to allow multiple target visualization
- Introduce the TargetMeasurement class to encapsulate leg measurements
- Introduce the TargetState class to encapsulate the target state
- reduce use of
staticmembers to support multiple targets - Changed the launch system to ROS2 launch files
Current problems originate from three sources:
The current measurement to track association is very simple and only matches the nearest detected leg to the predicted target position. This often leads to wrong associations when multiple people are close to each other or cross paths. A better association method needs to be investigated. Possible approaches are:
- Track-before-detect methods, which do not immediately initiate a track on a single measurement but wait for multiple measurements before initiating a new target
- Joint probabilistic data association (JPDA) methods
- Multiple hypothesis tracking (MHT)
- Global nearest neighbor (GNN) with auction algorithms
This would help reduce the error introduced at the moment by wrong associations and multiple targets tracking the same person.
The current leg detection algorithm is directly taken from the original implementation. It needs to be investigated how well this performs in a free environment with multiple people. Additionally, the parameters need to be adjusted to allow leg detection further away from the sensor. Possible improvements are:
- Adjust the parameters of the current leg detection algorithm
- Switch to a different leg detection algorithm that performs better in complex environments
- e.g., machine learning-based approaches
- This should be fairly easy to integrate since the leg detection is encapsulated in its own module
- Introduce a tracking method for the leg detection to reduce false positives and improve detection rate
The original algorithm assumed that the user - the person tracked - is facing the robot within close range during track instantiation. In a free multi-person environment, this often does not apply. The frontal-facing constraint can be neglected by assuming every person faces the robot when the track is initiated. This, however, leads to a number of problems. We no longer can determine whether the person is walking forward or backward and left or right. However, what we can do is to determine a velocity direction and thusly still gain insight into the people's movements. Currently, the implementation does not take this into account and only determines the change of the user's sagittal plane.
Additionally, the parameters in the algorithm were not yet adjusted for the different situations in a free environment. The original algorithm assumes a small distance to the tracked person, which implies a certain parameters for the orientation estimation. It needs to be investigated how these need to be adjusted to track the pose for further away targets. Alternatively, a distance threshold should be applied.
The current implementation was tested using multiple rosbag files captured using a 2D LIDAR in different scenarios. The results are not yet perfect but show that the approach can be feasible. The main problems arise from wrong measurement to track associations and the leg detection performance. It needs to be investigated how to improve these parts of the algorithm to achieve better results.
As of now, the algorithm fails to track multiple people for longer periods of time and instantiates multiple tracks for the same person. This could be caused by wrong and very noisy leg detections as well as the simple measurement to track association.
- One person walking left to right and turning
- Two people walking left to right and turning
- Two people crossing each other -> walking together
- one person standing still, one person walking around
- Turning, moving side to side, moving away and towards the LIDAR
- random movement in the fov of the LIDAR
Build the tools:
colcon build --parallel-workers 10 --packages-select lidar-people-trackingRun using the launch files:
source install/setup.bash && ros2 launch lidar-people-tracking lidar_people_tracking_launch.py
Sometimes RViz complains about missing tf transforms since we do rely on static odometry for simplicity. To fix this, set the global tf transform to odom.
For testing, we used ROSbag files, which were captured using ROS. In order to use them in ROS2, they need to be converted using a tool like rosbag-convert:
rosbag-convert src.bag --dst destinationThey then can be run via the play command from the parent folder:
source install/setup.bash # or source opt/ros/jazzy/setup.bash
ros2 bag play destination- Determine a velocity direction and not only the facing direction of the target user
- Improve measurement to track by investigating track-before-detect methods or switching to a measurement to track association that does not immediately initiate a track but waits for multiple measurements.
- Check if a Bernoulli particle filter to track each target and model non-linear movement is better.
- Pose-aware filtering -> Use the pose estimation to improve tracking by using interacting multiple models (IMM) filter per current pose (e.g. standing, turning, walking)
- this needs further investigation
- Switch to a dynamic robot system -> include robot movement in the tracking
Contributions are very much welcome! Unfortunately, I do not have a lot of time to actively work on this project and see if I can make this work. If you find this approach interesting, feel free to contribute and contact me.
Thank you to the original authors for providing the open-source code and the paper. This project directly builds upon their work and merely attempts to apply it to multiple people tracking.
Gao, Z., Wang, Z., Saint-Bauzel, L. et al. Large Workspace Frontal Human Following for Mobile Robots Utilizing 2D LiDAR. J Intell Robot Syst 111, 25 (2025). https://doi.org/10.1007/s10846-025-02225-4
The project uses nlopt for leg detection: https://nlopt.readthedocs.io/en/latest/