このページは前リリースの情報です。該当の英語のページはこのリリースで削除されています。

SLAM を使用した LiDAR データからのマップの作成

この例では次を使用します。

この例では、自己位置推定と環境地図作成の同時実行 (SLAM) の使用により、車両に取り付けられたセンサーからの 3 次元 LiDAR データを処理して、徐々にマップを作成し、車両の軌跡を推定する方法を説明します。3 次元 LiDAR データに加え、慣性航法センサー (INS) も使用して、マップを作成します。このように作成されたマップは、車両ナビゲーションの経路計画に役立ち、位置推定にも使用できます。

概要

LiDAR データからのマップの作成 (Automated Driving Toolbox)の例では、3 次元 LiDAR データと IMU の読み取り値を使用して、車両がたどった環境のマップを徐々に作成します。このアプローチでは局所的な一貫性のあるマップが作成されますが、これは小さな領域の地図作成にのみ適しています。長いシーケンスではドリフトが蓄積され、重大なエラーになります。この制限に対処するため、この例では以前に訪れた場所を認識し、グラフ SLAM アプローチを使用して、蓄積されたドリフトの修正を試みます。

記録データの読み込みと確認

この例で使用するデータは Velodyne SLAM データセットに含まれています。約 6 分間の記録データです。データを一時ディレクトリにダウンロードします。

メモ: このダウンロードは数分かかることがあります。

baseDownloadURL = 'https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip';
dataFolder      = fullfile(tempdir, 'kit_velodyneslam_data_scenario1', filesep);
options         = weboptions('Timeout', Inf);

zipFileName  = dataFolder + "scenario1.zip";

% Get the full file path to the PNG files in the scenario1 folder.
pointCloudFilePattern = fullfile(dataFolder, 'scenario1', 'scan*.png');
numExpectedFiles = 2513;

folderExists = exist(dataFolder, 'dir');
if ~folderExists
    % Create a folder in a temporary directory to save the downloaded zip
    % file.
    mkdir(dataFolder);

    disp('Downloading scenario1.zip (153 MB) ...')
    websave(zipFileName, baseDownloadURL, options);

    % Unzip downloaded file
    unzip(zipFileName, dataFolder);

elseif folderExists && numel(dir(pointCloudFilePattern)) < numExpectedFiles
    % Redownload the data if it got reduced in the temporary directory.
    disp('Downloading scenario1.zip (153 MB) ...')
    websave(zipFileName, baseDownloadURL, options);

    % Unzip downloaded file.
    unzip(zipFileName, dataFolder)
end

Downloading scenario1.zip (153 MB) ...

関数 helperReadDataset を使用して、作成したフォルダーから timetable 形式のデータを読み取ります。LiDAR によって取得された点群は PNG イメージファイル形式で格納されます。変数 pointCloudTable にある点群ファイル名のリストを抽出します。イメージファイルから点群データを読み取るには、関数 helperReadPointCloudFromFile を使用します。この関数はイメージファイル名を受け取り、pointCloud オブジェクトを返します。INS の読み取り値は、コンフィギュレーションファイルから直接読み取られ、変数 insDataTable に格納されます。

datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern);

pointCloudTable = datasetTable(:, 1);
insDataTable    = datasetTable(:, 2:end);

最初の点群を読み取り、それを MATLAB® コマンドプロンプトに表示します。

ptCloud = helperReadPointCloudFromFile(pointCloudTable.PointCloudFileName{1});
disp(ptCloud)

  pointCloud with properties:

     Location: [64×870×3 single]
        Count: 55680
      XLimits: [-78.4980 77.7062]
      YLimits: [-76.8795 74.7502]
      ZLimits: [-2.4839 2.6836]
        Color: []
       Normal: []
    Intensity: []

最初の INS 読み取り値を表示します。timetable は、INS の Heading、Pitch、Roll、X、Y、および Z の情報を保持しています。

disp(insDataTable(1, :))

         Timestamps         Heading     Pitch        Roll          X          Y         Z   
    ____________________    _______    ________    _________    _______    _______    ______

    13-Jun-2010 06:27:31    1.9154     0.007438    -0.019888    -2889.1    -2183.9    116.47

ストリーミング点群表示 pcplayer を使用して、点群を可視化します。車両は、2 つのループで構成される経路をたどります。1 番目のループでは、車両は連続した方向転換を行い、開始点に戻ります。2 番目のループでは、車両は別のルートに沿って連続した方向転換を行い、再び開始点に戻ります。

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

% Create a streaming point cloud display object
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')

% Skip evey other frame since this is a long sequence
skipFrames  = 2;
numFrames   = height(pointCloudTable);
for n = 1 : skipFrames : numFrames

    % Read a point cloud
    fileName = pointCloudTable.PointCloudFileName{n};
    ptCloud = helperReadPointCloudFromFile(fileName);

    % Visualize point cloud
    view(lidarPlayer, ptCloud);

    pause(0.01)
end

オドメトリを使用したマップの作成

まず、LiDAR データからのマップの作成 (Automated Driving Toolbox)の例で説明したアプローチを使用して、マップを作成します。このアプローチは次のステップで構成されています。

LiDAR スキャンを整列させる: 点群のレジストレーション手法を使用して、連続的な LiDAR スキャンを整列させます。この例では、スキャンのレジストレーションに pcregisterndt を使用します。これらの変換を連続して構成することにより、各点群は最初の点群の基準座標系に変換されます。
整列したスキャンを組み合わせる: 変換したすべての点群を組み合わせることで、マップを生成します。

インクリメンタルにマップを作成し、車両の軌跡を推定するこのアプローチは、"オドメトリ" と呼ばれます。

pcviewset オブジェクトを使用して、複数のビューのデータを格納および管理します。ビューセットには、接続されたビューのセットが含まれています。

各ビューには、1 つのビューに関連付けられた情報が格納されています。この情報には、ビューの絶対姿勢、そのビューで取得された点群センサーデータ、およびビューの一意な識別子が含まれています。addView を使用して、ビューをビューセットに追加します。
ビュー間の接続を確立するには、addConnection を使用します。接続には、接続されたビュー間の相対的な変換、この測定値の計算に伴う不確かさ (情報行列で表されます)、および関連付けられたビューの識別子などの情報が格納されます。
plot メソッドを使用して、ビューセットによって確立された接続を可視化します。これらの接続は、車両がたどった経路を可視化するために使用できます。

hide(lidarPlayer)

% Set random seed to ensure reproducibility
rng(0);

% Create an empty view set
vSet = pcviewset;

% Create a figure for view set display
hFigBefore = figure('Name', 'View Set Display');
hAxBefore = axes(hFigBefore);

% Initialize point cloud processing parameters
downsamplePercent = 0.1;
regGridSize       = 3;

% Initialize transformations
absTform   = rigidtform3d;  % Absolute transformation to reference frame
relTform   = rigidtform3d;  % Relative transformation between successive scans

viewId = 1;
skipFrames  = 5;
numFrames   = height(pointCloudTable);
displayRate = 100;      % Update display every 100 frames
for n = 1 : skipFrames : numFrames

    % Read point cloud
    fileName = pointCloudTable.PointCloudFileName{n};
    ptCloudOrig = helperReadPointCloudFromFile(fileName);

    % Process point cloud
    %   - Segment and remove ground plane
    %   - Segment and remove ego vehicle
    ptCloud = helperProcessPointCloud(ptCloudOrig);

    % Downsample the processed point cloud
    ptCloud = pcdownsample(ptCloud, "random", downsamplePercent);

    firstFrame = (n==1);
    if firstFrame
        % Add first point cloud scan as a view to the view set
        vSet = addView(vSet, viewId, absTform, "PointCloud", ptCloudOrig);

        viewId = viewId + 1;
        ptCloudPrev = ptCloud;
        continue;
    end

    % Use INS to estimate an initial transformation for registration
    initTform = helperComputeInitialEstimateFromINS(relTform, ...
        insDataTable(n-skipFrames:n, :));

    % Compute rigid transformation that registers current point cloud with
    % previous point cloud
    relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize, ...
        "InitialTransform", initTform);

    % Update absolute transformation to reference frame (first point cloud)
    absTform = rigidtform3d( absTform.A * relTform.A );

    % Add current point cloud scan as a view to the view set
    vSet = addView(vSet, viewId, absTform, "PointCloud", ptCloudOrig);

    % Add a connection from the previous view to the current view, representing
    % the relative transformation between them
    vSet = addConnection(vSet, viewId-1, viewId, relTform);

    viewId = viewId + 1;

    ptCloudPrev = ptCloud;
    initTform   = relTform;

    if n>1 && mod(n, displayRate) == 1
        plot(vSet, "Parent", hAxBefore);
        drawnow update
    end
end

ビューセットオブジェクト vSet は現在、ビューと接続を保持しています。vSet の Views テーブルでは、最初のビューに対する各ビューの絶対姿勢を変数 AbsolutePose で指定します。vSet の Connections テーブルでは、接続されたビュー間の相対的な制約を変数 RelativePose で指定し、接続に関連付けられた不確かさをエッジごとに変数 InformationMatrix で指定します。

% Display the first few views and connections
head(vSet.Views)
head(vSet.Connections)

    ViewId      AbsolutePose        PointCloud  
    ______    ________________    ______________

      1       1×1 rigidtform3d    1×1 pointCloud
      2       1×1 rigidtform3d    1×1 pointCloud
      3       1×1 rigidtform3d    1×1 pointCloud
      4       1×1 rigidtform3d    1×1 pointCloud
      5       1×1 rigidtform3d    1×1 pointCloud
      6       1×1 rigidtform3d    1×1 pointCloud
      7       1×1 rigidtform3d    1×1 pointCloud
      8       1×1 rigidtform3d    1×1 pointCloud

    ViewId1    ViewId2      RelativePose      InformationMatrix
    _______    _______    ________________    _________________

       1          2       1×1 rigidtform3d      {6×6 double}   
       2          3       1×1 rigidtform3d      {6×6 double}   
       3          4       1×1 rigidtform3d      {6×6 double}   
       4          5       1×1 rigidtform3d      {6×6 double}   
       5          6       1×1 rigidtform3d      {6×6 double}   
       6          7       1×1 rigidtform3d      {6×6 double}   
       7          8       1×1 rigidtform3d      {6×6 double}   
       8          9       1×1 rigidtform3d      {6×6 double}

次に、作成したビューセットを使用して点群マップを作成します。pcalign を使用して、ビューの絶対姿勢をビューセット内の点群の位置に合わせます。グリッドサイズを指定して、マップの分解能を制御します。マップは pointCloud オブジェクトとして返されます。

ptClouds = vSet.Views.PointCloud;
absPoses = vSet.Views.AbsolutePose;
mapGridSize = 0.2;
ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);

このアプローチを使用してたどった経路は、時間の経過とともにドリフトすることに注目してください。1 番目のループに沿った経路が開始点に戻っているのは妥当と考えられますが、2 番目のループは開始点から大幅にドリフトしています。ドリフトが蓄積したことにより、2 番目のループは開始点から数メーター離れたところで終了しています。

オドメトリのみを使用して作成したマップは不正確です。たどった経路と共に、作成した点群マップを表示します。2 番目のループのマップとたどった経路が 1 番目のループと一致していないことに注目してください。

hold(hAxBefore, 'on');
pcshow(ptCloudMap);
hold(hAxBefore, 'off');

close(hAxBefore.Parent)

姿勢グラフ最適化を使用したドリフトの修正

"グラフ SLAM" は、オドメトリのドリフトを解決するために広く使用されている手法です。グラフ SLAM のアプローチでは、グラフをインクリメンタルに作成します。このグラフでは、ノードは車両の姿勢に対応し、エッジは接続された姿勢を制約するセンサーの測定値を表します。このようなグラフは "姿勢グラフ" と呼ばれます。姿勢グラフにはエッジが含まれており、これが測定値におけるノイズや誤差に起因する矛盾した情報を符号化します。そして、構成されたグラフ内のノードが最適化され、測定値を最適に説明する車両の姿勢のセットが求められます。この手法は "姿勢グラフ最適化" と呼ばれます。

ビューセットから姿勢グラフを作成するには、関数 createPoseGraph を使用できます。この関数は、ビューセットに含まれる各ビューのノードと各接続のエッジを作成します。姿勢グラフを最適化するには、関数optimizePoseGraph (Navigation Toolbox)を使用できます。

ドリフトの修正においてグラフ SLAM の有効性に寄与する重要な要素の 1 つが、ループ、すなわち以前に訪れた場所を正確に検出することです。これは "ループ閉じ込み検出" または "場所認識" と呼ばれます。ループ閉じ込みに対応する姿勢グラフにエッジを追加すると、接続されたノードの姿勢の計測値に矛盾が生じます。これは姿勢グラフ最適化において解決できます。

ループ閉じ込みは、LiDAR センサーが認識するローカル環境の特徴を示す記述子を使用して検出できます。"スキャンコンテキスト" の記述子 [1] は、関数scanContextDescriptorを使用して点群から計算できる記述子の 1 つです。この例では、scanContextLoopDetectorオブジェクトを使用して、各ビューに対応するスキャンコンテキスト記述子を管理します。オブジェクト関数detectLoopを使用して、2 段階記述子検索アルゴリズムでループ閉じ込みを検出します。第 1 段階では、リングキーサブ記述子を計算して、候補になりうるループを求めます。第 2 段階では、スキャンコンテキストの距離をしきい値処理して、ビューをループ閉じ込みとして分類します。

% Set random seed to ensure reproducibility
rng(0);

% Create an empty view set
vSet = pcviewset;

% Create a loop closure detector
loopDetector = scanContextLoopDetector;

% Create a figure for view set display
hFigBefore = figure('Name', 'View Set Display');
hAxBefore = axes(hFigBefore);

% Initialize transformations
absTform   = rigidtform3d;  % Absolute transformation to reference frame
relTform   = rigidtform3d;  % Relative transformation between successive scans

maxTolerableRMSE  = 3; % Maximum allowed RMSE for a loop closure candidate to be accepted

viewId = 1;
for n = 1 : skipFrames : numFrames

    % Read point cloud
    fileName = pointCloudTable.PointCloudFileName{n};
    ptCloudOrig = helperReadPointCloudFromFile(fileName);

    % Process point cloud
    %   - Segment and remove ground plane
    %   - Segment and remove ego vehicle
    ptCloud = helperProcessPointCloud(ptCloudOrig);

    % Downsample the processed point cloud
    ptCloud = pcdownsample(ptCloud, "random", downsamplePercent);

    firstFrame = (n==1);
    if firstFrame
        % Add first point cloud scan as a view to the view set
        vSet = addView(vSet, viewId, absTform, "PointCloud", ptCloudOrig);

        % Extract the scan context descriptor from the first point cloud
        descriptor = scanContextDescriptor(ptCloudOrig);

        % Add the first descriptor to the loop closure detector
        addDescriptor(loopDetector, viewId, descriptor)

        viewId = viewId + 1;
        ptCloudPrev = ptCloud;
        continue;
    end

    % Use INS to estimate an initial transformation for registration
    initTform = helperComputeInitialEstimateFromINS(relTform, ...
        insDataTable(n-skipFrames:n, :));

    % Compute rigid transformation that registers current point cloud with
    % previous point cloud
    relTform = pcregisterndt(ptCloud, ptCloudPrev, regGridSize, ...
        "InitialTransform", initTform);

    % Update absolute transformation to reference frame (first point cloud)
    absTform = rigidtform3d( absTform.A * relTform.A );

    % Add current point cloud scan as a view to the view set
    vSet = addView(vSet, viewId, absTform, "PointCloud", ptCloudOrig);

    % Add a connection from the previous view to the current view representing
    % the relative transformation between them
    vSet = addConnection(vSet, viewId-1, viewId, relTform);

    % Extract the scan context descriptor from the point cloud
    descriptor = scanContextDescriptor(ptCloudOrig);

    % Add the descriptor to the loop closure detector
    addDescriptor(loopDetector, viewId, descriptor)

    % Detect loop closure candidates
    loopViewId = detectLoop(loopDetector);

    % A loop candidate was found
    if ~isempty(loopViewId)
        loopViewId = loopViewId(1);

        % Retrieve point cloud from view set
        loopView = findView(vSet, loopViewId);
        ptCloudOrig = loopView.PointCloud;

        % Process point cloud
        ptCloudOld = helperProcessPointCloud(ptCloudOrig);

        % Downsample point cloud
        ptCloudOld = pcdownsample(ptCloudOld, "random", downsamplePercent);

        % Use registration to estimate the relative pose
        [relTform, ~, rmse] = pcregisterndt(ptCloud, ptCloudOld, ...
            regGridSize, "MaxIterations", 50);

        acceptLoopClosure = rmse <= maxTolerableRMSE;
        if acceptLoopClosure
            % For simplicity, use a constant, small information matrix for
            % loop closure edges
            infoMat = 0.01 * eye(6);

            % Add a connection corresponding to a loop closure
            vSet = addConnection(vSet, loopViewId, viewId, relTform, infoMat);
        end
    end

    viewId = viewId + 1;

    ptCloudPrev = ptCloud;
    initTform   = relTform;

    if n>1 && mod(n, displayRate) == 1
        hG = plot(vSet, "Parent", hAxBefore);
        drawnow update
    end
end

createPoseGraphメソッドを使用して、ビューセットから姿勢グラフを作成します。この姿勢グラフは、次をもつ digraph オブジェクトです。

各ビューの絶対姿勢を含むノード
各接続の相対姿勢の制約を含むエッジ

G = createPoseGraph(vSet);
disp(G)

  digraph with properties:

    Edges: [539×3 table]
    Nodes: [503×2 table]

ビューセットには、連続するビュー間のオドメトリの接続に加えて、ループ閉じ込みの接続が含まれています。たとえば、2 番目のループのトラバーサルと 1 番目のループのトラバーサルの間にある、新しい接続に注目してください。これらはループ閉じ込みの接続です。これらは、終了ノードが連続していないグラフのエッジとして識別できます。

% Update axes limits to focus on loop closure connections
xlim(hAxBefore, [-50 50]);
ylim(hAxBefore, [-100 50]);

% Find and highlight loop closure connections
loopEdgeIds = find(abs(diff(G.Edges.EndNodes, 1, 2)) > 1);
highlight(hG, 'Edges', loopEdgeIds, 'EdgeColor', 'red', 'LineWidth', 3)

optimizePoseGraph を使用して姿勢グラフを最適化します。

optimG = optimizePoseGraph(G, 'g2o-levenberg-marquardt');

vSetOptim = updateView(vSet, optimG.Nodes);

姿勢が最適化されたビューセットを表示します。検出されたループがマージされており、軌跡の精度が上がっていることがわかります。

plot(vSetOptim, 'Parent', hAxBefore)

最適化されたビューセットの絶対姿勢を使用して、より精度の高いマップが作成できるようになりました。関数pcalignを使用し、ビューセットの点群と最適化されたビューセットの絶対姿勢と合わせて、1 つの点群マップにします。グリッドサイズを指定して、作成された点群マップの分解能を制御します。

mapGridSize = 0.2;
ptClouds = vSetOptim.Views.PointCloud;
absPoses = vSetOptim.Views.AbsolutePose;
ptCloudMap = pcalign(ptClouds, absPoses, mapGridSize);

hFigAfter = figure('Name', 'View Set Display (after optimization)');
hAxAfter = axes(hFigAfter);
pcshow(ptCloudMap, 'Parent', hAxAfter);

% Overlay view set display
hold on
plot(vSetOptim, 'Parent', hAxAfter);

helperMakeFigurePublishFriendly(hFigAfter);

精度はまだ改善できるものの、この点群マップの精度は大幅に向上しています。

参考文献

G. Kim and A. Kim, "Scan Context: Egocentric Spatial Descriptor for Place Recognition Within 3D Point Cloud Map," 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, 2018, pp. 4802-4809.

サポート関数とサポートクラス

helperReadDataset は、指定されたフォルダーから timetable へデータを読み取ります。

function datasetTable = helperReadDataset(dataFolder, pointCloudFilePattern)
%helperReadDataset Read Velodyne SLAM Dataset data into a timetable
%   datasetTable = helperReadDataset(dataFolder) reads data from the
%   folder specified in dataFolder into a timetable. The function
%   expects data from the Velodyne SLAM Dataset.
%
%   See also fileDatastore, helperReadINSConfigFile.

% Create a file datastore to read in files in the right order
fileDS = fileDatastore(pointCloudFilePattern, 'ReadFcn', ...
    @helperReadPointCloudFromFile);

% Extract the file list from the datastore
pointCloudFiles = fileDS.Files;

imuConfigFile = fullfile(dataFolder, 'scenario1', 'imu.cfg');
insDataTable = helperReadINSConfigFile(imuConfigFile);

% Delete the bad row from the INS config file
insDataTable(1447, :) = [];

% Remove columns that will not be used
datasetTable = removevars(insDataTable, ...
    {'Num_Satellites', 'Latitude', 'Longitude', 'Altitude', 'Omega_Heading', ...
     'Omega_Pitch', 'Omega_Roll', 'V_X', 'V_Y', 'V_ZDown'});

datasetTable = addvars(datasetTable, pointCloudFiles, 'Before', 1, ...
    'NewVariableNames', "PointCloudFileName");
end

helperProcessPointCloud は、グランドプレーンと自車の点を削除することにより、点群を処理します。

function ptCloud = helperProcessPointCloud(ptCloudIn, method)
%helperProcessPointCloud Process pointCloud to remove ground and ego vehicle
%   ptCloud = helperProcessPointCloud(ptCloudIn, method) processes
%   ptCloudIn by removing the ground plane and the ego vehicle.
%   method can be "planefit" or "rangefloodfill".
%
%   See also pcfitplane, pointCloud/findPointsInCylinder.

arguments
    ptCloudIn (1,1) pointCloud
    method          string      {mustBeMember(method, ["planefit","rangefloodfill"])} = "rangefloodfill"
end

isOrganized = ~ismatrix(ptCloudIn.Location);

if (method=="rangefloodfill" && isOrganized)
    % Segment ground using floodfill on range image
    groundFixedIdx = segmentGroundFromLidarData(ptCloudIn, ...
        "ElevationAngleDelta", 11);
else
    % Segment ground as the dominant plane with reference normal
    % vector pointing in positive z-direction
    maxDistance         = 0.4;
    maxAngularDistance  = 5;
    referenceVector     = [0 0 1];

    [~, groundFixedIdx] = pcfitplane(ptCloudIn, maxDistance, ...
    referenceVector, maxAngularDistance);
end

if isOrganized
    groundFixed = false(size(ptCloudIn.Location,1),size(ptCloudIn.Location,2));
else
    groundFixed = false(ptCloudIn.Count, 1);
end
groundFixed(groundFixedIdx) = true;

% Segment ego vehicle as points within a cylindrical region of the sensor
sensorLocation = [0 0 0];
egoRadius = 3.5;
egoFixed = findPointsInCylinder(ptCloudIn,egoRadius,"Center",sensorLocation);

% Retain subset of point cloud without ground and ego vehicle
if isOrganized
    indices = ~groundFixed & ~egoFixed;
else
    indices = find(~groundFixed & ~egoFixed);
end

ptCloud = select(ptCloudIn, indices);
end

helperComputeInitialEstimateFromINS は、INS の読み取り値から、レジストレーションのための初期変換を推定します。

function initTform = helperComputeInitialEstimateFromINS(initTform, insData)

% If no INS readings are available, return
if isempty(insData)
    return;
end

% The INS readings are provided with X pointing to the front, Y to the left
% and Z up. Translation below accounts for transformation into the lidar
% frame.
insToLidarOffset = [0 -0.79 -1.73]; % See DATAFORMAT.txt
Tnow = [-insData.Y(end), insData.X(end), insData.Z(end)].' + insToLidarOffset';
Tbef = [-insData.Y(1)  , insData.X(1)  , insData.Z(1)].' + insToLidarOffset';

% Since the vehicle is expected to move along the ground, changes in roll
% and pitch are minimal. Ignore changes in roll and pitch, use heading only.
Rnow = rotmat(quaternion([insData.Heading(end) 0 0], 'euler', 'ZYX', 'point'), 'point');
Rbef = rotmat(quaternion([insData.Heading(1)   0 0], 'euler', 'ZYX', 'point'), 'point');

T = [Rbef Tbef;0 0 0 1] \ [Rnow Tnow;0 0 0 1];

initTform = rigidtform3d(T);
end

helperMakeFigurePublishFriendly は、publish で取得するスクリーンショットが適切になるよう、図を調整します。

function helperMakeFigurePublishFriendly(hFig)

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

参考

関数

pcregisterndt | pcshow | createPoseGraph | optimizePoses

オブジェクト

pcviewset | rigidtform3d | pointCloud

外部の Web サイト

Velodyne SLAM Dataset