poseGraph3D

3차원 자세 그래프 만들기

R2019b 이후

설명

poseGraph3D 객체는 3차원 자세 그래프 표현에 대한 정보를 저장합니다. 자세 그래프는 간선으로 연결된 노드를 포함합니다. 각 노드 추정값은 노드 간 상대 자세와 해당 측정값의 불확실성을 정의하는 간선 제약 조건에 의해 그래프에 연결됩니다.

자세 그래프를 반복적으로 구성하려면, addRelativePose 함수를 사용하여 상대 자세 추정값을 추가하고 이를 지정된 간선 제약 조건이 있는 기존 노드에 연결합니다. 자세 노드는 하나의 자세 노드를 기준으로 지정해야 합니다. 측정값의 불확실성은 정보 행렬을 사용하여 지정합니다.

두 개의 비순차적 노드 사이에 간선을 추가하면 그래프에 루프 폐쇄가 생성됩니다. 노드 쌍 사이의 다중 간선도 지원되며, 여기에는 루프 폐쇄가 포함됩니다. 간선 제약 조건 또는 루프 폐쇄를 추가하려면 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 (디폴트 값) | 음이 아닌 정수

읽기 전용 속성입니다.

자세 그래프의 간선 개수로, 음이 아닌 정수로 지정됩니다. 각 간선은 자세 그래프에서 두 개의 노드를 연결합니다. 루프 폐쇄 간선 및 랜드마크 간선이 포함됩니다.

NumLoopClosureEdges — 루프 폐쇄 개수

0 (디폴트 값) | 음이 아닌 정수

읽기 전용 속성입니다.

자세 그래프의 루프 폐쇄 개수로, 음이 아닌 정수로 지정됩니다. 루프 폐쇄의 간선 ID를 가져오려면 LoopClosureEdgeIDs 속성을 사용합니다.

LoopClosureEdgeIDs — 루프 폐쇄 간선 ID

벡터

읽기 전용 속성입니다.

루프 폐쇄 간선 ID로, 간선 ID로 구성된 벡터로 지정됩니다.

LandmarkNodeIDs — 랜드마크 노드 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 데이터셋에서 가져온 것이며 주차장에서 추출된 정보를 사용하여 생성되었습니다.

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]

IDs를 off로 설정하여 자세 그래프를 플로팅합니다. 빨간색 선은 데이터 세트에서 식별된 루프 폐쇄를 나타냅니다.

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