%% Configure Ideal Vision Sensor
% Generate detections from an ideal vision sensor and compare these detections to ones from a noisy sensor. An ideal sensor is one that always generates detections, with no false positives and no added random noise.
%% Create a Driving Scenario
% Create a driving scenario in which the ego vehicle is positioned in front of a diagonal array of target cars. With this configuration, you can later plot the measurement noise covariances of the detected targets without having the target cars occlude one another.

scenario = drivingScenario;
egoVehicle = vehicle(scenario,'ClassID',1);

numTgts = 6;
x = linspace(20,50,numTgts)';
y = linspace(-20,0,numTgts)';
x = [x;x(1:end-1)];
y = [y;-y(1:end-1)];
numTgts = numel(x);

for m = 1:numTgts
    vehicle(scenario,'ClassID',1,'Position',[x(m) y(m) 0]);
end

% Plot the driving scenario in a bird's-eye plot.

bep = birdsEyePlot('XLim',[0 60]);
legend('hide')

olPlotter = outlinePlotter(bep);
[position,yaw,length,width,originOffset,color] = targetOutlines(egoVehicle);
plotOutline(olPlotter,position,yaw,length,width, ...
    'OriginOffset',originOffset,'Color',color)

%
%% Create an Ideal Vision Sensor
% Create a vision sensor by using the visionDetectionGenerator System object™. To generate ideal detections, set DetectionProbability to 1, FalsePositivesPerImage to 0, and HasNoise to false.
% DetectionProbability = 1 — The sensor always generates detections for a target, as long as the target is not occluded and meets the range, speed, and image size constraints.
% FalsePositivesPerImage = 0 — The sensor generates detections from only real targets in the driving scenario.
% HasNoise = false — The sensor does not add random noise to the reported position and velocity of the target. However, the objectDetection objects returned by the sensor have measurement noise values set to the noise variance that would have been added if HasNoise were true. With these noise values, you can process ideal detections using the multiObjectTracker. This technique is useful for analyzing maneuver lag without needing to run time-consuming Monte Carlo simulations.

idealSensor = visionDetectionGenerator( ...
    'SensorIndex',1, ...
    'UpdateInterval',scenario.SampleTime, ...
    'SensorLocation',[0.75*egoVehicle.Wheelbase 0], ...
    'Height',1.1, ...
    'Pitch',0, ...
    'Intrinsics',cameraIntrinsics(800,[320 240],[480 640]), ...
    'BoundingBoxAccuracy',50, ... % Make the noise large for illustrative purposes
    'ProcessNoiseIntensity',5, ...
    'MaxRange',60, ...
    'DetectionProbability',1, ...
    'FalsePositivesPerImage',0, ...
    'HasNoise',false, ...
    'ActorProfiles',actorProfiles(scenario))

% Plot the coverage area of the ideal vision sensor.

legend('show')
caPlotter = coverageAreaPlotter(bep,'DisplayName','Coverage area','FaceColor','blue');
mountPosition = idealSensor.SensorLocation;
range = idealSensor.MaxRange;
orientation = idealSensor.Yaw;
fieldOfView = idealSensor.FieldOfView(1);
plotCoverageArea(caPlotter,mountPosition,range,orientation,fieldOfView);

%
%% Simulate Ideal Vision Detections
% Obtain the positions of the targets. The positions are in ego vehicle coordinates.

gTruth = targetPoses(egoVehicle);

% Generate timestamped vision detections. These detections are returned as a cell array of objectDetection objects.

time = scenario.SimulationTime;
dets = idealSensor(gTruth,time);

% Inspect the measurement and measurement noise variance of the first (leftmost) detection. Even though the detection is ideal and therefore has no added random noise, the MeasurementNoise property shows the values as if the detection did have noise.

dets{1}.Measurement
dets{1}.MeasurementNoise

% Plot the ideal detections and ellipses for the 2-sigma contour of the measurement noise covariance.

pos = cell2mat(cellfun(@(d)d.Measurement(1:2)',dets, ...
    'UniformOutput',false));
cov = reshape(cell2mat(cellfun(@(d)d.MeasurementNoise(1:2,1:2),dets, ...
    'UniformOutput',false))',2,2,[]);
plotter = trackPlotter(bep,'DisplayName','Ideal detections', ...
    'MarkerEdgeColor','blue','MarkerFaceColor','blue');
sigma = 2;
plotTrack(plotter,pos,sigma^2*cov)

%
%% Simulate Noisy Detections for Comparison
% Create a noisy sensor based on the properties of the ideal sensor.

noisySensor = clone(idealSensor);
release(noisySensor)
noisySensor.HasNoise = true;

% Reset the driving scenario back to its original state.

restart(scenario)

% Collect statistics from the noisy detections.

numMonte = 1e3;
pos = [];
for itr = 1:numMonte
    time = scenario.SimulationTime;
    dets = noisySensor(gTruth,time);

    % Save noisy measurements
    pos = [pos;cell2mat(cellfun(@(d)d.Measurement(1:2)',dets,'UniformOutput',false))];

    advance(scenario);
end

% Plot the noisy detections.

plotter = detectionPlotter(bep,'DisplayName','Noisy detections', ...
    'Marker','.','MarkerEdgeColor','red','MarkerFaceColor','red');
plotDetection(plotter,pos)

% Copyright 2012 The MathWorks, Inc.