poseGraph3D

3 次元姿勢グラフの作成

R2019b 以降

説明

poseGraph3D オブジェクトは、3 次元姿勢グラフの表現の情報を格納します。姿勢グラフはエッジによって接続されたノードを含みます。それぞれのノードの推定は、ノード間の相対姿勢とその測定の不確かさを定義するエッジの制約によってグラフに接続されます。

姿勢グラフを反復的に作成するには、関数 addRelativePose を使用して相対姿勢の推定を追加し、指定したエッジの制約を使用して既存のノードにそれらを接続します。姿勢ノードは、いずれかの姿勢ノードを基準に相対的に指定しなければなりません。測定の不確かさは情報行列を使用して指定します。

2 つの連続していないノード間にエッジを追加すると、グラフで "ループ閉じ込み" が作成されます。ノード ペア間に複数のエッジをもつ "マルチエッジ" もサポートされます。これには、ループ閉じ込みも含まれます。エッジの制約やループ閉じ込みをさらに追加するには、関数 addRelativePose を使用してノード ID を指定します。関数 optimizePoseGraph で姿勢グラフを最適化する際、それらのエッジの制約をすべて満たす解が求められます。

ランドマーク ポイント ノードを追加するには、関数 addPointLandmark を使用します。この関数は、ノードを向きの推定がない xyz 点として指定します。ランドマークは、いずれかの姿勢ノードを基準に相対的に指定しなければなりません。

2 次元姿勢グラフについては、poseGraph を参照してください。

実際のセンサー データから 3 次元姿勢グラフを作成して最適化する例については、Landmark SLAM Using AprilTag Markersを参照してください。

作成

構文

poseGraph = poseGraph3D

poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)

説明

poseGraph = poseGraph3D は、3 次元姿勢グラフ オブジェクトを作成します。姿勢グラフを反復的に作成するには、addRelativePose を使用して姿勢を追加します。

poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) は、コード生成時に姿勢グラフで受け入れ可能なエッジ数とノード数の上限を指定します。この上限はコード生成時にのみ必要です。

プロパティ

NumNodes — 姿勢グラフ内のノードの数

1 (既定値) | 正の整数

この プロパティ は読み取り専用です。

姿勢グラフ内のノードの数。正の整数として指定します。各ノードは、姿勢の測定またはポイント ランドマークの測定を表します。ノード間の相対姿勢を指定するには、addRelativePose を使用します。ランドマーク姿勢を指定するには、addLandmarkPose を使用します。すべてのノードのリストを取得するには、edgeNodePairs を使用します。

NumEdges — 姿勢グラフ内のエッジの数

0 (既定値) | 非負の整数

この プロパティ は読み取り専用です。

姿勢グラフ内のエッジの数。非負の整数として指定します。各エッジは姿勢グラフ内の 2 つのノードを接続します。ループ閉じ込みエッジとランドマーク エッジも含まれます。

NumLoopClosureEdges — ループ閉じ込みの数

0 (既定値) | 非負の整数

この プロパティ は読み取り専用です。

姿勢グラフ内のループ閉じ込みの数。非負の整数として指定します。ループ閉じ込みのエッジ ID を取得するには、LoopClosureEdgeIDs プロパティを使用します。

LoopClosureEdgeIDs — ループ閉じ込みエッジ ID

ベクトル

この プロパティ は読み取り専用です。

ループ閉じ込みエッジ ID。エッジ ID のベクトルとして指定します。

LandmarkNodeIDs — ランドマーク ノード ID

ベクトル

この プロパティ は読み取り専用です。

ランドマーク ノード ID。各ノードの ID のベクトルとして指定します。

オブジェクト関数

addPointLandmark	Add landmark point node to pose graph
addRelativePose	Add relative pose to pose graph
copy	Create copy of pose graph
edgeNodePairs	Edge node pairs in pose graph
edgeConstraints	Edge constraints in pose graph
edgeResidualErrors	Compute pose graph edge residual errors
findEdgeID	Find edge ID of edge
nodeEstimates	Poses of nodes in pose graph
removeEdges	Remove loop closure edges from graph
show	Plot pose graph

例

3 次元姿勢グラフの最適化

ノードおよびエッジの制約に基づいて姿勢グラフを最適化します。この例で使用する姿勢グラフは、MIT Dataset から取得したものです。これは、駐車場から抽出した情報を使用して生成されています。

MIT データセットから姿勢グラフを読み込みます。poseGraph3Dオブジェクトを検査して、ノードとループ閉じ込みの数を確認します。

load parking-garage-posegraph.mat pg
disp(pg);

  poseGraph3D with properties:

               NumNodes: 1661
               NumEdges: 6275
    NumLoopClosureEdges: 4615
     LoopClosureEdgeIDs: [128 129 130 132 133 134 135 137 138 139 140 142 143 144 146 147 148 150 151 204 205 207 208 209 211 212 213 215 216 217 218 220 221 222 223 225 226 227 228 230 231 232 233 235 236 237 238 240 241 242 243 244 ... ] (1x4615 double)
        LandmarkNodeIDs: [1x0 double]

ID をオフにして姿勢グラフをプロットします。赤い線は、データセットで識別されたループ閉じ込みを示しています。

title('Original Pose Graph')
show(pg,'IDs','off');
view(-30,45)

姿勢グラフを最適化します。エッジの制約およびループ閉じ込みに基づいてノードが調整されます。最適化された姿勢グラフをプロットして、ループ閉じ込みによるノードの調整を確認します。

updatedPG = optimizePoseGraph(pg);
figure
title('Updated Pose Graph')
show(updatedPG,'IDs','off');
view(-30,45)

参照

[1] Carlone, Luca, Roberto Tron, Kostas Daniilidis, and Frank Dellaert. "Initialization Techniques for 3D SLAM: a Survey on Rotation Estimation and its Use in Pose Graph Optimization." 2015 IEEE International Conference on Robotics and Automation (ICRA). 2015, pp. 4597–4604.

拡張機能

C/C++ コード生成

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

コード生成用の poseGraph3D オブジェクトを作成するときには、次の構文を使用します。

poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) は、コード生成時に姿勢グラフで受け入れ可能なエッジ数とノード数の上限を指定します。この上限はコード生成時にのみ必要です。

バージョン履歴

R2019b で導入

参考

関数

• optimizePoseGraph | addRelativePose | addPointLandmark

オブジェクト

• poseGraph | lidarSLAM

トピック

• Landmark SLAM Using AprilTag Markers