Grid-Based Tracking in Urban Environments Using Multiple Lidars

This example uses:

This example shows how to track moving objects with multiple lidars using a grid-based tracker. A grid-based tracker enables early fusion of data from high-resolution sensors such as radars and lidars to create a global object list.

Introduction

Most multi-object tracking approaches represent the environment as a set of discrete and unknown number of objects. The job of the tracker is to estimate the number of objects and their corresponding states, such as position, velocity, and dimensions, using the sensor measurements. With high-resolution sensors such as radar or lidar, the tracking algorithm can be configured using point-object trackers or extended object trackers.

Point-Object Trackers

Point-object trackers assume that each object may give rise to at most one detection per sensor. Therefore, when using point-target trackers for tracking extended objects, features like bounding box detections are first extracted from the sensor measurements at the object-level. These object-level features then get fused with object-level hypothesis from the tracker. A poor object-level extraction algorithm at the sensor level (such as imperfect clustering) thus greatly impacts the performance of the tracker. For an example of this workflow, refer to Track Vehicles Using Lidar: From Point Cloud to Track List (Automated Driving Toolbox).

Extended Object Trackers

On the other hand, extended object trackers process the detections without extracting object-level hypothesis at the sensor level. Extended object trackers associate sensor measurements directly with the object-level hypothesis maintained by tracker. To do this, a class of algorithms typically requires complex measurement models of the object extents specific to each sensor modality. For example, refer to Extended Object Tracking with Lidar for Airport Ground Surveillance and Extended Object Tracking of Highway Vehicles with Radar and Camera (Automated Driving Toolbox) to learn how to configure a multi-object PHD tracker for lidar and radar respectively.

A grid-based tracker can be considered as a type of extended object tracking algorithm which uses a dynamic occupancy grid map as an intermediate representation of the environment. In a dynamic occupancy grid map, the environment is discretized using a set of 2-D grid cells. The dynamic map represents the occupancy as well as kinematics of the space represented by a grid cell. Using the dynamic map estimate and further classification of cells as static and dynamic serves as a preprocessing step to filter out measurements from static objects and to reduce the computational complexity.

In this example, you use the trackerGridRFS System object™ to configure the grid-based tracker. This tracker uses the Random Finite Set (RFS) formulation with Dempster-Shafer approximation [1] to estimate the dynamic map. Further, it uses a nearest neighbor cell-to-track association [2] scheme to track dynamic objects in the scene. To initialize new tracks, the tracker uses the DBSCAN algorithm to cluster unassigned dynamic grid cells.

Set Up Scenario and Lidar Sensor Models

The scenario used in this example was created using the Driving Scenario Designer (Automated Driving Toolbox) app and was exported to a MATLAB® function. This MATLAB function was wrapped as a helper function helperCreateMultiLidarDrivingScenario. The scenario represents an urban intersection scene and contains a variety of objects that include pedestrians, bicyclists, cars, and trucks.

The ego vehicle is equipped with 6 homogeneous lidars, each with a horizontal field of view of 90 degrees and a vertical field of view of 40 degrees. The lidars are simulated using the lidarPointCloudGenerator (Automated Driving Toolbox) System object. Each lidar has 32 elevation channels and has a resolution of 0.16 degrees in azimuth. Under this configuration, each lidar sensor outputs approximately 18,000 points per scan. The configuration of each sensor is shown here.

% For reproducible results
rng(2020);

% Create scenario
[scenario, egoVehicle, lidars] = helperCreateMultiLidarDrivingScenario;

The scenario and the data from the different lidars can be visualized in the animation below. For brevity and to make the example easier to visualize, the lidar is configured to not return point cloud from the ground by specifying the HasRoadsInputPort property as false. When using real data or if using simulated data from roads, the returns from ground and other environment must be removed using point cloud preprocessing. For more information, refer to the Ground Plane and Obstacle Detection Using Lidar (Automated Driving Toolbox) example.

Set Up Grid-Based Tracker

You define a grid-based tracker using trackerGridRFS to track dynamic objects in the scene. The first step of defining the tracker is setting up sensor configurations as trackingSensorConfiguration objects. The sensor configurations allow you to specify the mounting of each sensor with respect to the tracking coordinate frame. The sensor configurations also allow you to specify the detection limits - field of view and maximum range - of each sensor. In this example, you use the properties of the simulated lidar sensors to define these properties.

The utility function helperGetLidarConfig uses the simulated lidar sensor model and returns its respective configuration. In this example, the targets are tracked in the global or world coordinate system by using the simulated pose of the vehicle. This information is typically obtained via an inertial navigation system. As the sensors move in the scenario system, their configuration must be updated each time by specifying the configurations as an input to the tracker.

% Store configurations of all sensor
sensorConfigs = cell(numel(lidars),1);

% Fill in sensor configurations
for i = 1:numel(sensorConfigs)
    sensorConfigs{i} = helperGetLidarConfig(lidars{i},egoVehicle);
end

% Create tracker. You can define the properties before using the tracker.
tracker = trackerGridRFS(SensorConfigurations = sensorConfigs,...
     HasSensorConfigurationsInput = true);

The tracker uses a two-dimensional grid for the intermediate representation of the environment. The grid is defined by 3 attributes: its length, its width, and the resolution. The length and width describe the span of the grid in local X and local Y direction of the ego vehicle respectively. The resolution defines the number of cells per meter of the grid. In this example, you use a 120 m by 120 m grid with 2 cells per meter.

tracker.GridLength = 120; % meters
tracker.GridWidth = 120; % meters
tracker.GridResolution = 2; % 1/meters

In addition to defining the grid, you also define the relative position of the ego vehicle by specifying the origin of the grid (left corner) with respect to the origin of the ego vehicle. In this example, the ego vehicle is located at the center of the grid.

tracker.GridOriginInLocal = [-tracker.GridLength/2 -tracker.GridWidth/2];

The tracker uses particle-based methods to estimate the state of each grid cell and further classify them as dynamic or static. It uses a fixed number of persistent particles on the grid which defines the distribution of existing targets. It also uses a fixed number of particles to sample the distribution for newborn targets. These birth particles get sampled in different grid cells based on the probability of birth. Further, the velocity and other unknown states like turn-rate and acceleration (applicable when MotionModel of the tracker is not constant-velocity) of the particles is sampled uniformly using prior information supplied using prior limits. A resampling step assures that the number of particles on the grid remain constant.

tracker.NumParticles = 1e5; % Number of persistent particles
tracker.NumBirthParticles = 2e4; % Number of birth particles
tracker.VelocityLimits = [-15 15;-15 15]; % To sample velocity of birth particles (m/s)
tracker.BirthProbability = 0.025; % Probability of birth in each grid cell
tracker.ProcessNoise = 5*eye(2); % Process noise of particles for prediction as variance of [ax;ay] (m/s^2)

The tracker uses the Dempster-Shafer approach to define the occupancy of each cell. The dynamic grid estimates the belief mass for occupancy and free state of the grid. During prediction, the occupancy belief mass of the grid cell updates due to prediction of the particle distribution. The DeathRate controls the probability of survival ( $P_{s}$ ) of particles and results in a decay of occupancy belief mass during prediction. As the free belief mass is not linked to the particles, the free belief mass decays using a pre-specified, constant discount factor. This discount factor specifies the probability that free regions remain free during prediction.

tracker.DeathRate = 1e-3; % Per unit time. Translates to Ps = 0.9999 for 10 Hz
tracker.FreeSpaceDiscountFactor = 1e-2; % Per unit time. Translates to a discount factor of 0.63 (1e-2^dT) for 10 Hz

After estimation of state of each grid cell, the tracker classifies each grid cell as static or dynamic by using its estimated velocity and associated uncertainty. Further, the tracker uses dynamic cells to extract object-level hypothesis using the following technique:

Each dynamic grid cell is considered for assignment with existing tracks. A dynamic grid cell is assigned to its nearest track if the negative log-likelihood between a grid cell and a track falls below an assignment threshold. A dynamic grid cell outside the assignment threshold is considered unassigned. The tracker uses unassigned grid cells at each step to initiate new tracks. Because multiple unassigned grid cells can belong to the same object track, a DBSCAN clustering algorithm is used to assist in this step. Because there are false positives while classifying the cells as static or dynamic, the tracker filters those false alarms in two ways. First, only unassigned cells which form clusters with more than a specified number of points (MinNumPointsPerCluster) can create new tracks. Second, each track is initialized as a tentative track first and is only confirmed if it is detected M out of N times.

tracker.AssignmentThreshold = 8; % Maximum distance or negative log-likelihood between cell and track
tracker.MinNumCellsPerCluster = 6; % Minimum number of grid cells per cluster for creating new tracks
tracker.ClusteringThreshold = 1; % Minimum Euclidean distance between two cells for clustering
tracker.ConfirmationThreshold = [3 4]; % Threshold to confirm tracks
tracker.DeletionThreshold = [4 4]; % Threshold to delete confirmed tracks

You can also accelerate simulation by performing the dynamic map estimation on GPU by specifying the UseGPU property of the tracker.

tracker.UseGPU = false;

Visualization

The visualization used for this example is defined using a helper class, helperGridTrackingDisplay, attached with this example. The visualization contains three parts.

Ground truth - Front View: This panel shows the front-view of the ground truth using a chase plot from the ego vehicle. To emphasize dynamic actors in the scene, the static objects are shown in gray.
Lidar Views: These panels show the point cloud returns from each sensor.
Grid-based tracker: This panel shows the grid-based tracker outputs. The tracks are shown as boxes, each annotated by their identity. The tracks are overlaid on the dynamic grid map. The colors of the dynamic grid cells are defined according to the color wheel, which represents the direction of motion in the scenario frame. The static grid cells are represented using a grayscale according to their occupancy. The degree of grayness denotes the probability of the space occupied by the grid cell as free. The positions of the tracks are shown in the ego vehicle coordinate system, while the velocity vector corresponds to the velocity of the track in the scenario frame.

display = helperGridTrackingDisplay;

Run Scenario and Track Dynamic Objects

Next, run the scenario, simulate lidar sensor data from each lidar sensor, and process the data using the grid-based tracker.

% Initialize pointCloud outputs from each sensor
ptClouds = cell(numel(lidars),1);
sensorConfigs = cell(numel(lidars),1);

while advance(scenario)
    % Current simulation time
    time = scenario.SimulationTime;
    
    % Poses of objects with respect to ego vehicle
    tgtPoses = targetPoses(egoVehicle);
    
    % Simulate point cloud from each sensor
    for i = 1:numel(lidars)
        [ptClouds{i}, isValidTime] = step(lidars{i},tgtPoses,time);
        sensorConfigs{i} = helperGetLidarConfig(lidars{i},egoVehicle);
    end
    
    % Pack point clouds as sensor data format required by the tracker
    sensorData = packAsSensorData(ptClouds,sensorConfigs,time);
    
    % Call the tracker
    tracks = tracker(sensorData,sensorConfigs,time);
    
    % Update the display
    display(scenario, egoVehicle, lidars, ptClouds, tracker, tracks);
    drawnow;
end

Results

Next, analyze the performance of the tracker using the visualization used in this example.

The grid-based tracker uses the dynamic cells from the estimated grid map to extract object tracks. The animation below shows the results of the tracker in this scenario. The "Grid-based tracker" panel shows the estimated dynamic map as well as the estimated tracks of the objects. It also shows the configuration of the sensors mounted on the ego vehicle as blue circular sectors. Notice that the area encapsulated by these sensors is estimated as "gray" in the dynamic map, representing that this area is not observed by any of the sensors. This patch also serves as an indication of ego-vehicle's position on the dynamic grid.

Notice that the tracks are extracted only from the dynamic grid cells and hence the tracker is able to filter out static objects. Also notice that after a vehicle enters the grid region, its track establishment takes few time steps. This is due to two main reasons. First, there is an establishment delay in classification of the cell as dynamic. Second, the confirmation threshold for the object takes some steps to establish a track as a confirmed object.

Next, you look at the history of a few tracks to understand how the state of a track gets affected by the estimation of the dynamic grid.

Longitudinally Moving Tracks

The following snapshots show the history for the track denoted by T1. The T1 track represents the yellow car that passes the ego vehicle on the left during the first few seconds of the simulation. Notice that the grid cells occupied by this track are colored in red, indicating their motion in the positive X direction. The track obtains the track's velocity and heading information using the velocity distribution of the assigned grid cells. It also obtains its length, width, and orientation using the spatial distribution of the assigned grid cells. The default TrackUpdateFcn of the trackerGridRFS extracts new length, width, and orientation information from the spatial distribution of associated grid cells at every step. This effect can be seen in the snapshots below, where the length and width of the track adjusts according to the bounding box of the associated grid cells. An additional filtering scheme can be added using the predicted length, width, and orientation of the track by using a custom TrackUpdateFcn.

% Show snapshots for TrackID = 1. Also shows close tracks like T3 and T4
% representing car and truck moving in the opposite direction. 
showSnapshots(display.GridView,1);

showSnapshots(display.GridView,4);

Next, take a closer look at the history of T4. The T4 track represents the truck moving in the opposite direction of the ego vehicle. Notice that the grid cells representing this track are colored in blue, representing the estimated motion direction of the grid cell. Also, notice that there are grid cells in the track that are misclassified by the tracker as static (white color). These misclassified grid cells often occur when sensors report previously occluded regions of an object, because the tracker has an establishment delay to classify these cells property.

Notice that at time = 4, when the truck and the vehicle came close to each other, the grid cells maintained their respective color, representing a stark difference between their estimated velocity directions. This also results in the correct data association between grid cells and predicted tracks of T1 and T4, which helps the tracker to resolve them as separate objects.

Laterally Moving Tracks

The following snapshots represent the track denoted by T7. This track represents the vehicle moving in the lateral direction, when the ego vehicle stops at the intersection. Notice that the grid cells of this track are colored in purple, representing the direction of motion in negative Y direction. Similar to other tracks, the track maintains its length and width using the spatial distribution of the assigned grid cells.

showSnapshots(display.GridView,7);

Tracks Changing Direction

In this example, you used a "constant-velocity" model with the tracker. This motion model assumes that the targets move at a constant velocity, meaning constant speed and direction. However, in urban scenes, this assumption is usually not accurate. To compensate for the unknown acceleration of the objects, a process noise is specified on the tracker. The following snapshots show the history of track T2. This track represents the vehicle directly in front of the ego vehicle. Notice in the ground truth that this vehicle turns right at the intersection.

showSnapshots(display.GridView, 2);

Notice that the color of the grid cells associated with this track changes from red to purple. Also, the transition of colors results in a few misclassified cells, which can result in a poor estimate of length and width of the vehicle. The ability of the tracker to maintain the track on this vehicle is due to a coupled effect of three main reasons. First, the tracker allows to specify an assignment threshold. Even if the predicted track does not align with the dynamic grid cells, it can associate with them up to a certain threshold. Second, to create a new track from grid cells that remain outside the threshold requires meeting the minimum number of cells criteria. Third, the tracker has a deletion threshold, which allows a track to be coasted for a few steps before deleting it. If the classification of grid cells is very poor during the turn, the track can survive a few steps and can get re-associated with the grid cells. Note that misclassified grid cells are far more observable with Track T8, as shown below in its history. The T8 track represents the light blue car traveling in the positive Y direction before taking at right turn at the intersection. This vehicle was partially occluded before the turn and had another closely traveling vehicle while making the turn.

showSnapshots(display.GridView,8);

Summary

In this example, you learned the basics of a grid-based tracker and how it can be used to track dynamic objects in a complex urban driving environment. You also learned how to configure the tracker to track an object using point clouds from multiple lidar sensors.

Supporting Functions

function sensorData = packAsSensorData(ptCloud, configs, time)
%The lidar simulation returns output as pointCloud object. The Location
%property of the point cloud is used to extract x,y and z locations of
%returns and pack them as structure with information required by a tracker.

sensorData = struct(SensorIndex = {},...
     Time = {},...
     Measurement = {},...
     MeasurementParameters = {});

for i = 1:numel(ptCloud)
    % This sensor's cloud
    thisPtCloud = ptCloud{i};
    
    % Allows mapping between data and configurations without forcing an
    % ordered input and requiring configuration input for static sensors.
    sensorData(i).SensorIndex = configs{i}.SensorIndex;
    
    % Current time
    sensorData(i).Time = time;
    
    % Measurement as 3-by-N defining locations of points
    sensorData(i).Measurement = reshape(thisPtCloud.Location,[],3)';
    
    % Data is reported in sensor coordinate frame and hence measurement
    % parameters are same as sensor transform parameters.
    sensorData(i).MeasurementParameters = configs{i}.SensorTransformParameters;
    sensorData(i).MeasurementParameters(1).Frame = fusionCoordinateFrameType(1);
end

end

function config = helperGetLidarConfig(lidar, egoVehicle)
platPose = struct(Position = egoVehicle.Position(:), Yaw = egoVehicle.Yaw, Pitch = ...
    egoVehicle.Pitch, Roll = egoVehicle.Roll, Velocity = egoVehicle.Velocity);
config = trackingSensorConfiguration(lidar,platPose, IsValidTime = true);
config.DetectionProbability = 0.95;
end

References

[1] Nuss, Dominik, et al. "A random finite set approach for dynamic occupancy grid maps with real-time application." The International Journal of Robotics Research 37.8 (2018): 841-866.

[2] Steyer, Sascha, Georg Tanzmeister, and Dirk Wollherr. "Object tracking based on evidential dynamic occupancy grids in urban environments." 2017 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2017.