%% Generate Visual Detections from Monocular Camera
% Create a vision sensor by using a monocular camera configuration, and generate detections from that sensor.
% Specify the intrinsic parameters of the camera and create a monoCamera object from these parameters. The camera is mounted on top of an ego vehicle at a height of 1.5 meters above the ground and a pitch of 1 degree toward the ground.

focalLength = [800 800];
principalPoint = [320 240];
imageSize = [480 640];
intrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);

height = 1.5;
pitch = 1;
monoCamConfig = monoCamera(intrinsics,height,'Pitch',pitch);

%
% Create a vision detection generator using the monocular camera configuration.

visionSensor = visionDetectionGenerator(monoCamConfig);

%
% Generate a driving scenario with an ego vehicle and two target cars. Position the first target car 30 meters directly in front of the ego vehicle. Position the second target car 20 meters in front of the ego vehicle but offset to the left by 3 meters.

scenario = drivingScenario;
egoVehicle = vehicle(scenario,'ClassID',1);
targetCar1 = vehicle(scenario,'ClassID',1,'Position',[30 0 0]);
targetCar2 = vehicle(scenario,'ClassID',1,'Position',[20 3 0]);

%
% Use a bird's-eye plot to display the vehicle outlines and sensor coverage area.

figure
bep = birdsEyePlot('XLim',[0 50],'YLim',[-20 20]);

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

caPlotter = coverageAreaPlotter(bep,'DisplayName','Coverage area','FaceColor','blue');
plotCoverageArea(caPlotter,visionSensor.SensorLocation,visionSensor.MaxRange, ...
    visionSensor.Yaw,visionSensor.FieldOfView(1))

%
% Obtain the poses of the target cars from the perspective of the ego vehicle. Use these poses to generate detections from the sensor.

poses = targetPoses(egoVehicle);
[dets,numValidDets] = visionSensor(poses,scenario.SimulationTime);

%
% Display the (X,Y) positions of the valid detections. For each detection, the (X,Y) positions are the first two values of the Measurement field.

for i = 1:numValidDets
    XY = dets{i}.Measurement(1:2);
    detXY = sprintf('Detection %d: X = %.2f meters, Y = %.2f meters',i,XY);
    disp(detXY)
end

% Copyright 2012 The MathWorks, Inc.