Обнаружение маршрута в 3-D Lidar Облако Точек

Открыть Live Script

Этот пример показов, как обнаружить полосы движения в лидар облаков точек. Можно использовать значения интенсивности, возвращенные из облаков лидарных точек, для обнаружения полос автомобиля , оборудованного датчиком. Можно дополнительно улучшить обнаружение маршрута с помощью алгоритма аппроксимирования кривыми и отслеживания параметров кривой. Обнаружение лидара маршрута позволяет вам создавать сложные рабочие процессы, такие как помощь по хранению маршрута, предупреждение о выезде из маршрута и адаптивный круиз-контроль для автономного управления автомобилем. Тестовое транспортное средство собирает данные лидара с помощью датчика лидара, установленного на его крыше.

Введение

Обнаружение маршрута в лидаре включает обнаружение непосредственных левой и правой дорожек, также известных как автомобиль , оборудованный датчиком дорожки, относительно датчика лидара. Он включает следующие шаги:

Необходимая область
Сегментация наземной плоскости
Обнаружение пиковой интенсивности
Обнаружение маршрута с помощью поиска в окне
Параболический полиномиальный подбор кривой
Параллельные подборы кривой маршрута
Отслеживание маршрута

Эта блок-схема дает обзор рабочего процесса, представленного в этом примере.

Преимуществами использования лидарных данных для обнаружения маршрута являются:

Облака точек Лидара дают лучшее 3-D представление поверхности дороги, чем данные изображений, таким образом уменьшая необходимые параметры калибровки, чтобы найти вид птичьего полета.
Лидар является более устойчивым к неблагоприятным климатическим условиям, чем обнаружение на основе изображений.
Данные Лидара имеют сантиметровый уровень точности, приводящий к точной локализации маршрута.

Загрузка и подготовка набора данных лидар

Данные лидара, используемые в этом примере, были собраны с помощью датчика лидара канала Ouster OS1-64, создавая облака точек с высоким разрешением. Этот набор данных содержит облака точек, сохраненные как массив ячеек pointCloud объект. Каждое 3-D облако точек состоит из местоположений XYZ вместе с информацией об интенсивности.

Примечание: Время загрузки данных зависит от вашего подключения к Интернету. MATLAB будет временно не реагировать во время выполнения этого блока кода.

% Download lidar data
lidarData = helperGetDataset;

Выбор первой системы координат набора данных для дальнейшей обработки.

% Select first frame 
ptCloud = lidarData{1};

% Visualize input point cloud
figure
pcshow(ptCloud)
title('Input Lidar Point Cloud')
axis([0 50 -15 15 -5 5])
view([-42 35])

Предварительная обработка

Чтобы оценить точки маршрута, сначала обработайте облака лидарных точек. Предварительная обработка включает следующие шаги:

Необходимая область
Сегментация наземной плоскости

% Define ROI in meters
xlim = [5 55];
ylim = [-3 3];
zlim = [-4 1];
roi = [xlim ylim zlim];

% Crop point cloud using ROI
indices = findPointsInROI(ptCloud,roi);
croppedPtCloud = select(ptCloud,indices);

% Remove ground plane
maxDistance = 0.1;
referenceVector = [0 0 1];
[model,inliers,outliers] = pcfitplane(croppedPtCloud,maxDistance,referenceVector);
groundPts = select(croppedPtCloud,inliers);

figure
pcshow(groundPts)
title('Ground Plane')
view(3)

Обнаружение точки маршрута

Обнаружите точки маршрута с помощью поиска в скользящем окне, где начальные оценки для скользящих окон сделаны с помощью гистограммы на основе интенсивности.

Обнаружение точек маршрута состоит, в основном, из этих двух шагов:

Обнаружение пиковой интенсивности
Поиск окна

Обнаружение пиковой интенсивности

Точки маршрута в облаке точек лидара имеют отчетливое распределение интенсивности. Обычно эти интенсивности занимают верхнюю область в распределении гистограмм и появляются как высокий peaks. Вычислите гистограмму интенсивности от обнаруженной плоскости земли вдоль оси автомобиль , оборудованный датчиком (положительная ось X). The helperComputeHistogram Функция helper создает гистограмму точек интенсивности. Управление количеством интервалов для гистограммы путем определения разрешения интервала.

histBinResolution = 0.2;
[histVal,yvals] = helperComputeHistogram(groundPts,histBinResolution);

figure
plot(yvals,histVal,'--k')
set(gca,'XDir','reverse')
hold on

Получите peaks в гистограмме при помощи helperfindpeaks вспомогательная функция. Дальнейшая фильтрация возможных точек маршрута на основе ширины маршрута с помощью helperInitialWindow вспомогательная функция.

[peaks,locs] = helperfindpeaks(histVal);
startYs = yvals(locs);

laneWidth = 4;
[startLanePoints,detectedPeaks] = helperInitialWindow(startYs,peaks,laneWidth);

plot(startYs,peaks,'*r')
plot(startLanePoints,detectedPeaks,'og')
legend('Histogram','Peak','Detected Peaks','Location','North')
title('Peak Detection')
hold off

Поиск окна

Поиск окна используется, чтобы обнаружить точки маршрута путем скольжения окон вдоль направления кривизны маршрута. Поиск окна состоит из двух шагов:

Инициализация окна
Скользящее окно

Инициализация окна

Обнаруженный peaks помогают в инициализации окна поиска. Инициализируйте окна поиска как несколько интервалов с красным и синим цветами для левой и правой линий соответственно.

vBinRes = 1;
hBinRes = 0.8;
numVerticalBins = ceil((groundPts.XLimits(2) - groundPts.XLimits(1))/vBinRes);
laneStartX = linspace(groundPts.XLimits(1),groundPts.XLimits(2),numVerticalBins);

% Display bin windows
figure
pcshow(groundPts)
view(2)
helperDisplayBins(laneStartX,startLanePoints(1),hBinRes/2,groundPts,'red');
helperDisplayBins(laneStartX,startLanePoints(2),hBinRes/2,groundPts,'blue');
title('Initialized Sliding Windows')

Скользящее окно

Скользящие интервалы инициализируются из положения интервала и итеративно перемещаются вдоль направления автомобиль , оборудованный датчиком (положительная ось X). The helperDetectLanes Функция helper обнаруживает точки маршрута и визуализирует скользящие интервалы. Последовательные интервалы скользят вдоль оси Y на основе значений интенсивности внутри предыдущего интервала.

В областях, где точки маршрута отсутствуют, функция предсказывает скользящие интервалы с помощью полинома второй степени. Это условие обычно возникает, когда движущийся объект пересекает полосы движения. Скользящие интервалы желтые, а интервалы, которые предсказаны с помощью полинома, зеленые.

display = true;
lanes = helperDetectLanes(groundPts,hBinRes, ...
    numVerticalBins,startLanePoints,display);

% Plot final lane points
lane1 = lanes{1};
lane2 = lanes{2};

figure
pcshow(groundPts)
title('Detected Lane Points')
hold on
p1 = plot3(lane1(:, 1),lane1(:, 2),lane1(:, 3),'*y');
p2 = plot3(lane2(:, 1),lane2(:, 2),lane2(:, 3),'*r');
hold off
view(2)
lgnd = legend([p1 p2],{'Left Lane Points','Right Lane Points'});
set(lgnd,'color','White','Location','southoutside')

Переулок Подбора кривой

Подбор кривой маршрута включает оценку полиномиальной кривой на обнаруженных точках маршрута. Эти полиномы используются наряду с ограничением параллельного маршрута для подбора кривой маршрута.

Параболический полиномиальный подбор кривой

Полином установлен на точках X-Y, используя полином 2 степени, представленный как $a x^{2} + bx + c$ , где a, b и c являются полиномиальными параметрами. Для выполнения аппроксимирования кривыми используйте helperFitPolynomial вспомогательная функция, которая также обрабатывает выбросы с помощью алгоритма консенсуса случайной выборки (RANSAC). Чтобы оценить 3-D точки маршрута, извлеките параметры из модели плоскости, созданной на шаге предварительной обработки. Модель плоскости представлена как $ax + by + cz + d = 0$ , где Z-координата неизвестна. Оцените координату Z путем подстановки координат X - и Y в плоском уравнении.

[P1,error1] = helperFitPolynomial(lane1(:,1:2),2,0.1);
[P2,error2] = helperFitPolynomial(lane2(:,1:2),2,0.1);

xval = linspace(5,40,80);
yval1 = polyval(P1,xval);
yval2 = polyval(P2,xval);

% Z-coordinate estimation
modelParams = model.Parameters;
zWorld1 = (-modelParams(1)*xval - modelParams(2)*yval1 - modelParams(4))/modelParams(3);
zWorld2 = (-modelParams(1)*xval - modelParams(2)*yval2 - modelParams(4))/modelParams(3);

% Visualize fitted lane
figure
pcshow(croppedPtCloud)
title('Fitted Lane Polynomial')
hold on
p1 = plot3(xval,yval1,zWorld1,'y','LineWidth',0.2);
p2 = plot3(xval,yval2,zWorld2,'r','LineWidth',0.2);
lgnd = legend([p1 p2],{'Left Lane Points','Right Lane Points'});
set(lgnd,'color','White','Location','southoutside')
view(2)
hold off

Параллельные Подборы кривой маршрута

Полосы обычно параллельны друг другу вдоль дороги. Чтобы сделать фитинг маршрута устойчивым, используйте это параллельное ограничение. При подгонке полиномов helperFitPolynomial helper функция также вычисляет, аппроксимация ошибки. Обновите полосы движения, имеющие ошибочные точки, новым полиномом. Обновите этот полином путем сдвига его вдоль оси Y.

lane3d1 = [xval' yval1' zWorld1'];
lane3d2 = [xval' yval2' zWorld2'];

% Shift the polynomial with a high score along the Y-axis towards
% the polynomial with a low score
if error1 > error2
    lanePolynomial = P2;
    if lane3d1(1,2) > 0
        lanePolynomial(3) = lane3d2(1,2) + laneWidth;
    else
        lanePolynomial(3) = lane3d2(1,2) - laneWidth;
    end
    lane3d1(:,2) = polyval(lanePolynomial,lane3d1(:,1));
    lanePolynomials = [lanePolynomial; P2];
else
    lanePolynomial = P1;
    if lane3d2(1,2) > 0
        lanePolynomial(3) = lane3d1(1,2) + laneWidth;
    else
        lanePolynomial(3) = lane3d1(1,2) - laneWidth;
    end
    lane3d2(:,2) = polyval(lanePolynomial,lane3d2(:,1));
    lanePolynomials = [P1; lanePolynomial]
end

% Visualize lanes after parallel fitting
figure
pcshow(ptCloud)
axis([0 50 -15 15 -5 5])
hold on
p1 = plot3(lane3d1(:,1),lane3d1(:,2),lane3d1(:,3),'y','LineWidth',0.2);
p2 = plot3(lane3d2(:,1),lane3d2(:,2),lane3d2(:,3),'r','LineWidth',0.2);
view([-90 90])
title('Fitted Lanes')
lgnd = legend([p1 p2],{'Left Lane Points','Right Lane Points'});
set(lgnd,'color','White',"Location","southoutside")
hold off

Отслеживание маршрута

Отслеживание маршрута помогает в стабилизации кривизны маршрута, вызванной внезапными рывками и дрейфами. Эти изменения могут произойти из-за отсутствия точек маршрута, транспортных средств, движущихся по полосам, и ошибочного обнаружения маршрута. Отслеживание маршрута является двухэтапным процессом:

Проследите за полиномиальными параметрами маршрута $(a, b)$ для управления кривизной полинома.
Отслеживайте начальные точки, поступающие от пикового обнаружения. Этот параметр обозначается как $c$ в полиноме.

Эти параметры обновляются с помощью фильтра Калмана с постоянной моделью движения ускорения. Чтобы инициировать фильтр Калмана, используйте configureKalmanFilter.

% Initial values
curveInitialParameters = lanePolynomials(1,1:2);
driftInitialParameters = lanePolynomials(:,3)';
initialEstimateError = [1 1 1] * 1e-1;
motionNoise = [1 1 1] * 1e-7;
measurementNoise = 10;

% Configure Kalman filter
curveFilter = configureKalmanFilter('ConstantAcceleration', ...
    curveInitialParameters,initialEstimateError,motionNoise,measurementNoise);
driftFilter = configureKalmanFilter('ConstantAcceleration', ...
    driftInitialParameters,initialEstimateError,motionNoise,measurementNoise);

Цикл через данные

Проследите и обработайте данные лидара с помощью helperLaneDetector класс helper. Этот класс helper реализует все предыдущие шаги, а также выполняет дополнительную предварительную обработку, чтобы удалить транспортные средства из облака точек. Это гарантирует, что обнаруженные точки Земли являются плоскими, и модель плоскости является точной. Метод класса detectLanes обнаруживает и извлекает точки маршрута для левого и правого маршрута как двухэлементный массив ячеек, где первый элемент соответствует левой полосе и второй элемент - правой полосе.

% Initialize the random number generator
rng(2020)
numFrames = numel(lidarData);
detector = helperLaneDetector('ROI',[5 40 -3 3 -4 1]);

% Turn on display
player = pcplayer([0 50],[-15 15],[-5 5]);

drift = zeros(numFrames,1);
filteredDrift = zeros(numFrames,1);
curveSmoothness = zeros(numFrames,1);
filteredCurveSmoothness = zeros(numFrames,1);
for i = 1:numFrames
    ptCloud = lidarData{i};

    % Detect lanes
    detectLanes(detector,ptCloud);

    % Predict polynomial from Kalman filter
    predict(curveFilter);
    predict(driftFilter);

    % Correct polynomial using Kalman filter
    lanePolynomials = detector.LanePolynomial;
    drift(i) = mean(lanePolynomials(:,3));
    curveSmoothness(i) = mean(lanePolynomials(:,1));
    updatedCurveParams = correct(curveFilter,lanePolynomials(1,1:2));
    updatedDriftParams = correct(driftFilter,lanePolynomials(:,3)');

    % Update lane polynomials
    updatedLanePolynomials = [repmat(updatedCurveParams,[2 1]),updatedDriftParams'];

    % Estimate new lane points with updated polynomial
    lanes = updateLanePolynomial(detector,updatedLanePolynomials);
    filteredDrift(i) = mean(updatedLanePolynomials(:,3));
    filteredCurveSmoothness(i) = mean(updatedLanePolynomials(:,1));

    % Visualize lanes after parallel fitting
    ptCloud.Color = uint8(repmat([0 0 255],ptCloud.Count,1));
    lane3dPc1 = pointCloud(lanes{1});
    lane3dPc1.Color = uint8(repmat([255 0 0],lane3dPc1.Count,1));
    lanePc = pccat([ptCloud lane3dPc1]);
    lane3dPc2 = pointCloud(lanes{2});
    lane3dPc2.Color = uint8(repmat([255 255 0],lane3dPc2.Count,1));
    lanePc = pccat([lanePc lane3dPc2]);
    view(player,lanePc)
end

Результаты

Чтобы проанализировать результаты обнаружения маршрута, сравните их с отслеживаемыми полиномами маршрута путем построения графиков на рисунках. Каждый график сравнивает параметры с фильтром Калмана и без него. Первый график сравнивает дрейф полос вдоль оси Y, а второй график сравнивает плавность полиномов полос. Гладкость определяется как скорость изменения наклона кривой маршрута.

figure
plot(drift)
hold on
plot(filteredDrift)
hold off
title('Lane Drift Along Y-axis')
legend('Drift Values','Filtered Drift Values')

figure
plot(curveSmoothness)
hold on
plot(filteredCurveSmoothness)
hold off
title('Curve Smoothness')
legend('Curve Smoothness','Filtered Curve Smoothness')

Сводные данные

Этот пример показал вам, как обнаружить полосы на канале интенсивности облаков точек, поступающих с датчика лидара. Вы также научились аппроксимировать полином 2-D на обнаруженных точках маршрута и использовать модель наземной плоскости для оценки 3-D точек маршрута. Вы также использовали отслеживание фильтра Калмана для дальнейшего улучшения обнаружения маршрута.

Вспомогательные функции

The helperLoadData Функция helper загружает набор данных лидара в рабочее пространство MATLAB.

function reflidarData = helperGetDataset()
outputFolder = fullfile(tempdir,'WPI');
url = 'https://www.mathworks.com/supportfiles/lidar/data/WPI_LidarData.tar.gz';
lidarDataTarFile = fullfile(outputFolder,'WPI_LidarData.tar.gz');
if ~exist(lidarDataTarFile,'file')
    mkdir(outputFolder);
    disp('Downloading WPI Lidar driving data (760 MB)...');
    websave(lidarDataTarFile,url);
    untar(lidarDataTarFile,outputFolder);
end

% Check if tar.gz file is downloaded, but not uncompressed
if ~exist(fullfile(outputFolder,'WPI_LidarData.mat'),'file')
    untar(lidarDataTarFile,outputFolder);
end

% Load lidar data
load(fullfile(outputFolder,'WPI_LidarData.mat'), 'lidarData');

% Select region with a prominent intensity value
reflidarData = cell(300,1);
count = 1;
roi = [-50 50 -30 30 -inf inf];
for i = 81:380
    pc = lidarData{i};
    ind = findPointsInROI(pc,roi);
    reflidarData{count} = select(pc,ind);
    count = count + 1;
end
end

Этот helperInitialWindow Функция helper вычисляет начальные точки окна поиска, используя обнаруженный peaks гистограммы.

function [yval,detectedPeaks] = helperInitialWindow(yvals,peaks,laneWidth)
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(laneWidth - (leftLaneYs(i) - rightLaneYs(j)));
    end
end
[~,minIndex] = min(diff(:));
[row,col] = ind2sub(size(diff),minIndex);
yval = [leftLaneYs(row) rightLaneYs(col)];
detectedPeaks = [peaksLeft(row) peaksRight(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 - laneWidth) > 0.5
    if max(peaksLeft) > max(peaksRight)
        yval = [leftLaneYs(maxLeftInd) NaN];
        detectedPeaks = [peaksLeft(maxLeftInd) NaN];
    else
        yval = [NaN rightLaneYs(maxRightInd)];
        detectedPeaks = [NaN rightLaneYs(maxRightInd)];
    end
end
end

Этот helperFitPolynomial Функция helper подбирает полином на основе RANSAC к обнаруженным точкам маршрута и вычисляет счет соответствия.

function [P,score] = helperFitPolynomial(pts,degree,resolution)
P = fitPolynomialRANSAC(pts,degree,resolution);
ptsSquare = (polyval(P,pts(:,1)) - pts(:,2)).^2;
score =  sqrt(mean(ptsSquare));
end

Этот helperComputeHistogram Функция helper вычисляет гистограмму значений интенсивности облаков точек.

function [histVal,yvals] = helperComputeHistogram(ptCloud,histogramBinResolution)
numBins = ceil((ptCloud.YLimits(2) - ptCloud.YLimits(1))/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

Этот helperfindpeaks Функция helper извлекает peaks из значений гистограммы.

function [pkHistVal,pkIdx] = helperfindpeaks(histVal)
pkIdxTemp = (1:size(histVal,2))';
histValTemp = [NaN; histVal'; NaN];
tempIdx = (1:length(histValTemp)).';

% keep only the first of any adjacent pairs of equal values (including NaN)
yFinite = ~isnan(histValTemp);
iNeq = [1; 1 + find((histValTemp(1:end-1) ~= histValTemp(2:end)) & ...
    (yFinite(1:end-1) | yFinite(2:end)))];
tempIdx = tempIdx(iNeq);

% Take the sign of the first sample derivative
s = sign(diff(histValTemp(tempIdx)));

% Find local maxima
maxIdx = 1 + find(diff(s)<0);

% Index into the original index vector without the NaN bookend
pkIdx = tempIdx(maxIdx) - 1;

% Fetch the coordinates of the peak
pkHistVal = histVal(pkIdx);
pkIdx = pkIdxTemp(pkIdx)';
end

Ссылки

[1] Ghallabi, Farouk, Fawzi Nashashibi, Ghayath El-Haj-Shhhade, и Marie-Anne Mitte. «Обнаружение маркировки маршрута на основе LIDAR для позиционирования транспортного средства в HD-карте». В 2018 году 21-я Международная конференция по интеллектуальным транспортным системам (ITSC), 2209-14. Maui: IEEE, 2018. https://doi.org/10.1109/ITSC.2018.8569951.

[2] Туи, Михаэль и Фернандо Леон. «Обнаружение и отслеживание маршрута на основе данных лидар». Метрология и системы измерений 17, № 3 (2010): 311-322. https://doi.org/10.2478/v10178-010-0027-3.

Документация