이해하기: 루프 클로저 후 3D 장면 그래프 최적화
- 이번 글에서는 Hydra의 Scene Graph Backend가 어떻게 작동하는지, 특히 루프 클로저(loop closure)가 발생한 후 장면 그래프를 어떻게 최적화하는지에 대해 알아보겠습니다.
목차
- 루프 클로저와 Scene Graph Backend의 역할
- Embedded Deformation Graph란 무엇인가?
- 최적화에 사용되는 그래프 구성 요소들
- 에이전트 계층 (Pose Graph)
- 메쉬 제어 포인트 (Mesh Control Points)
- 장소 계층의 최소 신장 트리 (Minimum Spanning Tree)
- 변형 그래프의 최적화
- Factor Graph와 에너지 최소화
- Rigid 변환과 Affine 변환의 차이
- GTSAM과 Graduated Non-Convexity (GNC) Solver
- 최적화 후 장면 그래프의 업데이트
- 메쉬의 보간 (Interpolation)
- 객체 중심점과 경계 상자의 재계산
- 노드 병합과 유지
- 최종 방 검출과 재구성
1. 루프 클로저와 Scene Graph Backend의 역할
- Hydra에서 Scene Graph Backend는 루프 클로저가 감지되었을 때, 프론트엔드에서 생성한 장면 그래프를 최적화하여 오차를 보정하는 역할을 합니다.
2. Embedded Deformation Graph란 무엇인가?
- Embedded Deformation Graph 원래의 메쉬나 포인트 클라우드를 간단한 그래프로 표현하여, 이 그래프의 노드들을 변형시킴으로써 전체 데이터를 효율적으로 변형할 수 있게 합니다.
3. 최적화에 사용되는 그래프 구성 요소들
Scene Graph Backend는 루프 클로저 후에 장면 그래프를 최적화하기 위해 세 가지 주요 구성 요소를 사용
A. 에이전트 계층 (Pose Graph)
- 정의: 로봇의 위치와 자세를 나타내는 노드들로 구성된 그래프
- 내용: 로봇이 이동하면서 추정한 위치 정보(odometry)와 루프 클로저로 인해 얻은 상대 위치 정보(edge)를 모두 포함
- 역할: 로봇의 이동 경로를 최적화하여 위치 오차를 줄임
B. 메쉬 제어 포인트 (Mesh Control Points)
- 정의: 환경의 3D 메쉬를 대표하는 소수의 포인트들입니다.
- 생성 방법: 원래의 메쉬를 클러스터링하여 중요한 포인트들만 샘플링합니다.
- 역할: 메쉬의 변형을 효율적으로 계산하기 위해 사용됩니다.
C. 장소 계층의 최소 신장 트리 (Minimum Spanning Tree)
- 정의: 장소(place) 노드들로 구성된 그래프에서, 모든 노드를 연결하면서 전체 엣지의 가중치 합이 최소가 되는 트리입니다.
- 이유: 계산 효율성을 위해 사용됩니다. 최소 신장 트리는 엣지의 수를 최소화하여 그래프의 복잡도를 줄입니다.
- 역할: 장소 노드들의 변형을 최적화하는 데 사용됩니다.
이 세 가지 구성 요소들은 프론트엔드에서 생성된 계층 간의 엣지로 연결되어 하나의 변형 그래프(deformation graph)를 형성합니다.
4. 변형 그래프의 최적화
- 변형 그래프를 최적화하는 과정: 그래프의 노드들의 위치(포즈)를 조정하여 루프 클로저로 인한 오차를 최소화하는 것
- 이를 위해 Factor Graph와 에너지 최소화 개념을 사용
A. Factor Graph와 에너지 최소화
- Factor Graph는 변수(노드)와 함수(팩터)로 구성된 그래프 구조로, 확률 모델의 요인화를 나타냅니다.
- 이 구조를 사용하면 복잡한 최적화 문제를 효율적으로 풀 수 있습니다.
- 노드:
- 최적화해야 하는 변수들입니다. 여기서는 변형 그래프의 노드 포즈들
- 팩터(Edge Potential):
- 노드들 간의 제약 조건을 나타냅니다. 각 엣지에 잠재적 에너지(potential)를 할당하며, 이는 노드 간의 관계(예: 거리, 변형 정도)를 나타냅니다.
- 최적화 목표:
- 모든 엣지의 잠재적 에너지 합을 최소화하는 것입니다.
- 이는 노드들 간의 불일치를 최소화하여 그래프를 가장 일관된 상태로 만드는 것을 의미
B. Rigid 변환과 Affine 변환의 차이
변형을 모델링할 때 두 가지 주요 변환 방식이 있습니다:
-
Rigid 변환:
- 정의: 회전과 평행 이동만 허용하는 변환입니다.
- 특징: 물체의 크기나 모양이 변하지 않습니다.
- 장점: 건물이나 단단한 객체처럼 물리적으로 변형되지 않는 구조를 정확하게 모델링할 수 있습니다.
-
Affine 변환:
- 정의: 회전, 평행 이동, 스케일링(크기 변화), 전단(shear) 등을 포함하는 일반적인 선형 변환입니다.
- 특징: 물체의 크기나 모양이 변할 수 있습니다.
- 단점: 물리적으로 불가능한 변형을 허용할 수 있어, 현실적인 모델링에 부적합할 수 있습니다.
-
Hydra에서는 Rigid 변환을 사용하여 현실적인 변형만을 허용하고, 최적화 문제를 단순화합니다.
C. GTSAM과 Graduated Non-Convexity (GNC) Solver
-
GTSAM:
- 정의: "Georgia Tech Smoothing and Mapping"의 약자로, 그래프 기반의 최적화 문제를 풀기 위한 C++ 라이브러리입니다.
- 기능: 다양한 최적화 알고리즘을 제공하며, 로봇 공학에서 SLAM 문제를 해결하는 데 널리 사용됩니다.
-
Graduated Non-Convexity (GNC) Solver:
- 정의: 비볼록(non-convex) 최적화 문제를 풀기 위한 방법으로, 점진적으로 문제를 볼록(convex)하게 만들어 풀어갑니다.
- 장점: 로컬 최적해에 빠지지 않고 전역 최적해에 근접할 수 있으며, 이상치(outlier)를 효과적으로 처리할 수 있습니다.
- Hydra에서의 역할: 잘못된 루프 클로저(이상치)를 자동으로 감지하고 무시함으로써 최적화의 정확도를 높입니다.
5. 최적화 후 장면 그래프의 업데이트
최적화 과정이 완료되면, Scene Graph Backend는 다음과 같은 업데이트를 수행합니다.
A. 메쉬의 보간 (Interpolation)
- 정의: 변형 그래프의 노드 포즈 변경에 따라, 전체 메쉬의 각 포인트의 위치를 새롭게 계산하는 과정입니다.
- 방법: [59]에서 제시된 보간 방식을 사용하여, 메쉬의 세부적인 변형을 효율적으로 계산합니다.
- 결과: 루프 클로저로 인한 위치 오차가 보정된 메쉬를 얻습니다.
B. 객체 중심점과 경계 상자의 재계산
- 이유: 메쉬의 변형으로 인해 객체의 위치와 크기가 변경될 수 있으므로, 이를 최신화해야 합니다.
- 방법:
- 메쉬에서 각 객체에 해당하는 버텍스들을 추출합니다.
- 이 버텍스들의 위치를 기반으로 객체의 중심점과 3D 경계 상자를 다시 계산합니다.
- 결과: 객체 정보가 환경의 실제 상태와 일치하게 됩니다.
C. 노드 병합과 유지
- 노드 병합:
- 장소 노드의 경우:
- 서로 0.4m 이하의 거리에 있는 노드들은 동일한 장소로 간주하여 병합합니다.
- 객체 노드의 경우:
- 같은 종류의 객체이며,
- 하나의 객체가 다른 객체의 경계 상자 내에 포함되면 병합합니다.
- 장소 노드의 경우:
- 노드 유지:
- 최적화 과정에서 병합되지 않은 이전의 장면 그래프 버전을 저장합니다.
- 이유: 나중에 GNC Solver에 의해 잘못된 루프 클로저로 판명된 경우, 해당 변경을 쉽게 취소할 수 있도록 하기 위해서입니다.
6. 최종 방 검출과 재구성
마지막으로, 업데이트된 장소 노드들을 기반으로 방(layer 4)을 다시 검출합니다. 이는 이전 섹션 III에서 설명한 방법을 다시 적용하여, 환경의 공간 구조를 최신화합니다.
- 방법:
- 장소 노드들의 연결성을 분석하여, 서로 연결된 노드들의 그룹을 방으로 정의합니다.
- 새로운 공간 관계를 반영하여 방의 경계를 재설정합니다.
결론
Hydra의 Scene Graph Backend는 루프 클로저가 감지되었을 때, 환경의 3D 장면 그래프를 최적화하여 로봇의 공간 이해를 향상시킵니다. 이 과정에서 변형 그래프를 사용하여 메쉬와 노드들의 위치를 조정하고, GNC Solver를 통해 이상치를 효과적으로 처리합니다. 최적화 후에는 객체와 장소 정보가 최신화되고, 방의 구조도 재검출되어 환경의 정확한 3D 표현이 완성됩니다.
참고 문헌
- [59] Embedded Deformation for Shape Manipulation
- [2] GTSAM: A C++ library for Georgia Tech Smoothing And Mapping
용어 정리
- 루프 클로저(loop closure): 로봇이 이전에 방문했던 장소를 다시 방문했을 때 이를 인식하고 위치 오차를 수정하는 과정.
- Scene Graph: 환경의 계층적 공간 표현으로, 메쉬, 객체, 장소, 방 등의 계층으로 구성됨.
- Embedded Deformation Graph: 공간 데이터를 효율적으로 변형하기 위한 그래프 구조.
- Factor Graph: 변수와 함수로 구성된 그래프 구조로, 최적화 문제를 효율적으로 풀 수 있음.
- Rigid 변환: 회전과 평행 이동만을 허용하는 변환으로, 물체의 크기나 모양은 변하지 않음.
- Affine 변환: 회전, 평행 이동, 스케일링 등을 포함하는 변환으로, 물체의 크기나 모양이 변할 수 있음.
- GTSAM: 그래프 기반의 최적화 문제를 풀기 위한 C++ 라이브러리.
- Graduated Non-Convexity (GNC) Solver: 비볼록 최적화 문제를 풀기 위한 방법으로, 이상치를 효과적으로 처리함.
이 글을 통해 Hydra의 Scene Graph Backend와 루프 클로저 후의 최적화 과정을 이해하는 데 도움이 되었기를 바랍니다. 복잡한 개념들이 많지만, 하나하나 살펴보면 로봇의 공간 이해를 향상시키는 핵심 요소들을 파악할 수 있습니다.
새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.