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

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

Обзор

Карты высокого определения (HD) являются картографическими услугами, которые обеспечивают точную геометрию дорог до нескольких сантиметров с точностью. Этот уровень точности делает HD-карты подходящими для автоматизированных рабочих процессов вождения, таких как локализация и навигация. Такие HD-карты генерируются путем создания карты из 3-D сканов лидара, в сочетании с высокоточными GPS и или датчиками 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-файла. The 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  

Загрузите данные БИНС из MAT-файла. БИНС обычно состоит из отдельных датчиков, которые сообщают информацию о движении транспортного средства. Они объединяют несколько датчиков, включая акселерометры, гироскопы и магнитометры. The Orientation переменная сохраняет отчетную ориентацию датчика БИНС. Эти показания сообщаются как кватернионы. Каждое чтение задается как вектор 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 Гц, далее следует БИНС со скоростью почти 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, потому что он обычно более точен, особенно при рассмотрении вращений. The pcregisterndt функция возвращает твердое преобразование, которое выравнивает движущееся облако точек относительно облака опорной точки. Путем последовательного компиляции этих преобразований каждое облако точек преобразуется назад в опорную систему координат первого облака точек.

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

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

skipFrames = 10;
frameNum   = 100;

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

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

Эти шаги описаны более подробно в примере «Детекция наземной плоскости и препятствий» с использованием Лидара. В этом примере helperProcessPointCloud Функция helper выполняет эти шаги.

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);

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

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

  • Понижающая дискретизация облака точек.

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

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

  • Объединение облака точек с накопленной картой облака точек.

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

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

The updateDisplay способ дополнительно создает и обновляет 2-D отображение потокового облака точек верхнего вида.

% 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 в локальные Декартовы координаты East-North-Up с помощью 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);

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

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

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

% 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);

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

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

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 визуализирует данные облака точек в перспективе ego путем поворота вокруг центра.

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, используя показания ориентации БИНС и ранее оцененное преобразование.

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

См. также

Функции

Объекты

Похожие темы

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

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