insertPointCloud

3 次元の点または点群の観測値をマップに挿入

R2019b 以降

構文

insertPointCloud(map3D,pose,points,maxrange)

insertPointCloud(map3D,pose,ptcloud,maxrange)

insertPointCloud(___,invModel)

説明

insertPointCloud(map3D,pose,points,maxrange) は、与えられた points における 1 つ以上のセンサー観測値を占有マップ map3D に挿入します。占有されている点は 0.7 の観測値で更新されます。センサー pose と points の間にある他のすべての点は障害物なしとして扱われ、0.4 の観測値で更新されます。maxrange の範囲外の点は更新されません。NaN 値は無視されます。

insertPointCloud(map3D,pose,ptcloud,maxrange) は、ptcloud オブジェクトをマップに挿入します。

insertPointCloud(___,invModel) は、障害物のない観測と占有された観測に対応する更新された確率 invModel をもつ点群を挿入します。前述の任意の構文を使用して点群を入力します。

例

3 次元占有マップの作成と点のインフレーション

occupancyMap3D オブジェクトは、センサーの観測値を使用して環境のマップを作成して、障害物を 3 次元空間に格納します。マップを作成し、点群から障害物を特定する点を追加します。次にマップ内の障害物をインフレートして、障害物の周囲の安全な動作空間を確保します。

10 セル数/メートルのマップ解像度で occupancyMap3D オブジェクトを作成します。

map3D = occupancyMap3D(10);

一連の 3 次元の点を姿勢 [x y z qw qx qy qz] からの観測値として定義します。この姿勢はこれらの点を観測するセンサー用で、中心が原点上にあります。複数の観測値を挿入する 2 組の点を定義します。

pose = [ 0 0 0 1 0 0 0];

points = repmat((0:0.25:2)', 1, 3);
points2 = [(0:0.25:2)' (2:-0.25:0)' (0:0.25:2)'];
maxRange = 5;

insertPointCloud を使用して 1 組目の点を挿入します。この関数は、センサーの姿勢と指定された点を使用して、観測値をマップに挿入します。表示されている色は、わかりやすく説明する目的のためにのみ点の高さと関連します。

insertPointCloud(map3D,pose,points,maxRange)
show(map3D)

2 組目の点を挿入します。センサーの姿勢 (原点) とこれらの点の間の光線は、前に挿入した点とオーバーラップします。したがって、センサーと新しい点の間の自由空間が更新されて、自由空間とマークされます。

insertPointCloud(map3D,pose,points2,maxRange)
show(map3D)

マップをインフレートして、障害物の周囲の安全な動作のためにバッファー ゾーンを追加します。ビークル半径と安全距離を定義し、これらの値の合計を使ってマップのインフレーション半径を定義します。

vehicleRadius = 0.2;
safetyRadius = 0.3;
inflationRadius = vehicleRadius + safetyRadius;
inflate(map3D, inflationRadius);

show(map3D)

入力引数

map3D — 3 次元占有マップ

occupancyMap3D オブジェクト

3 次元占有マップ。occupancyMap3D オブジェクトとして指定します。

points — 点群の点

n 行 3 列の行列

センサー座標での点群の点。点 [x y z] の n 行 3 列の行列として指定します。ここで、n は点群に含まれる点の数です。

ptcloud — 点群の読み取り値

pointCloud オブジェクト

点群の読み取り値。pointCloud オブジェクトとして指定します。

メモ

pointCloud オブジェクトの使用には Computer Vision Toolbox™ が必要です。

pose — ビークルの位置と向き

[x y z qw qx qy qz] ベクトル

ビークルの位置と向き。[x y z qw qx qy qz] ベクトルとして指定します。ビークルの姿勢は、xyz の位置ベクトルと [qw qx qy qz] として指定される四元数の方向ベクトルです。

maxrange — センサーの最大範囲

スカラー

点群のセンサーの最大範囲。スカラーとして指定します。この範囲を外れる点は無視されます。

invModel — 逆センサー モデル値

2 要素ベクトル

逆センサー モデル値。障害物がない確率と占有されている確率に対応する 2 要素ベクトルとして指定します。点群の点は、逆センサー モデルと指定された範囲の読み取り値に従って更新されます。NaN の範囲値は無視されます。maxrange を超える範囲値は更新されません。逆センサー モデルを参照してください。

詳細

逆センサー モデル

"逆センサー モデル" は、センサーの読み取り値と障害物の値が点群でどのように設定されるかを決定します。引数 invModel で障害物がない位置と占有されている位置に対して異なる確率を指定することで、このモデルをカスタマイズできます。NaN の範囲値は無視されます。maxrange を超える範囲値は更新されません。

範囲の読み取り値を含むグリッド位置は、占有されている確率で更新されます。読み取り値の前の位置は、障害物がない確率で更新されます。読み取り値の後のすべての位置は更新されません。

拡張機能

C/C++ コード生成

MATLAB® Coder™ を使用して C および C++ コードを生成します。

insertPointCloud は、packType 引数を "flat" に設定した場合、関数 packNGo (MATLAB Coder) を使用したコード生成をサポートしません。

バージョン履歴

R2019b で導入

R2024a: 逆センサー モデル値の指定

対応する障害物のない確率と占有されている確率の逆センサー モデル値を指定できるようになりました。逆センサー モデル値の指定の詳細については、引数 invModel を参照してください。

参考

クラス

• occupancyMap3D | lidarSLAM | occupancyMap

関数

• inflate | setOccupancy | show