1. Factor Graph

1.1. 그림으로 설명

• 
• SLAM에서의 팩터 그래프의 각 요소가 SLAM 문제를 어떻게 모델링하고 있는지를 보여주고 있어. 여기서 중요한 요소들을 하나씩 설명해 줄게.

1.1.1. 변수 노드(파란색 원, 녹색 원, 빨간 원)

• 파란색 원(x₁, x₂, x₃, ...): 각 로봇의 포즈(위치와 자세)를 나타내는 변수들이야. 이는 시간에 따라 순차적으로 변화하는 로봇의 위치를 나타내고 있어. 여기서 x₁, x₂, x₃는 각각 로봇이 t₁, t₂, t₃ 시점에 있었던 포즈를 의미해.
• 녹색 원(l₁, l₂, ...): 환경 내 랜드마크의 위치를 나타내는 변수들이야. 랜드마크는 로봇이 이동하면서 주변에서 관찰한 고정된 지점들을 의미해. 예를 들어, l₁과 l₂는 로봇이 인식한 특정 랜드마크들의 위치를 나타내고 있어.
• 빨간색 원(K): 카메라의 내재 매개변수(intrinsic parameters)를 나타내. 이는 카메라가 로봇에 장착되어 있을 때, 카메라의 초점 거리, 이미지 센서의 크기 등과 같은 정보를 포함해. SLAM 시스템에서는 카메라의 이러한 매개변수도 정확하게 추정해야 하기 때문에 변수로 포함돼.

1.1.2. 팩터 노드(검은색 사각형)

• p: 로봇의 초기 상태를 나타내는 사전 확률(prior factor)이야. 초기 위치와 자세에 대한 정보가 여기에 반영돼.
• u₁, u₂:
  • 로봇의 연속된 포즈 간의 관계를 나타내는 오도메트리 팩터(odometry factors)를 의미해.
  • 예를 들어, u₁은 x₁과 x₂ 사이에서 로봇이 얼마나 이동했는지에 대한 제약을 나타내고,
  • u₂는 x₂와 x₃ 사이의 이동 정보를 반영해.
  • 오도메트리 정보는 로봇이 시간에 따라 어떻게 이동했는지를 나타내는 중요한 정보야.
• v₁, v₂, v₃, v₄:
  • 로봇 포즈와 랜드마크 위치 간의 관계를 나타내는 카메라 관측 팩터(visual factors)야.
  • 이 팩터들은 로봇이 특정 위치에서 주변 랜드마크를 관찰했을 때, 그 랜드마크와 로봇의 위치 간의 상대적인 관계를 모델링해.
  • 예를 들어, v₁은 x₁에서 l₁을 관찰한 정보에 해당해, 그리고 v₄는 x₃에서 l₂를 관찰한 정보야.
• c₁, c₂:
  • 루프 클로저(loop closure)를 나타내는 팩터들이야.
  • c₁과 c₂는 로봇이 이전에 방문했던 장소를 다시 방문했을 때 발생하는 제약을 의미해.
  • SLAM에서 루프 클로저는 매우 중요한 역할을 하는데, 이는 로봇이 정확한 위치 추정을 가능하게 하기 때문이야.
  • 예를 들어, 로봇이 x₃ 시점에서 이전에 있었던 x₁ 근처로 돌아왔을 때, 이 정보를 통해 전체 맵의 정확성을 높일 수 있어.

1.1.3. 엣지(연결선)

• 엣지는 변수 노드(원)와 팩터 노드(사각형)를 연결하여 어떤 변수들이 어떤 팩터에 영향을 받는지를 나타내고 있어. 예를 들어:
  • x₁과 p는 연결되어 있어서, 로봇의 초기 위치에 대한 사전 정보가 있다는 것을 보여주고 있어.
  • x₂와 u₁, x₃와 u₂는 오도메트리 팩터를 통해 연결되어 있어서, 로봇이 이동한 경로에 대한 제약을 나타내.
  • x₁과 l₁는 v₁을 통해 연결되어 있어서, 로봇이 특정 위치에서 랜드마크를 관찰했음을 나타내고 있어.

1.1.5. 결론

• 이 그림은 팩터 그래프가 SLAM 문제에서 어떻게 활용되는지를 명확하게 보여주고 있어.
• 이를 통해 로봇이 이동하면서 관찰한 정보와 이동 데이터를 모두 활용하여 주변 환경과 자신의 위치를 동시에 추정할 수 있는 구조를 모델링한 거지.

---

---

1.2. 개념적으로 설명

1. 팩터 그래프란 무엇인가?

• 팩터 그래프는 확률 모델에서 변수들 간의 의존성과 결합 확률 분포를 시각적으로 표현하는 이분 그래프입니다. 팩터 그래프는 두 종류의 노드로 구성됩니다:

• 변수 노드 (Variable Nodes) (원): 추정하고자 하는 변수들입니다.

  • 예를 들어, 로봇의 위치(포즈)나 환경 내 랜드마크의 위치, 카메라의 내재적 매개변수(캘리브레이션 파라미터) 등

• 팩터 노드 (Factor Nodes) (사각형):

  • 변수들 간의 함수적 관계나 제약을 나타냅니다.
  • 이는 센서 측정치, 동작 모델, 오도메트리 정보, 루프 클로저 등의 정보를 통해 정의

• 엣지는 변수 노드와 팩터 노드를 연결하여 어떤 변수들이 어떤 제약에 영향을 받는지를 나타냅니다.

• 이를 통해 복잡한 확률 모델을 시각적으로 표현하고 효율적인 추론을 가능하게 합니다.

---

2. SLAM에서의 팩터 그래프

• SLAM에서의 목표는 주어진 측정치 ( Z )와 모델을 기반으로 -> 가장 가능성이 높은 변수 ( X )의 값을 추정하는 것입니다.
• 이는 최대 사후 확률(MAP) 추정 문제로 공식화되며, 팩터 그래프를 통해 이 문제를 효율적으로 표현하고 해결할 수 있습니다.

---

4. 팩터 그래프의 구성 요소 상세 설명

4.1. 변수 노드

• 로봇 포즈 (( X )): 로봇의 위치와 자세를 나타내며, 시간에 따라 변화하는 연속적인 포즈로 표현됩니다. SLAM에서는 연속된 포즈 간의 관계를 추정하여 로봇의 전체 경로를 최적화합니다.
• 랜드마크 위치 (( L )): 환경 내 특정 지점이나 객체의 위치를 나타냅니다. 로봇이 이동하면서 센서로 인식한 주변 랜드마크의 좌표를 추정합니다.
• 카메라의 내재 매개변수 (( K )): 로봇에 장착된 카메라의 초점 거리, 왜곡 계수 등을 포함하는 변수입니다. 이는 정확한 측정과 맵핑을 위해 필요합니다.

5. 팩터 그래프를 통한 최적화 과정

• 1. 초기화: 변수 노드에 대한 초기 값을 설정합니다.
  • 예를 들어, 로봇 포즈는 오도메트리 데이터를 기반으로 초기화됩니다.

2. 팩터 그래프 생성:

• 측정치와 모델을 기반으로 팩터 노드(사각형)와 엣지를 구성합니다.

3. 오차 함수 정의:

• 각 팩터에서 발생하는 잔여 오차를 수학적으로 표현합니다.

4. 최적화 문제 설정:

• 전체 오차를 최소화하는 최적화 문제를 구성합니다.

5. 해결:

• 비선형 최소제곱 문제로 변환하여, 효율적인 알고리즘(예: 가우스-뉴턴 방법)을 사용하여 최적 해를 구합니다.

6. 업데이트:

• 최적화 결과를 변수 노드에 반영하고, 필요한 경우 반복합니다.

---

6. 팩터 그래프의 수학적 표현

• 
  

---

7. 팩터 그래프의 이점

• 시각화 가능성:
  • 변수와 제약 간의 관계를 그래프로 표현하여 문제의 구조를 명확히 이해할 수 있습니다.
• 일반성:
  • 다양한 변수와 제약 조건을 통합하여 복잡한 문제를 모델링할 수 있습니다.
• 희소성 활용:
  • 그래프의 구조를 이용하여 계산 효율성을 높일 수 있습니다.
• 모듈성:
  • 새로운 센서 데이터나 제약 조건을 쉽게 추가하거나 제거할 수 있습니다.

---

8. 팩터 그래프와 3D 장면 그래프 최적화의 연결

• 3D 장면 그래프 최적화에서는 팩터 그래프를 사용하여
  • 로봇의 경로, 메쉬 변형, 장소 간의 관계를 동시에 최적화합니다.
• 임베디드 변형 그래프:
  • 메쉬 컨트롤 포인트에 변환(포즈)을 할당하여 메쉬를 부드럽게 변형
• 레이어 간 통합:
  • 에이전트, 메쉬, 장소 레이어를 팩터 그래프 내에서 통합하여 일관된 최적화를 수행합니다.
• 최적화 알고리즘:
  • GTSAM 등의 라이브러리를 사용하여 효율적인 최적화를 수행하며, 잘못된 루프 클로저와 같은 이상치를 처리하기 위해 견고한 최적화 기법(예: GNC)을 적용합니다.

---

9. 결론

• 팩터 그래프는 SLAM과 3D 장면 그래프 최적화에서 변수들 간의 복잡한 확률적 관계를 효율적으로 모델링하고 최적화하는 핵심 도구입니다.
• 이를 통해:
• 동시 최적화: 로봇의 위치 추정, 메쉬 재구성, 환경의 공간적 이해를 동시에 수행할 수 있음
• 확장성: 다양한 센서 데이터와 제약 조건을 통합하여 복잡한 시스템에도 적용 가능합니다.
• 효율성: 희소한 구조를 활용하여 대규모 최적화 문제를 효율적으로 해결합니다.

---

---

GPT의 설명

• deformation graph에서의 control point: https://velog.io/@jk01019/Hydra-용어-정리-Control-Points

백엔드에서의 팩터 그래프와 변형 그래프의 개념 및 관계

개요

팩터 그래프(Factor Graph)와 변형 그래프(Deformation Graph)는 로봇 시스템의 백엔드에서 핵심적인 역할을 하는 두 가지 그래프 구조입니다. 이들은 환경의 지도 생성, 로봇의 위치 추정, 그리고 환경의 메쉬(3D 모델) 변형 등에 사용됩니다. 본 문서에서는 각 그래프의 개념과 기능을 전문적으로 설명하고, 두 그래프 간의 관계를 상세히 분석하겠습니다.

---

팩터 그래프(Factor Graph)

개념

팩터 그래프는 확률적 그래프 모델의 한 종류로, 변수 노드(Variables)와 팩터 노드(Factors)로 구성됩니다. 변수 노드는 추정하고자 하는 미지의 변수들을 나타내며, 팩터 노드는 변수들 간의 제약 조건이나 측정치를 나타냅니다. 팩터 그래프는 이러한 변수들과 제약 조건의 관계를 시각적으로 표현하고, 확률 분포를 모델링합니다.

백엔드에서의 역할

• 위치 추정 및 지도 생성: 로봇의 위치(포즈)와 환경의 특징점(랜드마크)을 추정하기 위해 사용됩니다.
• 루프 클로저 처리: 로봇이 이전에 방문한 위치를 다시 방문할 때 발생하는 루프 클로저를 처리하여, 누적된 위치 추정 오차를 보정합니다.
• 최적화 수행: 팩터 그래프의 변수들을 최적화하여 가장 일관성 있는 지도를 생성하고, 로봇의 정확한 위치를 추정합니다.

동작 원리

1. 변수 노드 생성: 로봇의 위치, 랜드마크의 위치 등 추정해야 하는 변수들을 노드로 생성합니다.
2. 팩터 노드 추가: 센서 측정치, 이동 변환(odometry), 루프 클로저 등의 제약 조건을 팩터로 추가합니다.
3. 최적화 문제 구성: 팩터 그래프를 기반으로 비선형 최적화 문제를 설정합니다.
4. 최적화 알고리즘 적용: Levenberg-Marquardt, Gauss-Newton 등의 알고리즘을 사용하여 변수들을 최적화합니다.
5. 결과 추출: 최적화된 변수 값들을 사용하여 로봇의 위치와 지도를 업데이트합니다.

---

변형 그래프(Deformation Graph)

개념

변형 그래프는 3D 메쉬 또는 포인트 클라우드 데이터를 변형하기 위한 그래프 구조입니다. 노드들은 제어점(Control Points)을 나타내며, 엣지들은 제어점들 간의 공간적 관계를 나타냅니다. 변형 그래프를 사용하면 메쉬를 유연하게 변형하여 환경의 일관성을 유지할 수 있습니다.

백엔드에서의 역할

• 메쉬 변형 및 업데이트: 루프 클로저로 인해 발생한 메쉬의 왜곡을 보정하고, 일관된 환경 지도를 유지합니다.
• 제어점 관리: 메쉬 변형을 위한 제어점을 생성하고, 최적화된 위치로 업데이트합니다.
• 실시간 변형: 로봇의 이동과 센서 데이터에 따라 메쉬를 실시간으로 변형하여 최신 상태를 반영합니다.

동작 원리

1. 제어점 설정: 메쉬의 특정 위치에 제어점을 배치합니다.
2. 변형 그래프 구성: 제어점들 간의 공간적 관계를 엣지로 연결하여 그래프를 구성합니다.
3. 제약 조건 적용: 팩터 그래프의 최적화 결과(예: 로봇 위치 변화)를 제어점에 반영합니다.
4. 메쉬 변형: 제어점들의 위치 변화에 따라 메쉬의 정점들을 변형합니다.
5. 메쉬 업데이트: 변형된 메쉬를 DSG에 반영하여 환경 지도를 업데이트합니다.

---

팩터 그래프와 변형 그래프의 관계

상호 작용 및 데이터 흐름

• 최적화 결과 전달: 팩터 그래프의 최적화 결과는 로봇의 위치 변화와 환경의 구조적 정보를 제공합니다.
• 변형 그래프 업데이트: 변형 그래프는 팩터 그래프의 결과를 받아 제어점들의 위치를 조정합니다.
• 메쉬 변형 수행: 변형 그래프의 업데이트된 제어점을 기반으로 메쉬를 변형하여 일관된 지도를 유지합니다.

상세 설명

1. 루프 클로저 감지:

   • 팩터 그래프에서 루프 클로저가 감지되면, 로봇의 위치 추정에 큰 변화가 발생합니다.
   • 이는 환경 지도에 왜곡을 일으킬 수 있으므로 보정이 필요합니다.

2. 팩터 그래프 최적화:

   • 루프 클로저를 포함한 모든 제약 조건을 반영하여 팩터 그래프를 최적화합니다.
   • 최적화 결과로 각 변수(로봇 위치 등)의 최적 추정치를 얻습니다.

3. 변형 그래프의 제어점 업데이트:

   • 팩터 그래프의 최적화 결과를 사용하여 변형 그래프의 제어점 위치를 업데이트합니다.
   • 이는 제어점들이 환경의 새로운 구조를 반영하도록 합니다.

4. 메쉬 변형 및 업데이트:

   • 변형 그래프의 제어점을 기반으로 메쉬의 정점들을 변형합니다.
   • 이를 통해 메쉬가 최적화된 로봇 위치와 환경 구조를 반영하도록 합니다.

5. DSG 업데이트:

   • 변형된 메쉬와 업데이트된 노드 정보를 DSG(Dynamic Scene Graph)에 반영합니다.
   • 이는 시스템의 다른 모듈들이 일관된 환경 정보를 사용할 수 있도록 합니다.

예시 시나리오

• 환경 지도 생성 중 루프 클로저 발생:
  • 로봇이 처음 방문한 위치로 돌아와 루프 클로저가 감지됩니다.
  • 팩터 그래프는 이 정보를 사용하여 로봇의 위치 추정치를 수정합니다.
  • 변형 그래프는 수정된 위치 정보를 받아 메쉬를 변형하여 지도의 왜곡을 보정합니다.

---

결론

팩터 그래프와 변형 그래프는 로봇 시스템의 백엔드에서 서로 협력하여 환경의 일관된 모델을 유지하는 데 핵심적인 역할을 합니다.

• 팩터 그래프는 로봇의 위치와 환경의 구조를 최적화하여 정확한 위치 추정과 지도 생성을 담당합니다.
• 변형 그래프는 팩터 그래프의 최적화 결과를 기반으로 메쉬를 변형하여 환경 지도의 왜곡을 보정하고 일관성을 유지합니다.

두 그래프는 데이터 흐름과 최적화 과정을 통해 긴밀하게 연결되어 있으며, 이를 통해 로봇은 동적인 환경에서도 정확하고 일관된 지도를 유지할 수 있습니다.

---

추가 참고 사항

• GTSAM 라이브러리: 팩터 그래프 최적화를 위해 GTSAM과 같은 그래프 최적화 라이브러리가 사용됩니다.
• 실시간 처리: 변형 그래프를 사용한 메쉬 변형은 실시간으로 수행되어야 하므로 효율적인 알고리즘과 데이터 구조가 필요합니다.
• 확장성: 이러한 구조는 다중 로봇 시스템이나 대규모 환경에서도 확장 가능하도록 설계되어 있습니다.

---