Grid-Based Tracking in Urban Environments Using Multiple Radars

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 then 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 in a single scan. 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.

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 of Highway Vehicles with Radar and Camera (Automated Driving Toolbox) to learn how to configure a multi-object PHD tracker for radar sensors.

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 estimate of the dynamic map and further classification of grid cells as static and dynamic serves as a preprocessing step to filter out measurements from static objects and to reduce the computational complexity for object extraction. For trackers which represent the environment at the object-level such as the PHD tracker, returns from environments much be filtered out before processing them with the tracker. For an example showing such radar pre-processing, refer to Highway Vehicle Tracking with Multipath Radar Reflections (Sensor Fusion and Tracking Toolbox) example.

In this example, you use the trackerGridRFS (Sensor Fusion and Tracking Toolbox) 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 Radar 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 extended to include radar sensors was wrapped as a helper function helperCreateGridRadarDrivingScenario. 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 radars, each with a horizontal field of view of 90 degrees, a vertical field of view of 40 degrees, and a maximum range of 100 meters. The radars are simulated using the radarDataGenerator System object. Each radar has an azimuth resolution of 2 degrees, an elevation resolution of 10 degrees, and a range resolution of 2.5 meters. To simulate a different configuration of the radar, you can change the properties of the radar model in the helper function helperCreateGridRadarDrivingScenario.

% For reproducible results
rng(2);

% Create scenario
[scenario, egoVehicle, radars] = helperCreateGridBasedRadarDrivingScenario;

The data set obtained by sensor simulation is recorded in a MAT file that contains returns from the radar. To record the data for a different scenario or sensor configuration, you can use the following command:

helperRecordGridBasedDrivingScenario(scenario, egoVehicle, sensors, fName);

% Load the recorded data
load('MultiRadarGridBasedTrackingData.mat','radarPointCloudLog','egoPoseLog','radarSpecification');

Visualization

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

display = helperRadarGridTrackingDisplay;

The scenario and the data from the different radars overlaid on ground truth can be visualized in the animation below.

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 specifications of the radar sensors to define these properties. An example of radar specification is shown below. You can configure the tracker with recorded data by modifying the specifications of the sensors. The packData supporting function converts the point cloud into the input format required by the tracker. The trackerGridRFS does not consume doppler measurements and the packData function filters out the doppler measurements before sending the point cloud to the tracker.

disp(radarSpecification{1});

             SensorIndex: 1
        MountingLocation: [3.7000 0 0.6000]
          MountingAngles: [0 0 0]
             FieldOfView: [90 40]
             RangeLimits: [0 100]
         RangeRateLimits: [-100 100]
    DetectionProbability: 0.9500

The utility function helperGetRadarConfig uses the specification of the radar sensor and returns its respective configuration in a format required by the tracker. 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 coordinate 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(radarSpecification),1);

% Fill in sensor configurations
for i = 1:numel(sensorConfigs)
    sensorConfigs{i} = helperGetRadarConfig(radarSpecification{i},egoPoseLog{1});
end

% Construct tracker using sensor configurations. You can define the rest of
% 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 a resolution of 1.5 cells per meter.

tracker.GridLength = 120; % meters
tracker.GridWidth = 120; % meters
tracker.GridResolution = 1.5; % 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 because the sensors provide surround coverage.

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 ensures that the number of particles on the grid remain constant.

tracker.NumParticles = 2e5; % 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 (${\mathit{P}}_{\mathit{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, it 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 estimating the 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 is 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. The dynamic map estimate can contain false positives in static and dynamic classification and tracker filters them 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 = 9; % Maximum distance or negative log-likelihood between cell and track
tracker.MinNumCellsPerCluster = 3; % Minimum number of grid cells per cluster for creating new tracks
tracker.ClusteringThreshold = 4; % Minimum Euclidean distance between two cells for clustering
tracker.ConfirmationThreshold = [4 5]; % 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;

Run Scenario and Track Dynamic Objects

Next, run the scenario, assemble recorded radar sensor data, and process the data using the grid-based tracker.

% Number of time steps
numSteps = numel(radarPointCloudLog);

% Loop over time steps
for i = 1:numSteps
    % Advance the scenario
    advance(scenario);

    % Current simulation time
    time = scenario.SimulationTime;
    
    % Pack point clouds and sensor configurations using recorded
    [sensorData, sensorConfigs] = packData(radarPointCloudLog{i},egoPoseLog{i},radarSpecification);
    
    % Call the tracker
    tracks = tracker(sensorData,sensorConfigs,time);
    
    % Update the display
    display(scenario, egoVehicle, radarSpecification, radarPointCloudLog{i}, tracker, tracks);
    drawnow;
end

Results

Next, analyze the performance of the tracker using the visualization provided 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 a 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 track T1 represents the yellow car that passes the ego vehicle on the left during the first few seconds of the simulation. Note that the grid cells occupied by this track are colored in red, indicating their motion in the positive X direction. The track acquires its velocity and heading information using the velocity distribution of the assigned grid cells. Similarly, 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 observed in the snapshots below, where the length and width of the track adjusts according to the bounding box of the associated grid cells. This results in shrinkage of box as it travels away from the ego vehicle, but it can also create fluctuating extent estimates if the dynamic grid cells are not classified accurately. 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.
showSnapshots(display.GridView,1);

Next, take a closer look at the history of T6. The track T6 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. Also, because the uncertainty of the velocity estimate at the grid increases when the object is occluded, the probability of the grid cells to be classified as dynamic reduces.

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 T6, which helps the tracker to resolve them as separate objects.

showSnapshots(display.GridView,6);

Laterally Moving Tracks

The following snapshots represent the track denoted by T20. 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 from left to right (in negative Y direction). Similar to other tracks, the track acquires its length and width estimate using the spatial distribution of the assigned grid cells.

showSnapshots(display.GridView,20);

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. Given the sparse resolution of the point cloud with noisy observations, as shown in this example, the process noise also helps to compensate for the measurement uncertainty when the measurement from the same true position jumps from one grid cell to another due to noise. The sparse point cloud and the measurement noise also impacts the tracker's ability to detect slowly moving objects as dynamic. Notice in the images below that the tracker struggles to maintain the track T18 on light blue car traveling in the positive Y direction before taking a right turn at the intersection. This is because its grid cells were misclassified as static. Due to coasting (DeletionThreshold) of the tracks and allowed threshold between tracks and cells (AssignmentThreshold), the tracker was able to maintain the track on this vehicle.

showSnapshots(display.GridView, 18)

However, notice in the images below that the tracker drops the track T3 on the car traveling in front of the ego in the positive X direction before taking a right turn at the intersection. After grid cells from the car were correctly classified as dynamic again, the tracker established a new track T15 on the car.

showSnapshots(display.GridView, [3 15]);

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 objects using sparse point clouds from multiple radar sensors.

Supporting Functions

function [sensorData, sensorConfigs] = packData(radarPtClouds, egoPose, radarSpecifications)
% Initialize arrays
sensorData = struct(SensorIndex = {},...
     Time = {},...
     Measurement = {},...
     MeasurementParameters = {});

sensorConfigs = cell(numel(radarPtClouds),1);

% Populate arrays
for i  = 1:numel(radarPtClouds)   
    % Get radar configuration as trackingSensorConfiguration object
    sensorConfigs{i} = helperGetRadarConfig(radarSpecifications{i}, egoPose);

    % Report time for sensorData. 
    sensorData(i).Time = radarPtClouds{i}.Time;

    % Allows mapping between sensor data and configuration without forcing
    % an ordered input
    sensorData(i).SensorIndex = sensorConfigs{i}.SensorIndex;
    
    % Assemble measurements. trackerGridRFS doesn't consume doppler
    % information yet.
    sensorData(i).Measurement = [radarPtClouds{i}.Azimuth;radarPtClouds{i}.Elevation;radarPtClouds{i}.Range];
    
    % Data is reported in sensor coordinate frame in spherical coordinates.
    % MeasurementParameters are same as sensor configuration transform
    % parameters
    sensorData(i).MeasurementParameters = sensorConfigs{i}.SensorTransformParameters;
end

end

function config = helperGetRadarConfig(radarSpecification, egoPose)
% Create configuration object
config = trackingSensorConfiguration(SensorIndex = radarSpecification.SensorIndex, ...
    IsValidTime = true);

% Provide detection limits (FOV of the sensors)
config.SensorLimits(1,:) = radarSpecification.FieldOfView(1)*[-1/2 1/2];
config.SensorLimits(2,:) = radarSpecification.FieldOfView(2)*[-1/2 1/2];
config.SensorLimits(3,:) = radarSpecification.RangeLimits;
config.SensorLimits(4,:) = radarSpecification.RangeRateLimits;

% Provide radar mounting with respect to ego
config.SensorTransformParameters(1).OriginPosition = radarSpecification.MountingLocation(:);
config.SensorTransformParameters(1).OriginVelocity = cross(radarSpecification.MountingLocation(:), deg2rad(egoPose.AngularVelocity(:)));
config.SensorTransformParameters(1).Orientation = rotmat(quaternion(radarSpecification.MountingAngles(:)','eulerd','ZYX','frame'),'frame');
config.SensorTransformParameters(1).IsParentToChild = true;

% Provide ego pose with respect to a static reference frame
config.SensorTransformParameters(2).OriginPosition = egoPose.Position(:);
config.SensorTransformParameters(2).OriginVelocity = egoPose.Velocity(:);
config.SensorTransformParameters(2).Orientation = rotmat(quaternion([egoPose.Yaw egoPose.Pitch egoPose.Roll],'eulerd','ZYX','frame'),'frame');
config.SensorTransformParameters(2).IsParentToChild = true;

% Detection probability of the radar
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.