Checking for CUDA availability on the Target...
Checking for 'nvcc' in the target system path...
Checking for cuDNN library availability on the Target...
Checking for TensorRT library availability on the Target...
Checking for prerequisite libraries is complete.
Gathering hardware details...
Checking for third-party library availability on the Target...
Gathering hardware details is complete.
 Board name         : NVIDIA Jetson AGX Xavier
 CUDA Version       : 10.2
 cuDNN Version      : 8.0
 TensorRT Version   : 7.1
 GStreamer Version  : 1.14.5
 V4L2 Version       : 1.14.2-1
 SDL Version        : 1.2
 OpenCV Version     : 4.1.1
 Available Webcams  : HP Webcam HD 4310
 Available GPUs     : Xavier

function [egoPoints, groundPoints, obstaclePoints] = ...
    segmentGroundAndObstacles(ptCloudLocation)
%segmentGroundAndObstacles segments ground and nearby obstacle points from 
%the pointcloud.
%   egoPoints = segmentGroundAndObstacles(ptCloudLocation) segments the 
%   ground points and nearby obstacle points from the input argument 
%   ptCloudLocation. egoPoints are the vehicle points segmented with the 
%   help of helper function helperSegmentEgoFromLidarData. 
%   segmentGroundFromLidarData is used to segment the ground points, 
%   groundPoints. findNeighborsInRadius is used to segment the nearby 
%   obstacles, obstaclePoints.
%
%   [..., groundPoints, obstaclePoints] = segmentGroundAndObstacles(...)
%   additionally returns segmented groundPoints and obstaclePoints.

%   Copyright 2021 The MathWorks, Inc.

%#codegen

% GPU Coder pragma
coder.gpu.kernelfun;

% Create a pointCloud object for point cloud locations
ptCloud = pointCloud(ptCloudLocation);

%% Segment the Ego Vehicle
% The lidar is mounted on top of the vehicle, and the point cloud may 
% contain points belonging to the vehicle itself, such as on the roof or 
% hood. Knowing the dimensions of the vehicle, we can segment out points 
% that are closest to the vehicle.

% Create a vehicleDimensions object for storing dimensions of the vehicle.
% Typical vehicle 4.7m by 1.8m by 1.4m

vehicleDims.Length = 4.7;
vehicleDims.Width = 1.8;
vehicleDims.Height = 1.4;
vehicleDims.RearOverhang = 1.0;

% Specify the mounting location of the lidar in the vehicle coordinate 
% system. The vehicle coordinate system is centered at the center of the 
% rear-axle, on the ground, with positive X direction pointing forward, 
% positive Y towards the left, and positive Z upwards. In this example, 
% the lidar is mounted on the top center of the vehicle, parallel to the 
% ground.

mountLocation = [...
    vehicleDims.Length/2 - vehicleDims.RearOverhang, ... % x
    0, ...                                               % y
    vehicleDims.Height];                                 % z

% Segment the ego vehicle using the helper function 
% |helperSegmentEgoFromLidarData|. This function segments all points within
% the cuboid defined by the ego vehicle. 

egoPoints = helperSegmentEgoFromLidarData(ptCloudLocation, vehicleDims, mountLocation);

%% Segment Ground Plane and Nearby Obstacles
% In order to identify obstacles from the lidar data, first segment the 
% ground plane using the segmentGroundFromLidarData function to accomplish 
% this. This function segments points belonging to the ground from organized 
% lidar data.

elevationDelta = 10;
groundPoints = segmentGroundFromLidarData(ptCloud, 'ElevationAngleDelta', elevationDelta);

% Remove points belonging to the ego vehicle and the ground plane by using 
% the select function on the point cloud. Specify the 'OutputSize' as 
% 'full' to retain the organized nature of the point cloud. For CUDA code 
% generation, an optimized approach for removing the points is considered.

nonEgoGroundPoints = coder.nullcopy(egoPoints);
coder.gpu.kernel;
for iter = 1:numel(egoPoints)
    nonEgoGroundPoints(iter) = ~egoPoints(iter) & ~groundPoints(iter);
end

ptCloudSegmented = select(ptCloud, nonEgoGroundPoints, 'OutputSize', 'full');

% Next, segment nearby obstacles by looking for all points that are not 
% part of the ground or ego vehicle within some radius from the ego
% vehicle. This radius can be determined based on the range of the lidar 
% and area of interest for further processing.

sensorLocation  = [0, 0, 0]; % Sensor is at the center of the coordinate system
radius          = 40; % meters

obstaclePoints = findNeighborsInRadius(ptCloudSegmented, sensorLocation, radius);

end


function egoPoints = helperSegmentEgoFromLidarData(ptCloudLocation, vehicleDims, mountLocation)
%#codegen
%helperSegmentEgoFromLidarData segment ego vehicle points from lidar data
%   egoPoints = helperSegmentEgoFromLidarData(ptCloudLocation,vehicleDims,mountLocation)
%   segments points belonging to the ego vehicle of dimensions vehicleDims
%   from the lidar scan ptCloud. The lidar is mounted at location specified
%   by mountLocation in the vehicle coordinate system. ptCloud is a
%   pointCloud object. vehicleDimensions is a vehicleDimensions object.
%   mountLocation is a 3-element vector specifying XYZ location of the
%   lidar in the vehicle coordinate system.
%
%   This function assumes that the lidar is mounted parallel to the ground
%   plane, with positive X direction pointing ahead of the vehicle,
%   positive Y direction pointing to the left of the vehicle in a
%   right-handed system.

% GPU Coder pragma
coder.gpu.kernelfun;

% Buffer around ego vehicle 
bufferZone = [0.1, 0.1, 0.1]; % in meters

% Define ego vehicle limits in vehicle coordinates
egoXMin = -vehicleDims.RearOverhang - bufferZone(1);
egoXMax = egoXMin + vehicleDims.Length + bufferZone(1);
egoYMin = -vehicleDims.Width/2 - bufferZone(2);
egoYMax = egoYMin + vehicleDims.Width + bufferZone(2);
egoZMin = 0 - bufferZone(3);
egoZMax = egoZMin + vehicleDims.Height + bufferZone(3);

egoXLimits = [egoXMin, egoXMax];
egoYLimits = [egoYMin, egoYMax];
egoZLimits = [egoZMin, egoZMax];

% Transform to lidar coordinates
egoXLimits = egoXLimits - mountLocation(1);
egoYLimits = egoYLimits - mountLocation(2);
egoZLimits = egoZLimits - mountLocation(3);

% Use logical indexing to select points inside ego vehicle cube
egoPoints = ptCloudLocation(:,:,1) > egoXLimits(1) ...
    & ptCloudLocation(:,:,1) < egoXLimits(2) ...
    & ptCloudLocation(:,:,2) > egoYLimits(1) ...
    & ptCloudLocation(:,:,2) < egoYLimits(2) ...
    & ptCloudLocation(:,:,3) > egoZLimits(1) ...
    & ptCloudLocation(:,:,3) < egoZLimits(2);
end

### Connectivity configuration for function 'segmentGroundAndObstacles': 'NVIDIA Jetson'
PIL execution is using Port 17725.
PIL execution is using 100 Sec(s) for receive time-out.
Code generation successful: View report

### Starting application: 'codegen/lib/segmentGroundAndObstacles/pil/segmentGroundAndObstacles.elf'
    To terminate execution: clear segmentGroundAndObstacles_pil
### Launching application segmentGroundAndObstacles.elf...
    Execution profiling data is available for viewing. Open Simulation Data Inspector.
    Execution profiling report available after termination.

PIL execution terminated on target.
### Connectivity configuration for function 'segmentGroundAndObstacles': 'NVIDIA Jetson'
PIL execution is using Port 17725.
PIL execution is using 100 Sec(s) for receive time-out.
    Execution profiling report: report(getCoderExecutionProfile('segmentGroundAndObstacles'))

### Connectivity configuration for function 'segmentGroundAndObstacles': 'NVIDIA Jetson'
PIL execution is using Port 17725.
PIL execution is using 100 Sec(s) for receive time-out.
Code generation successful: View report

### Starting application: 'codegen/lib/segmentGroundAndObstacles/pil/segmentGroundAndObstacles.elf'
    To terminate execution: clear segmentGroundAndObstacles_pil
### Launching application segmentGroundAndObstacles.elf...
    Execution profiling data is available for viewing. Open Simulation Data Inspector.
    Execution profiling report available after termination.

PIL execution terminated on target.
### Connectivity configuration for function 'segmentGroundAndObstacles': 'NVIDIA Jetson'
PIL execution is using Port 17725.
PIL execution is using 100 Sec(s) for receive time-out.
    Execution profiling report: report(getCoderExecutionProfile('segmentGroundAndObstacles'))