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

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

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

概要

LiDAR データからのマップの作成の例では、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

Figure Point Cloud Player contains an axes object. The axes object with title Lidar Sensor Data, xlabel X (m), ylabel Y (m) contains an object of type scatter.

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

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

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;

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

    if mod(viewId,displayRate) == 0
        plot(vSet)
        drawnow
    end
        
    ptCloudPrev = ptCloud;
    initTform = relTform;
end

title('View Set Display')

Figure contains an axes object. The axes object with title View Set Display, xlabel X, ylabel Y contains an object of type graphplot.

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

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

    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

head(vSet.Connections)

    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 番目のループと一致していないことに注目してください。

figure
pcshow(ptCloudMap);
hold on
plot(vSet)
title('Point Cloud Map','Color','w')

Figure contains an axes object. The axes object with title Point Cloud Map, xlabel X, ylabel Y contains 2 objects of type scatter, graphplot.

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

"グラフ 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;

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

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

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

G = createPoseGraph(vSet);
disp(G)

  digraph with properties:

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

% Display view set to display loop closure detections
figure
hG = plot(vSet);

% Update axes limits to focus on loop closure detections
xlim([-50 50]);
ylim([-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)
title('Loop Closure Connections')

Figure contains an axes object. The axes object with title Loop Closure Connections, xlabel X, ylabel Y contains an object of type graphplot.

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

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

vSetOptim = updateView(vSet,optimG.Nodes);

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

figure
plot(vSetOptim)
title('View Set Display (after optimization)')

Figure contains an axes object. The axes object with title View Set Display (after optimization), xlabel X, ylabel Y contains an object of type graphplot.

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

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

figure
pcshow(ptCloudMap);

% Overlay view set display
hold on
plot(vSetOptim);
title('Point Cloud Map (after optimization)','Color','w')

Figure contains an axes object. The axes object with title Point Cloud Map (after optimization), xlabel X, ylabel Y contains 2 objects of type scatter, graphplot.

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

GPU コード生成を使用した SLAM ワークフローの高速化

この例のほとんどの機能は GPU コード生成をサポートしています。NVIDIA® GPU で 3 次元 LiDAR SLAM を実行する方法を示す例については、次の例を参照してください。

GPU での SLAM の使用による LiDAR データからのマップ作成

他のレジストレーションアルゴリズムを使用した 3 次元 LiDAR SLAM

この例では、連続する点群を整列させるためにpcregisterndtを使用しています。点群をレジストレーションするための他の手法については、次の例を参照できます。

Build a Map with Lidar Odometry and Mapping (LOAM) Using Unreal Engine Simulation (Lidar Toolbox): pcregisterloam (Lidar Toolbox)を使用して点群をレジストレーションし、pcmaploam (Lidar Toolbox)を使用して配置を調整します。
Design Lidar SLAM Algorithm Using Unreal Engine Simulation Environment: pcregistericpを使用して点群をレジストレーションし、scanContextLoopDetectorを使用してループ閉じ込みを検出します。
Aerial Lidar SLAM Using FPFH Descriptors (Lidar Toolbox): 特徴の検出とマッチングの手法を使用して点群間の相対的な姿勢を検出し、pcregistericpを使用して配置を調整します。

参考文献

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