Detection and Tracking of Moving Objects with 2D LIDAR

This package aims to provide Detection and Tracking of Moving Objects capabilities to robotic platforms that are equipped with a 2D LIDAR sensor and publish ‘sensor_msgs/LaseScan’ ROS messages. Such a scenario would be the one visualized below, in which the black scaled car is equipped with a LIDAR sensor and it needs to track the motion of the red vehicle through the LIDAR measurements.

The output of this package is visualized below and it can be observed that it estimates position, velocity, orientation and dimensions of the red vehicle.

It specializes in tracking of rectangle shaped objects and therefore it is most used in vehicle tracking. The workflow of this package is inspired by the one presented in Kim et al., 2018 [1].

Overview

Below you can read a synopsis of its funcion and operation. A more in depth explanation of this package’s inner workings is given in this paper.

Detection

The detection part of the system is visualized in the following flowchart:

Clustering

In the clustering step the raw LIDAR measurements are divided to groups/clusters. In this way, the different objects in the environment are differentiatted. A simple way to do this is by separating clusters, based on the inbetween euclidean distance of LIDAR measurements. Therefore, if the distance of two consequtive LIDAR measurements is greater than a predefined threshold distance the two points are divided in two separate clusters. However, since LIDAR measurements become less dense as the distance from the sensor increases, objects For this reason, the threshold distance should be adapted in a way that it increses in relation with the range distance. In this system, this is achieved by using the Adaptive Breakpoint Detector algorithm. Its operation is visualized in the right side of the above figure and the equation that it uses is given below it.

Rectangle Fitting and L-shape extraction

In this step, rectangles are fitted onto the extracted clusters are fitted with rectangles. This is done to increase the tracking accuracy and shape estimation of rectangular objects. The algorithm used for rectangle fitting is the Search-Based Rectangle Fitting algorithm developed by Zhang et al., 2017 [2]. After rectangle fitting, L-shapes are extracted from all the rectangles. L-shapes represent the corner of the rectangle closer to the sensor and its two adjacent sides. Therefore, every L-shape contains five measurements, the position of the corner point, the orientation of the rectangle (theta) and the length of its sides (L1, L2).

Tracking

The tracking part of the system is visualized in the following flowchart:

Data Association

The clusters are tracked between time frames by a Nearest Neighbour data association scheme, with a Euclidean distance criterion.

Apperance Change Detector

In cases that the closest corner point of a tracked vehicle changes between measurements, this is detected by comparing the Mahalanobis distance of the four corner points of the vehicle with that of the new L-shape.

Kinematic and Shape Trackers

The motion of the detected vehicles is tracked based on two kinematic trackers. A Kalman Filter with a Constant Velocity Model and an Unscented Kalman Filter with a Coordinated-Turn model.

The shape and orientation of the detected vehicles are tracked by a Kalman Filter that contains two models. The first model is a Constant Shape model and it indicates that the shape of the detected vehicle remains constant. The second model is a Constant Turn Rate model that indicates that the turn rate of the detected vehicle remains constant, while its orientation depends on the turn rate.

Below you can find a video of a presentation of mine, in which I explain some early features of this package.

Installation and use

This ROS package can be installed in the following way:

1. First you should navigate to the source folder of your catkin_ws. For example cd ~/catkin_ws/src.
2. Run

git clone git@github.com:kostaskonkk/datmo.git
cd ..
catkin_make

The datmo package should be now installed to your computer and you will be able to use it after sourcing your workspace.

source devel/setup.bash

You can run a demonstration of the DATMO package by running:

roslaunch datmo example.launch bag:=overtakes

You can run it by typing:

roslaunch datmo datmo.launch

File truncated at 100 lines see the full file