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

ROS API

Subscribed Topics

scan(sensor_msgs/LaserScan) - This topic should be created be your LIDAR sensor.

Published Topics

This node can publish a variety of topics but the final configuration depends on the user. By default the majority of the topics are disabled and they should be enabled through the launch file configuration.

datmo/marker_array (visualization_msgs/MarkerArray) - In this topic a variety of Rviz markers are published, which can facilitate in understanding the inner workings of the program.
datmo/box_kf (datmo/TrackArray) - In this topic the output of a Kalman Filter with a Constant Velocity model, which tracks the center of the box that surrounds the clusters is published.\

Note: In case that the marker_array topic is published from a robot and visualized in computer, which has a different version of ROS installed (kinetic, melodic, …), the msgs will not be published and the datmo node will crash with an md5sum error. To mitigate this, you should install on your robot the visualization msgs package of the ROS installation that runs on your computer.

Custom Messages

This package uses two custom msgs types datmo/Track and datmo/TrackArray to facilitate the publishing of its results. To my knowledge, at the time of developement, there was no standard ROS messages that accomplishes the same task.

The datmo/Track message has the following structure:
int32 id - object ID, so it is possible to differentiate between different objects during tracking
float32 length - Estimated length of the object
float32 width Estimated width of the object
nav_msgs/Odometry odom - Estimated pose of the object

The datmo/TrackArray message is an array that contains multiple datmo/Track messages, with the goal of efficient publishing.

Rviz markers

In case that the pub_markers flag is set to true, this package publishes visualization messages, which can be displayed in Rviz. The following messages are published:

closest_corner - The closest corner point of surrounding vehicles is visualized with a black rectangle.
bounding_box_center - The center of the bounding box is visualized with a yellow rectangle.
velocities - The velocities of the tracked objects are represented with an arrow.\

Parameters

• “lidar_frame” (“string”, default: “laser”) - Name of the transformation frame (frame_id) of the LaserScan msgs
• “world_frame” (“string”, default: “world”) - Name of the world coordinate frame, if it is not available, this value can be set equal to the lidar_frame
• “threshold_distance” (“double”, default: “0.2”) - This value sets the distance that is used by the clustering algorithm
• “euclidean_distance” (“double”, default: “0.25”) - This value sets the distance that is used by the euclidean distasnce data association algorithm
• “pub_markers” (“bool”, default: “false”) - publish of the the vizualization markers

References

[1] D. Kim, K. Jo, M. Lee, and M. Sunwoo, “L-shape model switching-based precise motion tracking of moving vehicles using laser scanners,” IEEE Transactions on Intelligent Transportation Systems, vol. 19, no. 2, pp. 598–612, 2018.
[2] X. Zhang, W. Xu, C. Dong, and J. M. Dolan, “Efficient l-shape fitting for vehicle detection using laser scanners,” in 2017 IEEE Intelligent Vehicles Symposium (IV), pp. 54–59, IEEE, 2017.

Changelog for package datmo

0.1.1 (24/5/2020) -----------* fixes message generation bug * removes unused rviz configurations * removes ukf filter tracking * organizes topic names under datmo namespace * adds tf2-geometry-msgs build dependency * changes default world frame from world to laser * adds experiment videos * merges presentation * adds corner switching * changes breakpoint image * adds clustering * removes data association video * adds association gif * adds flowcharts * deletes info stream * removes unnecessary file * changes covariance of ukf * removes old l_shape_tracker * moves two_robots bag file * removes new tag from recorded bag file * implements same orientation for kf and ukf * populates more ukf messages * publishes ukf tracking msgs * publishes length-width * tunes Q and R matrices * implements Mahalanobis distance for cornerswitches * deletes some clutter * starts Lshape tracker merge * implements ACT model * implements constant turn rate model * constant velocity model * removes poseCovariance visualization * refactors launch files * Merge branch 'corner_switching' * remoces unnecessary filters * simplifies findOrientation function * changes line length * separates dynamic and shape updates * same parameters kf ukf * improves length width estimation * fixes ukf Q matrix * fixes clustering bug * removes execution timing * Merge branch 'master' of github.com:kostaskonkk/datmo * adds overtake experiment * adds overtake bagfile * tries to improve rectanglefitting * moves ukf prediction in detectCornerPointSwitch * improves orientation estimation by KF * fixes velocity estimation bug * add experiment launch * adds expemeriment launch * adds launch for new experiment * finishes ukf l_shape_tracker * fixes orientatin bug * implements constant acceleration model * moves detectCornerPoint switch in lshapetracker * adds separate LShapeTracker object for UKF * adds launch files * better distance for data association * saves amount of tracked objects * add constant turn rate ukf * corrects orientations estimation * moves all rviz configs in datmo * adds box_kf * ukf works with newtonian model * refactoring of ukf filter class * adds example bag file * make filters remain between measurements * white background rviz * Adds some progress * Merges with refactored master * Fixes closest corner point bug * Moves tf to datmo class/ major refactoring * merge with master * Code refactoring and cleaning * Adds documentation * Adds arrow for theta box * Adds prototype covariance * Addition of UKF * Publishing of covariances * Adds buggy nonlinear observer * Refactoring and map_frame bug fix * Fixes transformation bug * Corrects the box_track_msg * Publishing of length and width * Timing of execution * changed coordinate frame of box_tracks * moved datmo constructor to source file * finished corner point switching * worked on shape switching * initial corner point switching * closeness criterion * fixed bounding box in rviz * fixed track.stamp bug * absolute velocities, arrows and new track msg * changed to PoseStamped in Track msg * publishing of /tracks topic * refactoring and initial bounding box * Trajectories and primitive classification * added configuration parameters xMerge branch 'master' of github.com:kostaskonkk/datmo * Addition of Kalman Filter and estimation of velocity * Update README.md * v1 * Initial commit * Contributors: Kostantinos Konstantinidis, Kostas, Kostas Flou, kostas