Создайте карту из данных о лидаре

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

Обзор

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

В этом примере вы учитесь как:

  • Загрузите, исследуйте и визуализируйте зарегистрированные ведущие данные

  • Создайте карту с помощью сканов лидара

  • Улучшите карту с помощью показаний IMU

Загрузите и исследуйте записанные ведущие данные

Данные, используемые в этом примере, от этого репозитория GitHub® и представляют приблизительно 100 секунд лидара, GPS и данных IMU. Данные сохранены в форме MAT-файлов, каждый содержащий timetable. Загрузите MAT-файлы с репозитория и загрузите их в рабочую область MATLAB®.

Примечание: Эта загрузка может занять несколько минут.

baseDownloadURL = 'https://github.com/mathworks/udacity-self-driving-data-subset/raw/master/drive_segment_11_18_16/';
dataFolder      = fullfile(tempdir, 'drive_segment_11_18_16', filesep);
options         = weboptions('Timeout', Inf);

lidarFileName = dataFolder + "lidarPointClouds.mat";
imuFileName   = dataFolder + "imuOrientations.mat";
gpsFileName   = dataFolder + "gpsSequence.mat";

folderExists  = exist(dataFolder, 'dir');
matfilesExist = exist(lidarFileName, 'file') && exist(imuFileName, 'file') ...
    && exist(gpsFileName, 'file');

if ~folderExists
    mkdir(dataFolder);
end

if ~matfilesExist
    disp('Downloading lidarPointClouds.mat (613 MB)...')
    websave(lidarFileName, baseDownloadURL + "lidarPointClouds.mat", options);

    disp('Downloading imuOrientations.mat (1.2 MB)...')
    websave(imuFileName, baseDownloadURL + "imuOrientations.mat", options);

    disp('Downloading gpsSequence.mat (3 KB)...')
    websave(gpsFileName, baseDownloadURL + "gpsSequence.mat", options);
end

Во-первых, загрузите данные об облаке точек, сохраненные от лидара Velodyne® HDL32E. Каждый скан данных о лидаре хранится как 3-D облако точек с помощью pointCloud объект. Этот объект внутренне организует данные с помощью древовидной структуры данных K-d для более быстрого поиска. Метка времени, сопоставленная с каждым сканом лидара, зарегистрирована в Time переменная расписания.

% Load lidar data from MAT-file
data = load(lidarFileName);
lidarPointClouds = data.lidarPointClouds;

% Display first few rows of lidar data
head(lidarPointClouds)
ans =

  8×1 timetable

        Time            PointCloud   
    _____________    ________________

    23:46:10.5115    [1×1 pointCloud]
    23:46:10.6115    [1×1 pointCloud]
    23:46:10.7116    [1×1 pointCloud]
    23:46:10.8117    [1×1 pointCloud]
    23:46:10.9118    [1×1 pointCloud]
    23:46:11.0119    [1×1 pointCloud]
    23:46:11.1120    [1×1 pointCloud]
    23:46:11.2120    [1×1 pointCloud]

Загрузите данные о GPS из MAT-файла. Latitude, Longitude, и Altitude переменные timetable используются, чтобы сохранить географические координаты, зарегистрированные устройством GPS на транспортном средстве.

% Load GPS sequence from MAT-file
data = load(gpsFileName);
gpsSequence = data.gpsSequence;

% Display first few rows of GPS data
head(gpsSequence)
ans =

  8×3 timetable

        Time         Latitude    Longitude    Altitude
    _____________    ________    _________    ________

    23:46:11.4563      37.4       -122.11      -42.5  
    23:46:12.4563      37.4       -122.11      -42.5  
    23:46:13.4565      37.4       -122.11      -42.5  
    23:46:14.4455      37.4       -122.11      -42.5  
    23:46:15.4455      37.4       -122.11      -42.5  
    23:46:16.4567      37.4       -122.11      -42.5  
    23:46:17.4573      37.4       -122.11      -42.5  
    23:46:18.4656      37.4       -122.11      -42.5  

Загрузите данные IMU из MAT-файла. IMU обычно состоит из отдельных датчиков, которые сообщают информацию о движении транспортного средства. Они комбинируют несколько датчиков, включая акселерометры, гироскопы и магнитометры. Orientation переменная хранит ориентацию, о которой сообщают, датчика IMU. Об этих показаниях сообщают как кватернионы. Каждое чтение задано как вектор 1 на 4, содержащий четыре части кватерниона. Преобразуйте вектор 1 на 4 в quaternion объект.

% Load IMU recordings from MAT-file
data = load(imuFileName);
imuOrientations = data.imuOrientations;

% Convert IMU recordings to quaternion type
imuOrientations = convertvars(imuOrientations, 'Orientation', 'quaternion');

% Display first few rows of IMU data
head(imuOrientations)
ans =

  8×1 timetable

        Time           Orientation   
    _____________    ________________

    23:46:11.4570    [1×1 quaternion]
    23:46:11.4605    [1×1 quaternion]
    23:46:11.4620    [1×1 quaternion]
    23:46:11.4655    [1×1 quaternion]
    23:46:11.4670    [1×1 quaternion]
    23:46:11.4705    [1×1 quaternion]
    23:46:11.4720    [1×1 quaternion]
    23:46:11.4755    [1×1 quaternion]

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

lidarFrameDuration = median(diff(lidarPointClouds.Time));
gpsFrameDuration   = median(diff(gpsSequence.Time));
imuFrameDuration   = median(diff(imuOrientations.Time));

% Adjust display format to seconds
lidarFrameDuration.Format = 's';
gpsFrameDuration.Format   = 's';
imuFrameDuration.Format   = 's';

% Compute frame rates
lidarRate = 1/seconds(lidarFrameDuration);
gpsRate   = 1/seconds(gpsFrameDuration);
imuRate   = 1/seconds(imuFrameDuration);

% Display frame durations and rates
fprintf('Lidar: %s, %3.1f Hz\n', char(lidarFrameDuration), lidarRate);
fprintf('GPS  : %s, %3.1f Hz\n', char(gpsFrameDuration), gpsRate);
fprintf('IMU  : %s, %3.1f Hz\n', char(imuFrameDuration), imuRate);
Lidar: 0.10008 sec, 10.0 Hz
GPS  : 1.0001 sec, 1.0 Hz
IMU  : 0.002493 sec, 401.1 Hz

Датчик GPS является самым медленным, запускающийся на уровне близко к 1 Гц. Лидар является затем самым медленным, запускающийся на уровне близко к 10 Гц, сопровождаемым IMU на уровне почти 400 Гц.

Визуализируйте ведущие данные

Чтобы понять, что содержит сцена, визуализируйте записанные проигрыватели потоковой передачи использования данных. Чтобы визуализировать показания GPS, используйте geoplayer. Визуализировать показания лидара с помощью pcplayer.

% Create a geoplayer to visualize streaming geographic coordinates
latCenter = gpsSequence.Latitude(1);
lonCenter = gpsSequence.Longitude(1);
zoomLevel = 17;

gpsPlayer = geoplayer(latCenter, lonCenter, zoomLevel);

% Plot the full route
plotRoute(gpsPlayer, gpsSequence.Latitude, gpsSequence.Longitude);

% Determine limits for the player
xlimits = [-45 45]; % meters
ylimits = [-45 45];
zlimits = [-10 20];

% Create a pcplayer to visualize streaming point clouds from lidar sensor
lidarPlayer = pcplayer(xlimits, ylimits, zlimits);

% Customize player axes labels
xlabel(lidarPlayer.Axes, 'X (m)')
ylabel(lidarPlayer.Axes, 'Y (m)')
zlabel(lidarPlayer.Axes, 'Z (m)')

title(lidarPlayer.Axes, 'Lidar Sensor Data')

% Align players on screen
helperAlignPlayers({gpsPlayer, lidarPlayer});

% Outer loop over GPS readings (slower signal)
for g = 1 : height(gpsSequence)-1

    % Extract geographic coordinates from timetable
    latitude  = gpsSequence.Latitude(g);
    longitude = gpsSequence.Longitude(g);

    % Update current position in GPS display
    plotPosition(gpsPlayer, latitude, longitude);

    % Compute the time span between the current and next GPS reading
    timeSpan = timerange(gpsSequence.Time(g), gpsSequence.Time(g+1));

    % Extract the lidar frames recorded during this time span
    lidarFrames = lidarPointClouds(timeSpan, :);

    % Inner loop over lidar readings (faster signal)
    for l = 1 : height(lidarFrames)

        % Extract point cloud
        ptCloud = lidarFrames.PointCloud(l);

        % Update lidar display
        view(lidarPlayer, ptCloud);

        % Pause to slow down the display
        pause(0.01)
    end
end

Используйте записанные данные о лидаре, чтобы создать карту

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

% Hide players
hide(gpsPlayer)
hide(lidarPlayer)

% Select a frame of lidar data to demonstrate registration workflow
frameNum = 600;
ptCloud = lidarPointClouds.PointCloud(frameNum);

% Display and rotate ego view to show lidar data
helperVisualizeEgoView(ptCloud);

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

  1. Выровняйте сканы лидара: Выровняйте последовательные сканы лидара с помощью метода регистрации облака точек как алгоритм итеративной самой близкой точки (ICP) или алгоритм нормальных распределений преобразовывают (NDT). Смотрите pcregistericp и pcregisterndt для получения дополнительной информации о каждом алгоритме. Этот пример использует NDT, потому что это обычно более точно, особенно при рассмотрении вращений. pcregisterndt функция возвращает твердое преобразование, которое выравнивает движущееся облако точек относительно облака контрольной точки. Путем последовательного создания этих преобразований каждое облако точек преобразовывается назад к системе координат первого облака точек.

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

Запустите путем взятия двух облаков точек, соответствующих соседним сканам лидара. Чтобы ускорить обработку и накопить достаточно движения между сканами, используют каждый десятый скан.

skipFrames = 10;
frameNum   = 100;

fixed  = lidarPointClouds.PointCloud(frameNum);
moving = lidarPointClouds.PointCloud(frameNum + skipFrames);

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

Эти шаги описаны более подробно в Наземном Обнаружении Плоскости и Препятствия Используя пример Лидара. В этом примере, helperProcessPointCloud функция помощника выполняет эти шаги.

fixedProcessed  = helperProcessPointCloud(fixed);
movingProcessed = helperProcessPointCloud(moving);

Отобразите сырые данные и обработанные облака точек в виде сверху. Пурпурные точки были удалены во время обработки. Эти точки соответствуют наземной плоскости и автомобилю, оборудованному датчиком.

hFigFixed = figure;
pcshowpair(fixed, fixedProcessed)
view(2);                            % Adjust view to show top-view

helperMakeFigurePublishFriendly(hFigFixed);

% Downsample the point clouds prior to registration. Downsampling improves
% both registration accuracy and algorithm speed.
downsamplePercent = 0.1;
fixedDownsampled  = pcdownsample(fixedProcessed, 'random', downsamplePercent);
movingDownsampled = pcdownsample(movingProcessed, 'random', downsamplePercent);

После предварительной обработки облаков точек укажите их использующий NDT. Визуализируйте выравнивание до и после регистрации.

regGridStep = 5;
tform = pcregisterndt(movingDownsampled, fixedDownsampled, regGridStep);

movingReg = pctransform(movingProcessed, tform);

% Visualize alignment in top-view before and after registration
hFigAlign = figure;

subplot(121)
pcshowpair(movingProcessed, fixedProcessed)
title('Before Registration')
view(2)

subplot(122)
pcshowpair(movingReg, fixedProcessed)
title('After Registration')
view(2)

helperMakeFigurePublishFriendly(hFigAlign);

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

Затем объедините облака точек с помощью pcmerge.

mergeGridStep = 0.5;
ptCloudAccum = pcmerge(fixedProcessed, movingReg, mergeGridStep);

hFigAccum = figure;
pcshow(ptCloudAccum)
title('Accumulated Point Cloud')
view(2)

helperMakeFigurePublishFriendly(hFigAccum);

Теперь, когда конвейер обработки для одной пары облаков точек хорошо изучен, соедините это в цикле по целой последовательности записанных данных. helperLidarMapBuilder класс соединяет все это. updateMap метод класса берет в новом облаке точек и проходит шаги, детализированные ранее:

  • Обработка облака точек путем удаления наземной плоскости и автомобиля, оборудованного датчиком, использования processPointCloud метод.

  • Субдискретизация облака точек.

  • Оценка твердого преобразования, требуемого объединять предыдущее облако точек с облаком текущей точки.

  • Преобразование облака точек назад к первой системе координат.

  • Слияние облака точек с накопленной картой облака точек.

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

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

updateDisplay метод дополнительно создает и обновляет 2D вид сверху, передающий отображение облака точек потоком.

% Create a map builder object
mapBuilder = helperLidarMapBuilder('DownsamplePercent', downsamplePercent);

% Set random number seed
rng(0);

closeDisplay = false;
numFrames    = height(lidarPointClouds);

tform = rigid3d;
for n = 1 : skipFrames : numFrames - skipFrames

    % Get the nth point cloud
    ptCloud = lidarPointClouds.PointCloud(n);

    % Use transformation from previous iteration as initial estimate for
    % current iteration of point cloud registration. (constant velocity)
    initTform = tform;

    % Update map using the point cloud
    tform = updateMap(mapBuilder, ptCloud, initTform);

    % Update map display
    updateDisplay(mapBuilder, closeDisplay);
end

Одна только регистрация облака точек создает карту среды, пересеченной транспортным средством. В то время как карта может казаться локально сопоставимой, она может разработать значительный дрейф по целой последовательности.

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

  1. Преобразуйте координаты GPS в локальные Декартовы координаты с помощью "Восточный Север" latlon2local функция. Местоположение GPS от запуска траектории используется в качестве контрольной точки и задает источник локального x, y, z система координат.

  2. Вращайте Декартовы координаты так, чтобы система локальной координаты была выровнена с первыми координатами датчика лидара. Поскольку точная настройка монтирования лидара и GPS на транспортном средстве не известна, они оцениваются.

% Select reference point as first GPS reading
origin = [gpsSequence.Latitude(1), gpsSequence.Longitude(1), gpsSequence.Altitude(1)];

% Convert GPS readings to a local East-North-Up coordinate system
[xEast, yNorth, zUp] = latlon2local(gpsSequence.Latitude, gpsSequence.Longitude, ...
    gpsSequence.Altitude, origin);

% Estimate rough orientation at start of trajectory to align local ENU
% system with lidar coordinate system
theta = median(atan2d(yNorth(1:15), xEast(1:15)));

R = [ cosd(90-theta) sind(90-theta) 0;
     -sind(90-theta) cosd(90-theta) 0;
     0               0              1];

% Rotate ENU coordinates to align with lidar coordinate system
groundTruthTrajectory = [xEast, yNorth, zUp] * R;

Наложите траекторию основной истины на созданную карту.

hold(mapBuilder.Axes, 'on')
scatter(mapBuilder.Axes, groundTruthTrajectory(:,1), groundTruthTrajectory(:,2), ...
    'green','filled');

helperAddLegend(mapBuilder.Axes, ...
    {'Map Points', 'Estimated Trajectory', 'Ground Truth Trajectory'});

После начального поворота предполагаемая траектория значительно поворачивает от траектории основной истины. Траектория, оцененная с помощью одной только регистрации облака точек, может дрейфовать по ряду причин:

  • Шумные сканы от датчика без достаточного перекрытия

  • Отсутствие достаточно сильных функций, например, около дальних дорог

  • Неточное начальное преобразование, особенно когда вращение является значительным.

% Close map display
updateDisplay(mapBuilder, true);

Используйте ориентацию IMU, чтобы улучшить созданную карту

IMU является электронным устройством, смонтированным на платформе. IMUs содержат несколько датчиков, которые сообщают различную информацию о движении транспортного средства. Типичные IMUs включают акселерометры, гироскопы и магнитометры. IMU может обеспечить надежную меру ориентации.

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

% Reset the map builder to clear previously built map
reset(mapBuilder);

% Set random number seed
rng(0);

initTform = rigid3d;
for n = 1 : skipFrames : numFrames - skipFrames

    % Get the nth point cloud
    ptCloud = lidarPointClouds.PointCloud(n);

    if n > 1
        % Since IMU sensor reports readings at a much faster rate, gather
        % IMU readings reported since the last lidar scan.
        prevTime = lidarPointClouds.Time(n - skipFrames);
        currTime = lidarPointClouds.Time(n);
        timeSinceScan = timerange(prevTime, currTime);

        imuReadings = imuOrientations(timeSinceScan, 'Orientation');

        % Form an initial estimate using IMU readings
        initTform = helperComputeInitialEstimateFromIMU(imuReadings, tform);
    end

    % Update map using the point cloud
    tform = updateMap(mapBuilder, ptCloud, initTform);

    % Update map display
    updateDisplay(mapBuilder, closeDisplay);
end


% Superimpose ground truth trajectory on new map
hold(mapBuilder.Axes, 'on')
scatter(mapBuilder.Axes, groundTruthTrajectory(:,1), groundTruthTrajectory(:,2), ...
    'green','filled');

helperAddLegend(mapBuilder.Axes, ...
    {'Map Points', 'Estimated Trajectory', 'Ground Truth Trajectory'});

% Capture snapshot for publishing
snapnow;

% Close open figures
close([hFigFixed, hFigAlign, hFigAccum]);
updateDisplay(mapBuilder, true);

Используя оценку ориентации от IMU значительно улучшил регистрацию, ведя к намного более близкой траектории с меньшим дрейфом.

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

helperAlignPlayers выравнивает массив ячеек потоковой передачи проигрывателей, таким образом, они располагаются слева направо на экране.

function helperAlignPlayers(players)

validateattributes(players, {'cell'}, {'vector'});

hasAxes = cellfun(@(p)isprop(p,'Axes'),players,'UniformOutput', true);
if ~all(hasAxes)
    error('Expected all viewers to have an Axes property');
end

screenSize   = get(groot, 'ScreenSize');
screenMargin = [50, 100];

playerSizes = cellfun(@getPlayerSize, players, 'UniformOutput', false);
playerSizes = cell2mat(playerSizes);

maxHeightInSet = max(playerSizes(1:3:end));

% Arrange players vertically so that the tallest player is 100 pixels from
% the top.
location = round([screenMargin(1), screenSize(4)-screenMargin(2)-maxHeightInSet]);
for n = 1 : numel(players)
    player = players{n};

    hFig = ancestor(player.Axes, 'figure');
    hFig.OuterPosition(1:2) = location;

    % Set up next location by going right
    location = location + [50+hFig.OuterPosition(3), 0];
end

    function sz = getPlayerSize(viewer)

        % Get the parent figure container
        h = ancestor(viewer.Axes, 'figure');

        sz = h.OuterPosition(3:4);
    end
end

helperVisualizeEgoView визуализирует данные об облаке точек в перспективе эго путем вращения о центре.

function player = helperVisualizeEgoView(ptCloud)

% Create a pcplayer object
xlimits = ptCloud.XLimits;
ylimits = ptCloud.YLimits;
zlimits = ptCloud.ZLimits;

player = pcplayer(xlimits, ylimits, zlimits);

% Turn off axes lines
axis(player.Axes, 'off');

% Set up camera to show ego view
camproj(player.Axes, 'perspective');
camva(player.Axes, 90);
campos(player.Axes, [0 0 0]);
camtarget(player.Axes, [-1 0 0]);

% Set up a transformation to rotate by 5 degrees
theta = 5;
R = [ cosd(theta) sind(theta) 0 0
     -sind(theta) cosd(theta) 0 0
      0          0            1 0
      0          0            0 1];
rotateByTheta = rigid3d(R);

for n = 0 : theta : 359
    % Rotate point cloud by theta
    ptCloud = pctransform(ptCloud, rotateByTheta);

    % Display point cloud
    view(player, ptCloud);

    pause(0.05)
end
end

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

function ptCloudProcessed = helperProcessPointCloud(ptCloud)

% Check if the point cloud is organized
isOrganized = ~ismatrix(ptCloud.Location);

% If the point cloud is organized, use range-based flood fill algorithm
% (segmentGroundFromLidarData). Otherwise, use plane fitting.
groundSegmentationMethods = ["planefit", "rangefloodfill"];
method = groundSegmentationMethods(isOrganized+1);

if method == "planefit"
    % Segment ground as the dominant plane, with reference normal vector
    % pointing in positive z-direction, using pcfitplane. For organized
    % point clouds, consider using segmentGroundFromLidarData instead.
    maxDistance    = 0.4;       % meters
    maxAngDistance = 5;         % degrees
    refVector      = [0, 0, 1]; % z-direction

    [~,groundIndices] = pcfitplane(ptCloud, maxDistance, refVector, maxAngDistance);
elseif method == "rangefloodfill"
    % Segment ground using range-based flood fill.
    groundIndices = segmentGroundFromLidarData(ptCloud);
else
    error("Expected method to be 'planefit' or 'rangefloodfill'")
end

% Segment ego vehicle as points within a given radius of sensor
sensorLocation = [0, 0, 0];
radius         = 3.5;

egoIndices  = findNeighborsInRadius(ptCloud, sensorLocation, radius);

% Remove points belonging to ground or ego vehicle
ptsToKeep = true(ptCloud.Count, 1);
ptsToKeep(groundIndices) = false;
ptsToKeep(egoIndices)    = false;

% If the point cloud is organized, retain organized structure
if isOrganized
    ptCloudProcessed = select(ptCloud, find(ptsToKeep), 'OutputSize', 'full');
else
    ptCloudProcessed = select(ptCloud, find(ptsToKeep));
end
end

helperComputeInitialEstimateFromIMU оценивает начальное преобразование для NDT с помощью показаний ориентации IMU и ранее оцененного преобразования.

function tform = helperComputeInitialEstimateFromIMU(imuReadings, prevTform)

% Initialize transformation using previously estimated transform
tform = prevTform;

% If no IMU readings are available, return
if height(imuReadings) <= 1
    return;
end

% IMU orientation readings are reported as quaternions representing the
% rotational offset to the body frame. Compute the orientation change
% between the first and last reported IMU orientations during the interval
% of the lidar scan.
q1 = imuReadings.Orientation(1);
q2 = imuReadings.Orientation(end);

% Compute rotational offset between first and last IMU reading by
%   - Rotating from q2 frame to body frame
%   - Rotating from body frame to q1 frame
q = q1 * conj(q2);

% Convert to Euler angles
yawPitchRoll = euler(q, 'ZYX', 'point');

% Discard pitch and roll angle estimates. Use only heading angle estimate
% from IMU orientation.
yawPitchRoll(2:3) = 0;

% Convert back to rotation matrix
q = quaternion(yawPitchRoll, 'euler', 'ZYX', 'point');
R = rotmat(q, 'point');

% Use computed rotation
tform.T(1:3, 1:3) = R';
end

helperAddLegend добавляет легенда к осям.

function helperAddLegend(hAx, labels)

% Add a legend to the axes
hLegend = legend(hAx, labels{:});

% Set text color and font weight
hLegend.TextColor  = [1 1 1];
hLegend.FontWeight = 'bold';
end

helperMakeFigurePublishFriendly настраивает фигуры так, чтобы снимок экрана, полученный, опубликовал, правильно.

function helperMakeFigurePublishFriendly(hFig)

if ~isempty(hFig) && isvalid(hFig)
    hFig.HandleVisibility = 'callback';
end

end

Смотрите также

Функции

Объекты

Похожие темы

Внешние веб-сайты

Для просмотра документации необходимо авторизоваться на сайте