Main Content

poseGraph

2 次元姿勢グラフの作成

説明

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

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

Figure showing relative poses between node poses. Each sequential pose is connected by an edge. Poses are specified as x,y, theta relative to the previous node.

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

Figure showing loop closures and multiedges as constraints between nodes. Loop closures connect to previous non-sequential nodes. Multiedges are multiple constraints between the same node pair.

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

Figure showing a landmark position as an xy point relative to two nodes with an edge between each node. to.

lidarSLAM オブジェクトで実行される LiDAR ベースの自己位置推定と環境地図作成の同時実行は、2 次元姿勢グラフの最適化に基づきます。

3 次元姿勢グラフについては、poseGraph3D オブジェクトまたはLandmark SLAM Using AprilTag Markersの例を参照してください。

作成

説明

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

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

プロパティ

すべて展開する

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

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

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

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

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

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

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

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

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

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

オブジェクト関数

addPointLandmarkAdd landmark point node to pose graph
addRelativePoseAdd relative pose to pose graph
copyCreate copy of pose graph
edgeNodePairsEdge node pairs in pose graph
edgeConstraintsEdge constraints in pose graph
edgeResidualErrorsCompute pose graph edge residual errors
findEdgeIDFind edge ID of edge
nodeEstimatesPoses of nodes in pose graph
removeEdgesRemove loop closure edges from graph
showPlot pose graph

すべて折りたたむ

ノードおよびエッジの制約に基づいて姿勢グラフを最適化します。この例で使用する姿勢グラフは、Intel Research Lab Dataset から取得したものです。これは、屋内の実験室でホイール オドメトリとレーザー距離計のセンサー情報を収集して生成されています。

2 次元姿勢グラフを含む Intel データ セットを読み込みます。poseGraphオブジェクトを検査して、ノードとループ閉じ込みの数を確認します。

load intel-2d-posegraph.mat pg
disp(pg)
  poseGraph with properties:

               NumNodes: 1228
               NumEdges: 1483
    NumLoopClosureEdges: 256
     LoopClosureEdgeIDs: [1228 1229 1230 1231 1232 1233 1234 1235 1236 ... ]
        LandmarkNodeIDs: [1x0 double]

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

show(pg,'IDs','off');
title('Original Pose Graph')

Figure contains an axes object. The axes object with title Original Pose Graph contains 3 objects of type line.

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

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

Figure contains an axes object. The axes object with title Updated Pose Graph contains 3 objects of type line.

参照

[1] Grisetti, G., R. Kummerle, C. Stachniss, and W. Burgard. "A Tutorial on Graph-Based SLAM." IEEE Intelligent Transportation Systems Magazine. Vol. 2, No. 4, 2010, pp. 31–43. doi:10.1109/mits.2010.939925.

拡張機能

バージョン履歴

R2019b で導入