Generate Code for a Track Fuser with Heterogeneous Source Tracks

This example uses:

This example shows how to generate code for a track-level fusion algorithm in a scenario where the tracks originate from heterogeneous sources with different state definitions. This example is based on the Track-Level Fusion of Radar and Lidar Data (Sensor Fusion and Tracking Toolbox) example, in which the state spaces of the tracks generated from lidar and radar sources are different.

Define a Track Fuser for Code Generation

You can generate code for a trackFuser (Sensor Fusion and Tracking Toolbox) using MATLAB® Coder™. To do so, you must modify your code to comply with the following limitations:

Code Generation Entry Function

Follow the instructions on how to use System Objects in MATLAB Code Generation (MATLAB Coder). For code generation, you must first define an entry-level function, in which the object is defined. Also, the function cannot use arrays of objects as inputs or outputs. In this example, you define the entry-level function as the heterogeneousInputsFuser function. The function must be on the path when you generate code for it. Therefore, it cannot be part of this live script and is attached in this example. The function accepts local tracks and current time as input and outputs central tracks.

To preserve the state of the fuser between calls to the function, you define the fuser as a persistent variable. On the first call, you must define the fuser variable because it is empty. The rest of the following code steps the trackFuser and returns the fused tracks.

function tracks = heterogeneousInputsFuser(localTracks,time)
%#codegen

persistent fuser
if isempty(fuser)
    % Define the radar source configuration
    radarConfig = fuserSourceConfiguration('SourceIndex',1,...
        'IsInitializingCentralTracks',true,...
        'CentralToLocalTransformFcn',@central2local,...
        'LocalToCentralTransformFcn',@local2central);
    
    % Define the lidar source configuration
    lidarConfig = fuserSourceConfiguration('SourceIndex',2,...
        'IsInitializingCentralTracks',true,...
        'CentralToLocalTransformFcn',@central2local,...
        'LocalToCentralTransformFcn',@local2central);
    
    % Create a trackFuser object
    fuser = trackFuser(...
        'MaxNumSources', 2, ...
        'SourceConfigurations',{radarConfig;lidarConfig},...
        'StateTransitionFcn',@helperctcuboid,...
        'StateTransitionJacobianFcn',@helperctcuboidjac,...
        'ProcessNoise',diag([1 3 1]),...
        'HasAdditiveProcessNoise',false,...
        'AssignmentThreshold',[250 inf],...
        'ConfirmationThreshold',[3 5],...
        'DeletionThreshold',[5 5],...
        'StateFusion','Custom',...
        'CustomStateFusionFcn',@helperRadarLidarFusionFcn);
end

tracks = fuser(localTracks, time);
end

Homogeneous Source Configurations

In this example, you define the radar and lidar source configurations differently than in the original Track-Level Fusion of Radar and Lidar Data (Sensor Fusion and Tracking Toolbox) example. In the original example, the CentralToLocalTransformFcn and LocalToCentralTransformFcn properties of the two source configurations are different because they use different function handles. This makes the source configurations a heterogeneous cell array. Such a definition is correct and valid when executing in MATLAB. However, in code generation, all source configurations must use the same function handles. To avoid the different function handles, you define one function to transform tracks from central (fuser) definition to local (source) definition and one function to transform from local to central. Each of these functions switches between the transform functions defined for the individual sources in the original example. Both functions are part of the heterogeneousInputsFuser function.

Here is the code for the local2central function, which uses the SourceIndex property to determine the correct function to use. Since the two types of local tracks transform to the same definition of central track, there is no need to predefine the central track.

function centralTrack = local2central(localTrack)
switch localTrack.SourceIndex
    case 1 % radar
        centralTrack = radar2central(localTrack);
    otherwise % lidar
        centralTrack = lidar2central(localTrack);
end
end

The function central2local transforms the central track into a radar track if SourceIndex is 1 or into a lidar track if SourceIndex is 2. Since the two tracks have a different definition of State, StateCovariance, and TrackLogicState, you must first predefine the output. Here is the code snippet for the function:

function localTrack = central2local(centralTrack)
state = 0;
stateCov = 1;
coder.varsize('state', [10, 1], [1 0]);
coder.varsize('stateCov', [10 10], [1 1]);
localTrack = objectTrack('State', state, 'StateCovariance', stateCov);

switch centralTrack.SourceIndex
    case 1
        localTrack = central2radar(centralTrack);
    case 2
        localTrack = central2lidar(centralTrack);
    otherwise
        % This branch is never reached but is necessary to force code 
        % generation to use the predefined localTrack.
end
end

The functions radar2central and central2radar are the same as in the original example but moved from the live script to the heterogeneousInputsFuser function. You also add the lidar2central and central2lidar functions to the heterogeneousInputsFuser function. These two functions convert from the track definition that the fuser uses to the lidar track definition.

Run the Example in MATLAB

Before generating code, make sure that the example still runs after all the changes made to the fuser. The file lidarRadarData.mat contains the same scenario as in the original example. It also contains a set of radar and lidar tracks recorded at each step of that example. You also use a similar display to visualize the example and define the same trackGOSPAMetric objects to evaluate the tracking performance.

% Load the scenario and recorded local tracks
load('lidarRadarData.mat','scenario','localTracksCollection')
display = helperTrackFusionCodegenDisplay('FollowActorID',3);
showLegend(display,scenario);

% Radar GOSPA
gospaRadar = trackGOSPAMetric('Distance','custom',...
    'DistanceFcn',@helperRadarDistance,...
    'CutoffDistance',25);

% Lidar GOSPA
gospaLidar = trackGOSPAMetric('Distance','custom',...
    'DistanceFcn',@helperLidarDistance,...
    'CutoffDistance',25);

% Central/Fused GOSPA
gospaCentral = trackGOSPAMetric('Distance','custom',...
    'DistanceFcn',@helperLidarDistance,... % State space is same as lidar
    'CutoffDistance',25);


gospa = zeros(3,0);
missedTargets = zeros(3,0);
falseTracks = zeros(3,0);
% Ground truth for metrics. This variable updates every time step
% automatically, because it is a handle to the actors.
groundTruth = scenario.Actors(2:end);

fuserStepped = false;
fusedTracks = objectTrack.empty;
idx = 1;
clear heterogeneousInputsFuser
while advance(scenario)
    time = scenario.SimulationTime;
    localTracks = localTracksCollection{idx};
    
    if ~isempty(localTracks) || fuserStepped
        fusedTracks = heterogeneousInputsFuser(localTracks,time);
        fuserStepped = true;
    end
    
    radarTracks = localTracks([localTracks.SourceIndex]==1);
    lidarTracks = localTracks([localTracks.SourceIndex]==2);
    
    % Capture GOSPA and its components for all trackers
    [gospa(1,idx),~,~,~,missedTargets(1,idx),falseTracks(1,idx)] = gospaRadar(radarTracks, groundTruth);
    [gospa(2,idx),~,~,~,missedTargets(2,idx),falseTracks(2,idx)] = gospaLidar(lidarTracks, groundTruth);
    [gospa(3,idx),~,~,~,missedTargets(3,idx),falseTracks(3,idx)] = gospaCentral(fusedTracks, groundTruth);
    
    % Update the display
    display(scenario,[],[], radarTracks,...
        [],[],[],[], lidarTracks, fusedTracks);
    
    idx = idx + 1;
end

Generate Code for the Track Fuser

To generate code, you must define the input types for both the radar and lidar tracks and the timestamp. In both the original script and in the previous section, the radar and lidar tracks are defined as arrays of objectTrack (Sensor Fusion and Tracking Toolbox) objects. In code generation, the entry-level function cannot use an array of objects. Instead, you define an array of structures.

You use the struct oneLocalTrack to define the inputs coming from radar and lidar tracks. In code generation, the specific data types of each field in the struct must be defined exactly the same as the types defined for the corresponding properties in the recorded tracks. Furthermore, the size of each field must be defined correctly. You use the coder.typeof (MATLAB Coder) function to specify fields that have variable size: State, StateCovariance, and TrackLogicState. You define the localTracks input using the oneLocalTrack struct and the coder.typeof function, because the number of input tracks varies from zero to eight in each step. You use the function codegen (MATLAB Coder) to generate the code.

Notes:

If the input tracks use different types for the State and StateCovariance properties, you must decide which type to use, double or single. In this example, all tracks use double precision and there is no need for this step.
If the input tracks use different definitions of StateParameters, you must first create a superset of all StateParameters and use that superset in the StateParameters field. A similar process must be done for the ObjectAttributes field. In this example, all tracks use the same definition of StateParameters and ObjectAttributes.

% Define the inputs to fuserHeterogeneousInputs for code generation
oneLocalTrack = struct(...
    'TrackID', uint32(0), ...
    'BranchID', uint32(0), ...
    'SourceIndex', uint32(0), ...
    'UpdateTime', double(0), ...
    'Age', uint32(0), ...
    'State', coder.typeof(1, [10 1], [1 0]), ...
    'StateCovariance', coder.typeof(1, [10 10], [1 1]), ...
    'StateParameters', struct, ...
    'ObjectClassID', double(0), ...
    'ObjectClassProbabilities', double(1),...
    'TrackLogic', 'History', ...
    'TrackLogicState', coder.typeof(false, [1 10], [0 1]), ...
    'IsConfirmed', false, ...
    'IsCoasted', false, ...
    'IsSelfReported', false, ...
    'ObjectAttributes', struct);

localTracks = coder.typeof(oneLocalTrack, [8 1], [1 0]);
fuserInputArguments = {localTracks, time};

codegen heterogeneousInputsFuser -args fuserInputArguments;

Code generation successful.

Run the Example with the Generated Code

You run the generated code like you ran the MATLAB code, but first you must reinitialize the scenario, the GOSPA objects, and the display.

You use the toStruct (Sensor Fusion and Tracking Toolbox) object function to convert the input tracks to arrays of structures.

Notes:

If the input tracks use different data types for the State and StateCovariance properties, make sure to cast the State and StateCovariance of all the tracks to the data type you chose when you defined the oneLocalTrack structure above.
If the input tracks required a superset structure for the fields StateParameters or ObjectAttributes, make sure to populate these structures correctly before calling the mex file.

You use the gospaCG variable to keep the GOSPA metrics for this run so that you can compare them to the GOSPA values from the MATLAB run.

% Rerun the scenario with the generated code
fuserStepped = false;
fusedTracks = objectTrack.empty;
gospaCG = zeros(3,0);
missedTargetsCG = zeros(3,0);
falseTracksCG = zeros(3,0);

idx = 1;
clear heterogeneousInputsFuser_mex
reset(display);
reset(gospaRadar);
reset(gospaLidar);
reset(gospaCentral);
restart(scenario);
while advance(scenario)
    time = scenario.SimulationTime;
    localTracks = localTracksCollection{idx};
    
    if ~isempty(localTracks) || fuserStepped
        fusedTracks = heterogeneousInputsFuser_mex(toStruct(localTracks),time);
        fuserStepped = true;
    end
    
    radarTracks = localTracks([localTracks.SourceIndex]==1);
    lidarTracks = localTracks([localTracks.SourceIndex]==2);
    
    % Capture GOSPA and its components for all trackers
    [gospaCG(1,idx),~,~,~,missedTargetsCG(1,idx),falseTracksCG(1,idx)] = gospaRadar(radarTracks, groundTruth);
    [gospaCG(2,idx),~,~,~,missedTargetsCG(2,idx),falseTracksCG(2,idx)] = gospaLidar(lidarTracks, groundTruth);
    [gospaCG(3,idx),~,~,~,missedTargetsCG(3,idx),falseTracksCG(3,idx)] = gospaCentral(fusedTracks, groundTruth);
    
    % Update the display
    display(scenario,[],[], radarTracks,...
        [],[],[],[], lidarTracks, fusedTracks);
    
    idx = idx + 1;
end

At the end of the run, you want to verify that the generated code provided the same results as the MATLAB code. Using the GOSPA metrics you collected in both runs, you can compare the results at the high level. Due to numerical roundoffs, there may be small differences in the results of the generated code relative to the MATLAB code. To compare the results, you use the absolute differences between GOSPA values and check if they are all smaller than 1e-10. The results show that the differences are very small.

% Compare the GOSPA values from MATLAB run and generated code
areGOSPAValuesEqual = all(abs(gospa-gospaCG)<1e-10,'all');
disp("Are GOSPA values equal up to the 10th decimal (true/false)? " + string(areGOSPAValuesEqual))

Are GOSPA values equal up to the 10th decimal (true/false)? true

Summary

In this example, you learned how to generate code for a track-level fusion algorithm when the input tracks are heterogeneous. You learned how to define the trackFuser and its SourceConfigurations property to support heterogeneous sources. You also learned how to define the input in compilation time and how to pass it to the mex file in runtime.

Supporting Functions

The following functions are used by the GOSPA metric.

helperLidarDistance

Function to calculate a normalized distance between the estimate of a track in radar state-space and the assigned ground truth.

function dist = helperLidarDistance(track, truth)

% Calculate the actual values of the states estimated by the tracker

% Center is different than origin and the trackers estimate the center
rOriginToCenter = -truth.OriginOffset(:) + [0;0;truth.Height/2];
rot = quaternion([truth.Yaw truth.Pitch truth.Roll],'eulerd','ZYX','frame');
actPos = truth.Position(:) + rotatepoint(rot,rOriginToCenter')';

% Actual speed and z-rate
actVel = [norm(truth.Velocity(1:2));truth.Velocity(3)];

% Actual yaw
actYaw = truth.Yaw;

% Actual dimensions.
actDim = [truth.Length;truth.Width;truth.Height];

% Actual yaw rate
actYawRate = truth.AngularVelocity(3);

% Calculate error in each estimate weighted by the "requirements" of the
% system. The distance specified using Mahalanobis distance in each aspect
% of the estimate, where covariance is defined by the "requirements". This
% helps to avoid skewed distances when tracks under/over report their
% uncertainty because of inaccuracies in state/measurement models.

% Positional error. 
estPos = track.State([1 2 6]);
reqPosCov = 0.1*eye(3);
e = estPos - actPos;
d1 = sqrt(e'/reqPosCov*e);

% Velocity error
estVel = track.State([3 7]);
reqVelCov = 5*eye(2);
e = estVel - actVel;
d2 = sqrt(e'/reqVelCov*e);

% Yaw error
estYaw = track.State(4);
reqYawCov = 5;
e = estYaw - actYaw;
d3 = sqrt(e'/reqYawCov*e);

% Yaw-rate error
estYawRate = track.State(5);
reqYawRateCov = 1;
e = estYawRate - actYawRate;
d4 = sqrt(e'/reqYawRateCov*e);

% Dimension error
estDim = track.State([8 9 10]);
reqDimCov = eye(3);
e = estDim - actDim;
d5 = sqrt(e'/reqDimCov*e);

% Total distance
dist = d1 + d2 + d3 + d4 + d5;
end

helperRadarDistance