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

Скрипт Open Live Script

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

Введение

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

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

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

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

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

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

Данные о лидаре, используемые в этом примере, были собраны с помощью Изгнания датчик лидара канала 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). helperComputeHistogram функция помощника создает гистограмму точек интенсивности. Управляйте количеством интервалов для гистограммы путем определения разрешения интервала.

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) направление. helperDetectLanes функция помощника обнаруживает точки маршрута и визуализирует скользящие интервалы. Последовательные интервалы скользят вдоль Оси 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 функция помощника также вычисляет, подходящая ошибка. Обновите маршруты, имеющие ошибочные точки с новым полиномом. Обновите этот полином путем сдвига его вдоль Оси 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 класс помощника. Этот класс помощника реализует все предыдущие шаги, и также выполняет дополнительную предварительную обработку, чтобы удалить транспортные средства из облака точек. Это гарантирует, что обнаруженные наземные точки являются плоскими, и плоская модель точна. Метод класса 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')

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

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

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

helperLoadData функция помощника загружает набор данных лидара в рабочее пространство 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 функция помощника вычисляет начальные точки окна поиска с помощью обнаруженного 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 функция помощника соответствует, основанный на 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 функция помощника вычисляет гистограмму значений интенсивности облаков точек.

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 функция помощника извлекает 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, Фарук, Fawzi Nashashibi, Гаят Эль-Хэдж-Шхэйд и Мари-Энн Миттет. "Обнаружение отмечающего Маршрута LIDAR-Based для Расположения Транспортного средства в Карту HD". На 2 018 21-х Международных конференциях по вопросам Интеллектуальных Систем Транспортировки (ITSC), 2209-14. Мауи: IEEE, 2018. https://doi.org/10.1109/ITSC.2018.8569951.

[2] Thuy, Майкл и Фернандо Леон. "Обнаружение маршрута и Отслеживающий На основе Данных о Лидаре". Метрология и Системы измерения 17, № 3 (2010): 311-322. https://doi.org/10.2478/v10178-010-0027-3.

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