LiDAR データからのマップの作成

スクリプトを開く

この例では、慣性測定ユニット (IMU) の読み取り値を補助的に使用し、車両に搭載したセンサーからの 3-D LiDAR データを処理して、マップを順次作成する方法を説明します。このようなマップは、車両ナビゲーションのためのパスプランニングを容易にし、また位置推定に使用できます。生成したマップを評価するために、この例では車両の軌跡を全地球測位システム (GPS) の記録と比較する方法も説明します。

概要

高精細度 (HD) マップは、最大誤差が数センチメートルの正確性で道路の精密な形状を提供する地図サービスです。このレベルの精度により、HD マップは位置推定やナビゲーションなどの自動運転ワークフローに適しています。このような HD マップは、3-D LiDAR スキャンを高精度の GPS センサーおよび/または IMU センサーと組み合わせてマップを作成することにより生成され、数センチメートル以内の誤差での車両の位置指定に使用できます。この例では、そのようなシステムの作成に必要な機能のサブセットを実装します。

この例では、次の方法を学習します。

記録された運転データの読み込み、調査、および可視化
LiDAR スキャンを使用したマップの作成
IMU の読み取り値を使用したマップの改良

記録された運転データの読み込みと確認

この例で使用するデータはこの GitHub® リポジトリにあり、約 100 秒間の LiDAR、GPS、および IMU の各データを表しています。データは、それぞれがtimetableを含む MAT ファイル形式で保存されています。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 LiDAR から保存された点群データを読み込みます。LiDAR データの各スキャンは、pointCloudオブジェクトを使用して、3-D 点群として格納されています。このオブジェクトは、検索を高速にするために、kd 木データ構造を使用してデータを内部で整理します。各 LiDAR スキャンに関連付けられているタイムスタンプは、timetable 内の変数 Time に記録されています。

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

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

        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

MAT ファイルから GPS データを読み込みます。timetable の変数 Latitude、Longitude、および Altitude は、車両の GPS デバイスで記録された地理的座標の保存に使用されます。

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

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

        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 ファイルから IMU データを読み込みます。IMU は通常、車両の運動に関する情報を報告する個別のセンサーで構成されます。それらは、加速度計、ジャイロスコープ、磁力計などの複数のセンサーを組み合わせたものです。変数 Orientation は、報告された IMU センサーの向きを保存します。それらの読み取り値は四元数として報告されます。それぞれの読み取り値は、四元数の 4 つの部分を含む 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)

        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 Hz のレートで動作しています。LiDAR が次に遅く、ほぼ 10 Hz のレートで動作し、その次が IMU でほぼ 400 Hz のレートです。

ドライビングデータの可視化

シーンに含まれるものを確認するために、ストリーミングプレーヤーを使用して記録データを可視化します。GPS の読み取り値を可視化するには、geoplayerを使用します。LiDAR の読み取り値を可視化するには、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

記録済みの LiDAR データを使用したマップの作成

LiDAR は強力なセンサーであり、他のセンサーが役立たない困難な環境での知覚に使用できます。LiDAR は車両環境の詳細で完全な 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);

LiDAR を使用して、市全体の HD マップなど、センチメートルレベルの精度で HD マップを作成できます。その後、それらのマップを車両の位置推定に使用できます。そのようなマップを作成する一般的な手法では、移動中の車両から取得した連続する LiDAR スキャンを配置して、それらを組み合わせて単一の大規模な点群にします。この例の残りでは、マップ作成のこの手法について確認します。

LiDAR スキャンの配置: 反復最近接点 (ICP) アルゴリズム、正規分布変換 (NDT) アルゴリズムなどの点群レジストレーション手法を使用して、連続する LiDAR スキャンを配置します。それぞれのアルゴリズムの詳細については、pcregistericpおよびpcregisterndtを参照してください。この例では NDT を使用します。一般的に (特に回転を検討する場合に) 精度が高いためです。関数 pcregisterndt は、参照点群を基準として移動点群を配置する剛体変換を返します。これらの変換を連続的に構成することにより、各点群が変換されて最初の点群からなる参照フレームに戻ります。
配置したスキャンの結合: 新しい点群スキャンのレジストレーションを行い、変換によって最初の点群からなる参照フレームに戻したら、pcmergeを使用して、その点群を最初の点群とマージできます。

近接する LiDAR スキャンに対応する 2 つの点群から始めます。処理を高速化し、スキャン間で十分な運動を累積するために、10 番目ごとのスキャンを使用します。

skipFrames = 10;
frameNum   = 100;

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

レジストレーションの前に、点群の顕著な構造が保持されるように点群を処理します。それらの前処理手順には、次が含まれます。

地面の検出と除去
自車の検出と除去

これらの手順は、LiDAR を使用した地面および障害物の検出の例で詳細に説明しています。この例では、補助関数 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);

これで、1 組の点群の処理パイプラインがよく理解できたので、これをループにまとめて、記録されたデータのシーケンス全体に適用します。クラス helperLidarMapBuilder がこれらすべてをまとめます。このクラスの updateMap メソッドが新しい点群を受け入れて、前述した手順を実行します。

processPointCloud メソッドを使用して、地面と自車を除去することにより点群を処理する。
点群をダウンサンプリングする。
前の点群と現在の点群をマージするために必要な剛体変換を推定する。
点群を変換して最初のフレームに戻す。
累積した点群マップにその点群をマージする。

さらに、updateMap メソッドは、初期変換推定も受け入れます。初期変換推定はレジストレーションの初期化に使用されます。初期化が良好であれば、レジストレーション結果が大幅に改善されます。反対に、初期化が不良であれば、レジストレーションに悪影響を及ぼします。良好な初期化を行うと、アルゴリズムの実行時間も改善することがあります。

レジストレーションの初期推定を行う一般的な手法は、等速と仮定することです。前の反復の変換を初期推定として使用します。

さらに updateDisplay メソッドは、2-D 上面ビューのストリーミング点群表示を作成して更新します。

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

% Set random number seed
rng(0);

closeDisplay = false;
numFrames    = height(lidarPointClouds);

tform = rigidtform3d;
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 の読み取り値 (緯度、経度、標高) をローカル座標系に変換します。シーケンスの最初の点群の原点に一致するローカル座標系を選択します。この変換は、2 種類の変換を使用して計算します。

関数latlon2localを使用して、GPS の座標をローカル直交座標 (東-北-上) に変換します。軌跡開始の GPS 位置を基準点として使用して、ローカル座標系の x、y、z の原点を定義します。
ローカル座標系が LiDAR センサーの最初の座標と揃うように、直交座標を回転します。車両における LiDAR および 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 は、プラットフォーム上に搭載された電子デバイスです。IMU は、車両の運動に関するさまざまな情報を報告する複数のセンサーを内蔵しています。一般的な IMU には、加速度計、ジャイロスコープ、および磁力計が組み込まれています。IMU は信頼できる方向測定を提供できます。

IMU の読み取り値を使用して、レジストレーション用により良い初期推定を提供します。この例で使用する、IMU が報告したセンサーの読み取り値は、デバイス上で既にフィルター処理されています。

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

% Set random number seed
rng(0);

initTform = rigidtform3d;
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 は画面の左から右に配置されるように、ストリーミングプレーヤーの cell 配列を整列します。

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;

eulerAngles = [0 0 theta];
translation = [0 0 0];
rotateByTheta = rigidtform3d(eulerAngles, translation);

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 は、IMU の方向の読み取り値、および前に推定した変換を使用して、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 は、パブリッシュにより取得されるスクリーンショットが正しくなるように、Figure を調整します。

function helperMakeFigurePublishFriendly(hFig)

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

end

参考

関数

pcmerge | pcregistericp | pcregisterndt

オブジェクト

pcplayer | geoplayer | pointCloud

外部の Web サイト

Udacity Self-Driving Car Data Subset (MathWorks の GitHub リポジトリ)