Мультилоцируйте калибровку

Этот пример использует:

В этом примере показано, как калибровать несколько 3-D датчиков лидара, смонтированных на транспортном средстве, чтобы оценить относительное преобразование между ними. Традиционные методы, такие как основанная на маркере регистрация, затрудняют, когда датчики лидара имеют незначительное перекрытие между своими полями зрения (FOVs). Калибровка также становится более трудной как количество увеличений датчиков лидара. Этот пример демонстрирует использование траекторий отдельных датчиков лидара, чтобы оценить преобразование между ними. Этот метод калибровки также известен как зрительно-моторную калибровку.

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

Этот пример использует сгенерированное использование синтетических входных данных Нереального Engine® Epic Games®. figute обувь настройка датчиков, смонтированных на транспортном средстве.

Загрузите траекторию транспортного средства

Сгенерированные данные симулируют транспортное средство на предопределенной траектории в городской дорожной установке. Для получения дополнительной информации о том, как в интерактивном режиме выбрать последовательность waypoints от сцены и сгенерировать траектории транспортного средства, смотрите Выбрать пример Waypoints for Unreal Engine Simulation (Automated Driving Toolbox). Используйте helperShowSceneImage функция помощника, чтобы визуализировать путь транспортное средство следует при сборе данных.

% Load reference path for recorded drive segment
xData = load("refPosesX.mat");
yData = load("refPosesY.mat");
yawData = load("refPosesT.mat");

% Set up workspace variables used by model
refPosesX = xData.refPosesX;
refPosesY = yData.refPosesY;
refPosesT = yawData.refPosesT;

if ~ispc
    error(['3D Simulation is only supported on Microsoft', ...
        char(174),' Windows',char(174),'.']);
end

sceneName = "VirtualMCity";
hScene = figure;
helperShowSceneImage(sceneName)
hold on
scatter(refPosesX(:,2),refPosesY(:,2),7,"filled")
% Adjust axes limits
xlim([-50 100])
ylim([-50 75])

Запишите синтетические данные

MultiLidarSimulation Модель Simulink сконфигурирована для Виртуального Макити (Automated Driving Toolbox) 3-D среда с помощью блока Simulation 3D Scene Configuration (Automated Driving Toolbox). Транспортное средство тележки с кузовом коробчатой формы типа сконфигурировано в сцене с помощью Симуляции 3D Транспортное средство с блоком Ground Following (Automated Driving Toolbox). Транспортное средство имеет два датчика лидара, смонтированные на нем с помощью блока Simulation 3D Lidar (Automated Driving Toolbox). Два лидара смонтированы таким образом, что один датчик лидара смонтирован в переднем бампере и другом в заднем бампере. Монтирующееся положение датчика лидара может быть отрегулировано с помощью вкладки Mounting в блоке симуляции.

modelName = "MultiLidarSimulation";
open_system(modelName)

Модель записывает синтетические данные о лидаре и сохраняет их в рабочую область.

% Update simulation stop time to end when reference path is completed
simStopTime = refPosesX(end,1);
set_param(gcs,StopTime=num2str(simStopTime));

% Run the simulation
simOut = sim(modelName);

Извлеките одометрию лидара

Существует несколько методов одновременной локализации и картографии (SLAM), которые оценивают одометрию из данных о лидаре путем регистрации последовательных систем координат облака точек. Можно далее оптимизировать относительное преобразование между системами координат посредством обнаружения закрытия цикла. Для получения дополнительной информации о том, как сгенерировать траекторию движения с помощью ОСНОВАННОГО НА NDT регистрационного метода, смотрите Лидар Проекта Алгоритм SLAM Используя Нереальную Среду симуляции Engine (Automated Driving Toolbox) пример. В данном примере используйте helperExtractLidarOdometry функция помощника, чтобы сгенерировать траекторию движения как pcviewset возразите против симуляции выходу simOut.

% Front lidar translation and rotation
frontLidarTranslations = simOut.lidarLocation1.signals.values;
frontLidarRotations = simOut.lidarRotation1.signals.values;

% Back lidar translation and rotation
backLidarTranslations = simOut.lidarLocation2.signals.values;
backLidarRotations = simOut.lidarRotation2.signals.values;

% Extract point clouds from the simulation output
[frontLidarPtCloudArr,backLidarPtCloudArr] = helperExtractPointCloud(simOut);

% Extract lidar motion trajectories
frontLidarVset = helperExtractLidarOdometry(frontLidarTranslations,frontLidarRotations, ...
    frontLidarPtCloudArr);
backLidarVset = helperExtractLidarOdometry(backLidarTranslations,backLidarRotations, ...
    backLidarPtCloudArr);

helperVisualizeLidarOdometry функция помощника визуализирует накопленную карту облака точек с траекторией движения, наложенной на нем.

% Extract absolute poses of lidar sensor
frontLidarAbsPos = frontLidarVset.Views.AbsolutePose;
backLidarAbsPos  = backLidarVset.Views.AbsolutePose;

% Visualize front lidar point cloud map and trajectory
figure
plot(frontLidarVset)
hold on
plot(backLidarVset)
legend({'Front Lidar Trajectory','Back Lidar Trajectory'})
title("Lidar Trajectory")
view(2)

Траектории двух датчиков лидара, кажется, смещены 180 градусами. Это вызвано тем, что датчики лидара сконфигурированы, стоя в противоположных направлениях в модели Simulink.

Выровняйте траекторию лидара

Общим основанным на регистрации методам, с помощью облаков точек, часто не удается калибровать датчики лидара с неналожением или полями зрения незначительного перекрытия, потому что им недостает достаточных соответствующих функций. Чтобы преодолеть эту проблему, используйте движение транспортного средства для регистрации. Из-за твердой природы транспортного средства и датчиков, смонтированных на нем, движение каждого датчика коррелирует к относительному преобразованию между датчиками. Чтобы извлечь это относительное преобразование, сформулируйте решение выровнять траекторию лидара как зрительно-моторную калибровку, которая включает решение уравнения $AX = XB$ , где $Α$ и $Β$ последовательные положения этих двух датчиков, $Α$ и $Β$ . Можно далее разложить это уравнение на его компоненты вращения и перевода.

$R_{a_{k - 1}}^{a_{k}} * R_{b}^{a} = R_{b}^{a} * R_{b_{k - 1}}^{b_{k}}$

$R_{a_{k - 1}}^{a_{k}} * t_{b}^{a} + t_{a_{k - 1}}^{a_{k}} = R_{b} * t_{a}$

$R_{a_{k - 1}}^{a_{k}}$ , $t_{a_{k - 1}}^{a_{k}}$ компоненты вращения и перевода датчика $Α$ от метки времени $k - 1$ к $k$ . $R_{b}^{a}$ , $t_{b}^{a}$ компоненты вращения и перевода датчика $Α$ относительно датчика $Β$ . Этот рисунок показывает отношение между относительным преобразованием и последовательными положениями между этими двумя датчиками. $T_{a_{k - 1}}^{a_{k}}$ , $T_{b_{k - 1}}^{b_{k}}$ общее преобразование датчиков $Α,$ $Β$ и $T_{b}^{a}$ относительное преобразование.

Существует несколько способов решить уравнения для вращения и перевода [1]. Используйте helperEstimateHandEyeTransformation функция помощника, присоединенная как вспомогательный файл к этому примеру, чтобы оценить начальное преобразование между двумя датчиками лидара как rigid3d объект. Чтобы извлечь компонент вращения уравнения, функция преобразует матрицы вращения в форму кватерниона, реструктурированную как линейная система. Функция находит решение закрытой формы этой линейной системы с помощью сингулярного разложения [2].

tformInit = helperEstimateHandEyeTransformation(backLidarAbsPos, frontLidarAbsPos);

Улучшение преобразования

Чтобы далее совершенствовать преобразование, используйте основанный на регистрации метод. Введите перевод каждого датчика лидара от их соответствующих траекторий до регистрации. Используйте helperExtractPosFromTform функция помощника, чтобы преобразовать траектории датчиков в showPointCloud объекты. Для регистрации используйте pcregistericp функция с расчетным компонентом вращения tformInit как ваша начальная буква transformation.

% Extract the translation of each sensor in the form of a point cloud object
frontLidarTrans = helperExtractPosFromTform(frontLidarAbsPos);
backLidarTrans = helperExtractPosFromTform(backLidarAbsPos);

% Register the trajectories of the two sensors
tformRefine = pcregistericp(backLidarTrans,frontLidarTrans, ...
    "InitialTransform",tformInit,Metric="pointToPoint");

Обратите внимание на то, что точность калибровки зависит от того, как точно вы оцениваете движение каждого датчика. Чтобы упростить расчет, оценка движения для транспортного средства принимает, что наземная плоскость является плоской. Из-за этого предположения оценка теряет одну степень свободы вдоль оси Z. Можно оценить преобразование вдоль оси Z при помощи наземного метода обнаружения плоскости [3]. Используйте pcfitplane функционируйте, чтобы оценить наземную плоскость от облаков точек двух датчиков лидара. Функция оценивает высоту каждого датчика от обнаруженных наземных плоскостей двух датчиков лидара. Используйте helperExtractPointCloud функция помощника, чтобы извлечь pointCloud объектный массив от симуляции выход simOut.

% Maximum allowed distance between the ground plane and inliers
maxDist = 0.8;
% Reference vector for ground plane
refVecctor = [0 0 1];
% Fit plane on the a single point cloud frame
frame = 2;
frontPtCloud = frontLidarPtCloudArr(2);
backPtCloud = backLidarPtCloudArr(2);

[~,frontLidarInliers,~] = pcfitplane(frontPtCloud,maxDist,refVecctor);
[~,backLidarInliers,~] = pcfitplane(backPtCloud,maxDist,refVecctor);

% Extract relative translation between Z-axis
frontGroundPlane = select(frontPtCloud,frontLidarInliers);
backGroundPlane = select(backPtCloud,backLidarInliers );

frontGroundPts = frontGroundPlane.Location;
backGroundPlane = backGroundPlane.Location;

% Compute the difference between mean values of the extracted ground planes
zRel = mean(frontGroundPts(:,3)) - mean(backGroundPlane(:,3));

% Update the initial transformation with the estimated relative translation
% in the Z-axis
tformRefine.Translation(3) = zRel;

Плавьте облако точек

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

Используйте helperVisualizedFusedPtCloud функция помощника, чтобы плавить облака точек от двух датчиков лидара, наложенных со сплавленной траекторией после калибровки. Из сплавленной карты облака точек можно визуально вывести точность калибровки.

helperVisualizedFusedPtCloud(backLidarVset,frontLidarVset,tformRefine)

Результаты

Точность калибровки измеряется относительно преобразования основной истины, полученного из монтирующегося местоположения датчиков. Внедорожник (Vehicle Dynamics Blockset) страница документации предоставляет подробную информацию монтирующегося положения двух датчиков лидара. Относительное преобразование между двумя датчиками лидара загружается от gTruth.mat файл.

gt = load('gTruth.mat');
tformGt = gt.gTruth;

% Compute the translation error along the x-, y-, and z-axes
transError = tformRefine.Translation - tformGt.Translation;
fprintf("Translation error along x in meters: %d",transError(1));

Translation error along x in meters: 8.913606e-03

fprintf("Translation error along y in meters: %d",transError(2));

Translation error along y in meters: 6.720094e-03

fprintf("Translation error along z in meters: %d",transError(3));

Translation error along z in meters: 2.734658e-02

% Compute the translation error along the x-, y-, and z-axes
rEst = rad2deg(rotm2eul(tformRefine.Rotation));
rGt  = rad2deg(rotm2eul(tformGt.Rotation));
rotError = rEst - rGt;
fprintf("Rotation error along x in degrees: %d",rotError(3));

Rotation error along x in degrees: -4.509040e-04

fprintf("Rotation error along y in degrees: %d",rotError(2));

Rotation error along y in degrees: 2.201822e-05

fprintf("Rotation error along z in degrees: %d",rotError(1));

Rotation error along z in degrees: 2.545250e-02

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

helperExtractPointCloud извлекает массив pointCloud объекты от симуляции выводятся.

function [ptCloudArr1,ptCloudArr2] = helperExtractPointCloud(simOut)

% Extract signal
ptCloudData1 = simOut.ptCloudData1.signals.values;
ptCloudData2 = simOut.ptCloudData2.signals.values;

numFrames = size(ptCloudData1,4);
% Create a pointCloud array
ptCloudArr1 = pointCloud.empty(0,numFrames);
ptCloudArr2 = pointCloud.empty(0,numFrames);

for n = 1:size(ptCloudData1,4)
    ptCloudArr1(n) = pointCloud(ptCloudData1(:,:,:,n));
    ptCloudArr2(n) = pointCloud(ptCloudData2(:,:,:,n));
end
end

helperExtractLidarOdometry извлекает общее преобразование датчиков.

function vSet = helperExtractLidarOdometry(location,theta,ptCloud)

numFrames = size(location, 3);
vSet = pcviewset;
tformRigidAbs = rigid3d;
yaw = theta(:,3,1);
rot = [cos(yaw) sin(yaw) 0; ...
      -sin(yaw) cos(yaw) 0; ...
          0       0      1];
% Use first frame as reference frame
tformOrigin = rigid3d(rot,location(:,:,1));
vSet = addView(vSet,1,tformRigidAbs,PointCloud=ptCloud(1));
for i = 2:numFrames
    yawCurr = theta(:,3,i);
    rotatCurr = [cos(yawCurr) sin(yawCurr) 0; ...
                -sin(yawCurr) cos(yawCurr) 0; ...
                      0            0       1];
    transCurr = location(:,:,i);
    tformCurr = rigid3d(rotatCurr,transCurr);
    % Absolute pose
    tformRigidAbs(i) = rigid3d(tformCurr.T * tformOrigin.invert.T);
    vSet = addView(vSet,i,tformRigidAbs(i),PointCloud=ptCloud(i));
    % Transform between frame k-1 and k
    relPose = rigid3d(tformRigidAbs(i-1).T * tformRigidAbs(i).invert.T);
    vSet = addConnection(vSet,i-1,i,relPose);
end
end

helperVisualizedFusedPtCloud визуализирует карту облака точек из сплава двух датчиков лидара.

function helperVisualizedFusedPtCloud(movingVset,baseVset,tform)
hFig = figure(Name="Point Cloud Fusion", ...
    NumberTitle="off");

ax = axes(Parent= hFig);
% Create a scatter object for map points
scatterPtCloudBase = scatter3(ax,NaN,NaN,NaN, ...
    2,"magenta","filled");
hold(ax, 'on');
scatterPtCloudMoving = scatter3(ax,NaN,NaN,NaN, ...
    2,"green","filled");
scatterMap = scatter3(ax,NaN,NaN,NaN, ...
    5,"filled");

% Create a scatter object for relative positions
positionMarkerSize = 5;
scatterTrajectoryBase = scatter3(ax,NaN,NaN,NaN, ...
    positionMarkerSize,"magenta","filled");
scatterTrajectoryMoving = scatter3(ax,NaN,NaN,NaN, ...
    positionMarkerSize,"green","filled");
hold(ax,"off");

% Set background color
ax.Color = "k";
ax.Parent.Color = "k";

% Set labels
xlabel( ax,"X (m)")
ylabel( ax,"Y (m)")

% Set grid colors
ax.GridColor = "w";
ax.XColor = "w";
ax.YColor = "w";

% Set aspect ratio for axes
axis( ax,"equal")
xlim(ax, [-30 100]);
ylim(ax, [-80 40]);
title(ax,"Lidar Point Cloud Map Building",Color=[1 1 1])

ptCloudsMoving  = movingVset.Views.PointCloud;
absPoseMoving = movingVset.Views.AbsolutePose;

ptCloudsBase  = baseVset.Views.PointCloud;
absPoseBase = baseVset.Views.AbsolutePose;

numFrames = numel(ptCloudsMoving);

% Extract relative positions from the absolute poses
relPositionsMoving = arrayfun(@(poseTform) transformPointsForward(poseTform, ...
    [0 0 0]),absPoseMoving,UniformOutput=false);
relPositionsMoving = vertcat(relPositionsMoving{:});

relPositionsBase = arrayfun(@(poseTform) transformPointsForward(poseTform, ...
    [0 0 0]),absPoseBase,UniformOutput=false);
relPositionsBase = vertcat(relPositionsBase{:});

set(scatterTrajectoryBase,"XData",relPositionsMoving(1,1),"YData", ...
    relPositionsMoving(1,2),"ZData",relPositionsMoving(1,3));
set(scatterTrajectoryMoving,"XData",relPositionsBase(1,1),"YData", ...
    relPositionsBase(1,2),"ZData",relPositionsBase(1,3));
% Set legend
legend(ax, {'Front Lidar','Back Lidar'}, ...
    Location="southwest",TextColor="w")
skipFrames = 5;
for n = 2:skipFrames:numFrames
    pc1 = pctransform(removeInvalidPoints(ptCloudsMoving(n)),absPoseMoving(n));
    pc2 = pctransform(removeInvalidPoints(ptCloudsBase(n)),absPoseBase(n));
    % Transform moving point cloud to the base
    pc1 = pctransform(pc1,tform);

    % Create a point cloud map and merge point clouds from the sensors
    baseMap = pcalign(ptCloudsBase(1:n),absPoseBase(1:n),1);
    movingMap = pcalign(ptCloudsMoving(1:n),absPoseMoving(1:n),1);

    movingMap = pctransform(movingMap,tform);
    map = pcmerge(baseMap,movingMap,0.1);

    % Transform the position of the moving sensor to the base
    xyzTransformed = [relPositionsMoving(1:n,1),relPositionsMoving(1:n,2), ...
        relPositionsMoving(1:n,3)]*tform.Rotation + tform.Translation;

    % Plot current point cloud of individual sensor with respect to the ego
    % vehicle
    set(scatterPtCloudBase,"XData",pc2.Location(:,1),"YData", ...
        pc2.Location(:,2),"ZData",pc2.Location(:,3));
    set(scatterPtCloudMoving,"XData",pc1.Location(:,1),"YData", ...
        pc1.Location(:,2),"ZData",pc1.Location(:,3))

    % Plot fused point cloud map
    set(scatterMap,"XData", map.Location(:,1),"YData", ...
        map.Location(:,2),"ZData", map.Location(:,3),"CData", map.Location(:,3));

    % Plot trajectory
    set(scatterTrajectoryBase,"XData",relPositionsBase(1:n,1),"YData", ...
        relPositionsBase(1:n,2),"ZData",relPositionsBase(1:n,3));
    set(scatterTrajectoryMoving,"XData",xyzTransformed(:,1),"YData", ...
        xyzTransformed(:,2),"ZData",xyzTransformed(:,3));

    % Draw ego vehicle assuming the dimensions of a sports utility vehicle
    eul = rotm2eul(absPoseBase(n).Rotation');
    theta = rad2deg(eul);
    t = xyzTransformed(end,:) + [4.774 0 0]/2*(absPoseBase(n).Rotation);
    pos = [t 4.774 2.167 1.774 theta(2) theta(3) theta(1)];
    showShape("cuboid",pos,Color="yellow",Parent=ax,Opacity=0.9);
    view(ax,2)
    drawnow limitrate
end
end

helperExtractPosFromTform преобразует перевод от положения до pointCloud объект.

function ptCloud = helperExtractPosFromTform(pose)
numFrames = numel(pose);
location = zeros(numFrames,3);
for i = 1:numFrames
    location(i,:) = pose(i).Translation;
end
ptCloud = pointCloud(location);
end

Ссылки

[1] Шах, Mili, Роджер Д. Истмэн и Тсай Хун. ‘Обзор Калибровочных Методов Датчика Робота для Оценки Систем Восприятия. В Продолжениях Семинара по показателям производительности для Интеллектуальных Систем - PerMIS ’12, 15. Колледж-Парк, Мэриленд: Нажатие ACM, 2012. https://doi.org/10.1145/2393091.2393095.

[2] Трубочка из теста, Джек К. К. и М. Камель. "Находя Положение и Ориентацию Датчика на Манипуляторе Робота Используя Кватернионы". Международный журнал Исследования Робототехники 10, № 3 (июнь 1991): 240–54. https://doi.org/10.1177/027836499101000305.

[3] Цзяо, Jianhao, Янг Ю, Цинхай Ляо, Хэоянг Е, Жуй Фань и Мин Лю. ‘Автоматическая Калибровка Нескольких 3D LiDARs в Городских Средах’. На 2019 Международных конференциях IEEE/RSJ по вопросам Интеллектуальных Роботов и Систем (IROS), 15–20. Макао, Китай: IEEE, 2019. https://doi.org/10.1109/IROS40897.2019.8967797.

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