automobile/detect-lane-by-lidar-point-cloud/LaneDetector.m
% Detects lanes points on lidar point clouds by detecting peaks in the intensities of lidar data.
% The detected lane points are also fitted with a second degree polynomial.
%
% laneDetector = LaneDetector(Name,Value) specifies additional
% name-value pair arguments as described below:
%
% 'ROI' The roi is a cuboid specified as a
% [xmin, xmax, ymin, ymax, zmin, zmax].
% It is used to define the region of
% interest for possible lane points.
%
% Default: [0 40 -3, 3 -4, 1]
%
% 'LaneWidth' The lateral distance between the two
% lanes in world units.
%
% Default: 4
%
% 'HistogramBinResolution' The resolution to compute the histogram
% of intensities in world units.
%
% Default: 0.2
%
% 'VerticalBinResolution' The vertical bin resolution represents
% the size of bins in world units along
% ego vehicle direction for updating the
% next window in sliding window algorithm.
% If the distance between discontinous lane
% markings is more, this parameter can be
% increased to improve the detection.
%
% Default: 1
%
% 'HorizontalBinResolution' The horizontal bin resolution represents
% the size of bins in world units
% perpendicular to the ego vehicle
% direction for updating the next window
% in sliding window algorithm. If the
% lane markings are broad, this parameter
% can be increased to improve the detection.
%
% Default: 1
%
% LaneDetector methods :
%
% detectLanes Detects ego vehicle lanes as left lanes
% and right lanes stored in a cell array in
% the same order.
%
% updateLanePolynomial Updates the detected lane points using a
% polynomial defined in X-Y coordinates.
classdef LaneDetector < handle
properties (Access = public)
% Defines plane model
PlaneModel
% Defines distance between ego lanes
LaneWidth
% Defines detected lane parameters
LanePolynomial
end
properties (Access = private)
% Region of Interest
ROI
% Lane start points
StartLanePoints
% Resolution of windows defined along lane axis
VerticalBinResolution
% Resolution of windows defined along horizontal axis
HorizontalBinResolution
% Bin Resolution for histogram creation
HistogramBinResolution
% Number of lanes to detect
LanesDetect
end
properties (Constant, Access = protected)
DefaultROI = [0 40 -3, 3 -4, 1]
DefaultLaneWidth = 4
DefaultVerticalBinResolution = 1
DefaultHorizontalBinResolution = 1
DefaultHistogramBinResolution = 0.2
end
methods
function obj = LaneDetector(varargin)
% Construct an instance of this class
parser = inputParser;
parser.CaseSensitive = false;
% Optional argument parsers
addParameter(parser, 'ROI', obj.DefaultROI, @obj.validateROI);
addParameter(parser, 'LaneWidth', obj.DefaultLaneWidth, @obj.validateLaneWidth);
addParameter(parser, 'HistogramBinResolution', obj.DefaultHistogramBinResolution, @obj.validateHistogramBinResolution);
addParameter(parser, 'VerticalBinResolution', obj.DefaultVerticalBinResolution, @obj.validateVerticalBinResolution);
addParameter(parser, 'HorizontalBinResolution', obj.DefaultHorizontalBinResolution, @obj.validateHorizontalBinResolution);
% Parse input arguments
parse(parser, varargin{:});
obj.ROI = parser.Results.ROI;
obj.LaneWidth = parser.Results.LaneWidth;
obj.VerticalBinResolution = parser.Results.VerticalBinResolution;
obj.HorizontalBinResolution = parser.Results.HorizontalBinResolution;
obj.HistogramBinResolution = parser.Results.HistogramBinResolution;
end
function lanePoints = detectLanes(obj, ptCloudIn)
% Preprocess point cloud
ptCloudIn = preprocessPointCloud(obj, ptCloudIn);
% Extract lane start points using intensity histogram
detection = computeStartPoint(obj, ptCloudIn);
if ~detection
lanePoints = [];
return
end
% Extract lane points using window search
lanePoints = detectLanesImpl(obj, ptCloudIn);
% Refine lane points using fitting
lanePoints = refineLanePoints(obj, lanePoints);
end
function lanes = updateLanePolynomial(obj, lanePolynomial)
% 3D lane points are estimated using plane model and polynomial parameters.
% Plane model is represented as ax + by + cz + d = 0
% 2nd degree polynomial is represented as ax^2 + bx + c
P1 = lanePolynomial(1, :);
P2 = lanePolynomial(2, :);
xval = linspace(obj.ROI(1), 40, 80);
yval1 = polyval(P1, xval);
yval2 = polyval(P2, xval);
modelParams = obj.PlaneModel;
zWorld1 = (-modelParams(1) * xval - modelParams(2) * yval1 - modelParams(4)) / modelParams(3);
zWorld2 = (-modelParams(1) * xval - modelParams(2) * yval2 - modelParams(4)) / modelParams(3);
lane3d1 = [xval', yval1', zWorld1'];
lane3d2 = [xval', yval2', zWorld2'];
lanes{1} = lane3d1;
lanes{2} = lane3d2;
end
end
methods (Access = private, Hidden)
function [Ypk, Xpk] = findpeaks(~, histVal)
% Finding peaks in the histogram
x = (1:size(histVal, 2))';
y = histVal';
yTemp = [NaN; y; NaN];
iTemp = (1:length(yTemp)).';
% Keep only the first of any adjacent pairs of equal values (including NaN).
yFinite = ~isnan(yTemp);
iNeq = [1; 1 + find((yTemp(1:end - 1) ~= yTemp(2:end)) & (yFinite(1:end - 1) | yFinite(2:end)))];
iTemp = iTemp(iNeq);
% Take the sign of the first sample derivative
s = sign(diff(yTemp(iTemp)));
% Find local maxima
iMax = 1 + find(diff(s) < 0);
% Index into the original index vector without the NaN bookend.
iPk = iTemp(iMax) - 1;
% Fetch the coordinates of the peak
Ypk = y(iPk)';
Xpk = x(iPk)';
end
% Compute lane start point using intensity histogram
function isLaneDetected = computeStartPoint(obj, ptCloud)
[histVal, yvals] = computeHistogram(obj, ptCloud);
[peaks, locs] = findpeaks(obj, histVal);
startYs = yvals(locs);
yvals1 = computeInitialWindow(obj, startYs, peaks);
yvals2 = helperInitialWindow2(obj, startYs, peaks);
isLaneDetected = false;
if numel(yvals1) == 2 && numel(yvals2) == 2
if abs((yvals1(2) - yvals1(1)) - obj.LaneWidth) > abs((yvals2(2) - yvals2(1)) - obj.LaneWidth)
yvals = yvals2;
else
yvals = yvals1;
end
obj.StartLanePoints = yvals;
end
if ~isempty(yvals)
isLaneDetected = true;
end
end
% Intensity based histogram over ROI
function [histVal, yvals] = computeHistogram(obj, ptCloud)
numBins = ceil((ptCloud.YLimits(2) - ...
ptCloud.YLimits(1)) / obj.HistogramBinResolution);
histVal = zeros(1, numBins - 1);
binStartY = linspace(ptCloud.YLimits(1), ptCloud.YLimits(2), numBins);
yvals = zeros(1, numBins - 1);
for i = 1:numBins - 1
roi = [-inf, 15, binStartY(i), binStartY(i + 1) -inf, inf];
ind = findPointsInROI(ptCloud, roi);
subPc = select(ptCloud, ind);
if subPc.Count
histVal(i) = sum(subPc.Intensity);
yvals(i) = (binStartY(i) + binStartY(i + 1)) / 2;
end
end
end
% Finds the start window points using brute-force search
function yval = computeInitialWindow(obj, yvals, peaks)
leftLanesIndices = yvals >= 0;
rightLanesIndices = yvals < 0;
leftLaneYs = yvals(leftLanesIndices);
rightLaneYs = yvals(rightLanesIndices);
peaksLeft = peaks(leftLanesIndices);
peaksRight = peaks(rightLanesIndices);
diff = zeros(sum(leftLanesIndices), sum(rightLanesIndices));
for i = 1:sum(leftLanesIndices)
for j = 1:sum(rightLanesIndices)
diff(i, j) = abs(obj.LaneWidth - (leftLaneYs(i) - rightLaneYs(j)));
end
end
[~, minIndex] = min(diff(:));
[row, col] = ind2sub(size(diff), minIndex);
yval = [leftLaneYs(row), rightLaneYs(col)];
estimatedLaneWidth = leftLaneYs(row) - rightLaneYs(col);
% If the calculated lane width is not within the bounds return
% the lane with highest peak
if abs(estimatedLaneWidth - obj.LaneWidth) > 0.5
if peaksLeft(row) > peaksRight(col)
yval = leftLaneYs(row);
else
yval = rightLaneYs(col);
end
end
% Sort yval from min to max
yval = sort(yval);
end
% Finds start window points using maximum peak as anchor point
function startLanePoints = helperInitialWindow2(obj, startYs, peaks)
[~, maxInd] = max(peaks);
y1 = startYs(maxInd);
% Find ys > y1
ind1 = startYs > y1;
ind2 = startYs < y1;
dist1 = zeros(sum(ind1), 1);
dist2 = zeros(sum(ind2), 1);
startYs1 = startYs(ind1);
startYs2 = startYs(ind2);
for i = 1:sum(ind1)
dist1(i, :) = abs(obj.LaneWidth - (startYs1(i) - y1));
end
for i = 1:sum(ind2)
dist2(i, :) = abs(obj.LaneWidth - (y1 - startYs2(i)));
end
[~, minIndex1] = min(dist1);
[~, minIndex2] = min(dist2);
if isempty(minIndex1) || isempty(minIndex2)
if isempty(minIndex1)
y2 = startYs2(minIndex2);
else
y2 = startYs1(minIndex1);
end
else
if min(dist1) < min(dist2)
y2 = startYs1(minIndex1);
else
y2 = startYs2(minIndex2);
end
end
if y2 > y1
startLanePoints = [y1, y2];
else
startLanePoints = [y2, y1];
end
end
% Extract lane points using window search
function detectedLanePoints = detectLanesImpl(obj, ptCloud)
numVerticalBins = ceil((ptCloud.XLimits(2) - ptCloud.XLimits(1)) / obj.VerticalBinResolution);
laneStartX = linspace(ptCloud.XLimits(1), ptCloud.XLimits(2), numVerticalBins);
numLanes = numel(obj.StartLanePoints);
verticalBins = zeros(numVerticalBins, 3, numLanes);
lanes = zeros(numVerticalBins, 3, numLanes);
tmpStartY = obj.StartLanePoints;
roi = repmat([-inf, inf], 1, 3); %#ok<NASGU>
for i = 1:numVerticalBins - 1
for j = 1:numLanes
laneStartY = tmpStartY(j);
% Define a vertical roi window
roi = [laneStartX(i), laneStartX(i + 1), laneStartY - obj.HorizontalBinResolution / 2, ...
laneStartY + obj.HorizontalBinResolution / 2, -inf, inf];
tmpPc = select(ptCloud, findPointsInROI(ptCloud, roi));
if ~isempty(tmpPc.Location)
[~, maxIndex] = max(tmpPc.Intensity);
val = tmpPc.Location(maxIndex, :);
verticalBins(i, :, j) = val;
lanes(i, :, j) = val;
% Slide the window with the update mean value along y direction
tmpStartY(j) = val(2);
else
value = lanes(2:end, 1:2, j);
value(all(value == 0, 2), :) = [];
if size(value, 1) == 2
% Use linear prediction
P = fitPolynomialRANSAC(value, 1, 0.1);
elseif size(value, 1) > 2
% Use 2 degree polynomial prediction
P = fitPolynomialRANSAC(value, 2, 0.1);
else
verticalBins(i, :, j) = verticalBins(end, :, j);
continue
end
error = mean(sqrt((polyval(P, value(:, 1)) - value(:, 2)).^2));
if error < 0.1
xval = (roi(1) + roi(2)) / 2;
yval = polyval(P, xval);
% Use error to regularize the value of predicted y
yval = yval - error * abs(yval);
tmpStartY(j) = yval;
roi(3:4) = [yval - obj.HorizontalBinResolution, yval + obj.HorizontalBinResolution];
% Update the lane point with the centre of the predicted window
tmpPc = select(ptCloud, findPointsInROI(ptCloud, roi));
zmean = mean(tmpPc.Location(:, 3));
verticalBins(i, :, j) = [xval, yval, zmean];
else
roi(3:4) = [tmpStartY(j) - obj.HorizontalBinResolution, tmpStartY(j) + obj.HorizontalBinResolution]; %#ok<NASGU>
end
end
end
end
if size(lanes, 3) > 1
lane1 = lanes(:, :, 1);
lane2 = lanes(:, :, 2);
lane1(all(lane1 == 0, 2), :) = [];
lane2(all(lane2 == 0, 2), :) = [];
detectedLanePoints{1} = lane1;
detectedLanePoints{2} = lane2;
obj.LanesDetect = 2;
else
lane1 = lanes(:, :, 1);
lane1(all(lane1 == 0, 2), :) = [];
detectedLanePoints = lane1;
obj.LanesDetect = 1;
end
end
function refinedLanePoints = refineLanePoints(obj, lanePoints)
if obj.LanesDetect > 1
lane1 = lanePoints{1};
lane2 = lanePoints{2};
[P1, error1] = obj.fitPolynomial(lane1(:, 1:2), 2, 0.1);
[P2, error2] = obj.fitPolynomial(lane2(:, 1:2), 2, 0.1);
xval = linspace(obj.ROI(1), 40, 80);
yval1 = polyval(P1, xval);
yval2 = polyval(P2, xval);
% Z coordinate estimation
modelParams = obj.PlaneModel;
zWorld1 = (-modelParams(1) * xval - modelParams(2) * yval1 - modelParams(4)) / modelParams(3);
zWorld2 = (-modelParams(1) * xval - modelParams(2) * yval2 - modelParams(4)) / modelParams(3);
lane3d1 = [xval', yval1', zWorld1'];
lane3d2 = [xval', yval2', zWorld2'];
% Shift the polynomial with high score along Y-axis towards the polynomial with low score
if error1 > error2
lanePolynomial = P2;
if lane3d1(1, 2) > 0
lanePolynomial(3) = lane3d2(1, 2) + obj.LaneWidth;
else
lanePolynomial(3) = lane3d2(1, 2) - obj.LaneWidth;
end
lane3d1(:, 2) = polyval(lanePolynomial, lane3d1(:, 1));
else
lanePolynomial = P1;
if lane3d2(1, 2) > 0
lanePolynomial(3) = lane3d1(1, 2) + obj.LaneWidth;
else
lanePolynomial(3) = lane3d1(1, 2) - obj.LaneWidth;
end
P2 = lanePolynomial;
lane3d2(:, 2) = polyval(lanePolynomial, lane3d2(:, 1));
end
else
% For single lane
lane1 = lanePoints;
P1 = obj.fitPolynomial(lane1(:, 1:2), 2, 0.1);
P2 = P1;
xval = linspace(obj.ROI(1), 40, 80);
yval1 = polyval(P1, xval);
% Z coordinate estimation
modelParams = obj.PlaneModel;
zWorld1 = (-modelParams(1) * xval - modelParams(2) * yval1 - modelParams(4)) / modelParams(3);
lane3d1 = [xval', yval1', zWorld1'];
if lane3d1(1, 2) > 0
P2(3) = lane3d1(1, 2) - obj.LaneWidth;
else
P2(3) = lane3d1(1, 2) + obj.LaneWidth;
end
yval2 = polyval(P2, xval);
zWorld2 = (-modelParams(1) * xval - modelParams(2) * yval2 - modelParams(4)) / modelParams(3);
lane3d2 = [xval', yval2', zWorld2'];
end
refinedLanePoints{1} = lane3d1;
refinedLanePoints{2} = lane3d2;
obj.LanePolynomial = [P1; P2];
end
% Ground extraction and outlier removal
function [ground, obj] = preprocessPointCloud(obj, pc)
if obj.isOrganised(pc)
pc = removeInvalidPoints(pc);
end
ind = findPointsInROI(pc, obj.ROI);
pc = select(pc, ind);
% Remove Ego vehicle points
nearIndices = findNeighborsInRadius(pc, [0 0 0], 2);
nonEgoIndices = true(pc.Count, 1);
nonEgoIndices(nearIndices) = false;
indices = find(nonEgoIndices);
pc = select(pc, indices);
% Remove ground
[model, inliers, outliers] = pcfitplane(pc, 0.1, [0, 0, 1]);
obj.PlaneModel = model.Parameters;
ground = select(pc, inliers);
zlim = ground.ZLimits;
nonGround = select(pc, outliers);
[labels, numClusters] = pcsegdist(nonGround, 1);
% Remove all the outlier from the ground occuring because of possible obstacles
for i = 1:numClusters
tmp = nonGround.Location(labels == i, :);
xmin = min(tmp(:, 1));
xmax = max(tmp(:, 1));
ymin = min(tmp(:, 2));
ymax = max(tmp(:, 2));
roi = [xmin, xmax, ymin, ymax, zlim];
ground = obj.cropPointCloudFromROI(ground, roi);
end
end
end
methods (Static, Access = private)
function tf = validateROI(roi)
if ~isempty(roi)
if isnumeric(roi)
validateattributes(roi, {'single', 'double'}, {'real', 'nonsparse', 'nonnan', 'ncols', 6, 'nrows', 1}, mfilename, 'roi');
if any(roi(:, 1:2:5) > roi(:, 2:2:6))
error(message('lidar:lidarCameraCalibration:invalidROI'));
end
else
error(message('lidar:lidarCameraCalibration:invalidROIDataType'));
end
end
tf = true;
end
function [P, score] = fitPolynomial(pts, degree, resolution)
P = fitPolynomialRANSAC(pts, degree, resolution);
score = mean(sqrt((polyval(P, pts(:, 1)) - pts(:, 2)).^2));
end
function ptCloud = cropPointCloudFromROI(ptCloud, roi)
ind = findPointsInROI(ptCloud, roi);
if ind
flag = true(ptCloud.Count, 1);
flag(ind) = false;
outliers = find(flag);
ptCloud = select(ptCloud, outliers);
end
end
function tf = validateVerticalBinResolution(value)
validateattributes(value, {'single', 'double'}, {'real', 'nonsparse', 'vector', 'nonnan', 'finite', 'numel', 1}, mfilename, 'verticalBinResolution');
tf = true;
end
function tf = validateHorizontalBinResolution(value)
validateattributes(value, {'single', 'double'}, {'real', 'nonsparse', 'vector', 'nonnan', 'finite', 'numel', 1}, mfilename, 'horizontalBinResolution');
tf = true;
end
function tf = validateLaneWidth(value)
validateattributes(value, {'single', 'double'}, {'real', 'nonsparse', 'vector', 'nonnan', 'finite', 'numel', 1}, mfilename, 'laneWidth');
tf = true;
end
function tf = validateHistogramBinResolution(value)
validateattributes(value, {'single', 'double'}, {'real', 'nonsparse', 'vector', 'nonnan', 'finite', 'numel', 1}, mfilename, 'histogramBinResolution');
tf = true;
end
function tf = isOrganised(pc)
if isequal(ndims(pc.Location), 3)
tf = true;
else
tf = false;
end
end
end
end