GTSAM 팩터 그래프 기반 로봇 공학 및 비전 추정 안내서

1. GTSAM 개요

1.1 GTSAM의 정의와 철학: 희소 행렬을 넘어 팩터 그래프로

GTSAM(Georgia Tech Smoothing and Mapping)은 BSD 라이선스 하에 배포되는 C++ 라이브러리로, 로봇 공학 및 컴퓨터 비전 분야의 센서 퓨전 문제를 해결하기 위해 개발되었다.1 특히 동시적 위치 추정 및 지도 작성(Simultaneous Localization and Mapping, SLAM), 시각 주행 거리계(Visual Odometry, VO), 운동으로부터의 구조 복원(Structure from Motion, SfM)과 같은 핵심적인 응용 분야에 대한 최첨단 솔루션을 제공한다.1

GTSAM의 가장 중요한 철학적 특징은 전통적인 추정 기법들이 사용하는 희소 행렬(sparse matrices)을 직접 다루는 방식에서 벗어났다는 점이다. 대신, 팩터 그래프(Factor Graphs)와 베이즈 네트워크(Bayes Networks)를 기본 컴퓨팅 패러다임으로 채택하여, 가장 가능성 있는 상태 구성을 최적화한다.1

이러한 팩터 그래프 접근법은 단순히 다른 데이터 구조를 사용하는 것을 넘어, 문제 모델링 방식의 근본적인 전환을 의미한다. 개발자는 전체 시스템의 거대한 희소 행렬 구조를 직접적으로 고려할 필요가 없다. 대신, 주행 거리계 측정이나 GPS 측정과 같은 개별적인 ‘지역적(local)’ 제약 조건들을 독립적인 ’팩터(factor)’로 모델링하여 그래프에 추가함으로써 복잡한 추정 문제를 직관적으로 구성할 수 있다.4 이 방식은 문제의 본질적인 구조를 코드에 직접 반영하게 하여, 복잡한 추정 문제에 대한 인지적 장벽을 크게 낮춘다.

이러한 모델링 패러다임은 GTSAM을 매우 유연하고 확장 가능하게 만드는 핵심 요인이다. 예를 들어, 새로운 센서 모델이나 비표준적인 제약 조건을 시스템에 추가하는 작업은 거대한 행렬 솔버를 재설계하는 복잡한 과정이 아니라, 단순히 새로운 Factor 클래스를 정의하고 그래프에 추가하는 문제로 단순화된다. 이러한 설계 철학 덕분에 gtsam_points와 같이 포인트 클라우드 처리를 위한 전문 라이브러리 6나, 고등학교 로봇 대회(FRC)용 로봇의 특수한 휠 슬립 현상을 모델링하는 사용자 정의 팩터 7와 같은 풍부한 확장 생태계가 자연스럽게 형성될 수 있었다. 즉, 팩터 그래프라는 강력한 추상화가 GTSAM 생태계 활성화의 근본적인 동력으로 작용하고 있는 것이다.

1.2 주요 특징 및 장점

GTSAM은 다음과 같은 주요 특징과 장점을 통해 로봇 공학 및 비전 커뮤니티에서 널리 사용되고 있다.

• 희소성 활용(Sparsity Exploitation): 대부분의 로봇 공학 및 비전 문제는 본질적으로 희소한 연결 구조를 가진다. 예를 들어, 주행 거리계 측정은 시간적으로 연속된 두 로봇 포즈만을 연결하고, 랜드마크 관측은 특정 포즈와 특정 랜드마크만을 연결한다. GTSAM은 이러한 문제의 희소성을 적극적으로 활용하여 불필요한 계산을 피하고 계산 효율성을 극대화한다.2

• 증분 최적화(Incremental Optimization): 실시간 SLAM과 같은 온라인 애플리케이션에서는 새로운 센서 데이터가 들어올 때마다 전체 문제를 처음부터 다시 최적화하는 것은 비효율적이다. GTSAM은 iSAM2(incremental Smoothing and Mapping 2) 알고리즘을 통해, 새로운 측정값이 추가될 때 기존의 계산 결과를 최대한 재사용하여 매우 효율적으로 해를 업데이트할 수 있다.8

• 다중 언어 지원(Multi-language Support): GTSAM의 핵심 라이브러리는 고성능을 위해 C++로 작성되었지만, 연구 및 신속한 프로토타이핑을 지원하기 위해 MATLAB과 Python 래퍼(wrapper)를 공식적으로 제공한다.3 이를 통해 사용자는 C++의 성능과 Python/MATLAB의 개발 편의성을 동시에 활용할 수 있다.11

2. 이론적 배경: 팩터 그래프와 MAP 추정

2.1 그래프 모델의 이해: 베이즈 네트워크와 팩터 그래프

GTSAM의 핵심을 이해하기 위해서는 먼저 확률적 그래피컬 모델, 특히 베이즈 네트워크와 팩터 그래프의 개념을 이해해야 한다.

베이즈 네트워크는 확률 변수들을 노드로, 변수 간의 조건부 의존성을 유향(directed) 간선으로 표현하는 유향 비순환 그래프(Directed Acyclic Graph, DAG)이다.12 각 노드는 자신의 부모 노드가 주어졌을 때의 조건부 확률 분포와 연관된다. 예를 들어, 은닉 마르코프 모델(Hidden Markov Model, HMM)에서 상태 변수 X_k는 이전 상태 X_{k-1}에만 의존하고(P(X_k \vert X_{k-1})), 측정값 Z_k는 현재 상태 X_k에만 의존한다(P(Z_k \vert X_k)).5 베이즈 네트워크는 이러한 인과 관계를 직관적으로 표현하는 데 유용하다.

반면, 팩터 그래프는 변수를 나타내는 ’변수 노드’와 변수들 간의 확률적 제약(함수)을 나타내는 ’팩터 노드’로 구성된 이분 그래프(bipartite graph)이다.2 팩터 그래프는 결합 확률 분포의 인수분해(factorization)를 직접적으로 시각화한다. HMM 예제에서 사전 확률 P(X_1), 전이 확률 P(X_k \vert X_{k-1}), 그리고 측정에 대한 우도(likelihood) L(X_k; z_k) \propto P(Z_k=z_k \vert X_k)는 각각 팩터 노드로 표현되고, 이들은 관련된 상태 변수 노드 X_k에 연결된다.8 이처럼 팩터 그래프는 베이즈 네트워크보다 더 일반적인 모델링 도구로, 복잡한 확률적 관계를 명시적으로 표현하는 데 강점을 가진다.

2.2 최대 사후 확률(MAP) 추정 문제

로봇 공학에서 대부분의 추정 문제는 일련의 측정값 Z가 주어졌을 때, 추정하고자 하는 모든 변수 X(예: 로봇의 궤적, 랜드마크의 위치)의 가장 가능성 있는 구성을 찾는 문제로 귀결된다. 이는 통계적으로 사후 확률(posterior probability) P(X \vert Z)를 최대화하는 문제이며, 이를 최대 사후 확률(Maximum a Posteriori, MAP) 추정이라 한다.4

베이즈 정리에 따르면 사후 확률은 다음과 같이 표현된다.

P(X \vert Z) = \frac{P(Z \vert X) P(X)}{P(Z)} \propto P(Z \vert X) P(X)
여기서 P(Z \vert X)는 주어진 상태 X에 대한 측정값 Z의 확률인 우도(likelihood)이고, P(X)는 측정과 무관한 상태 X의 사전 확률(prior)이다.

팩터 그래프에서 MAP 추정은 모든 팩터의 곱으로 표현되는 전역 함수 f(X) = \prod_i f_i(X_i)를 최대화하는 것과 수학적으로 동일하다.5 각 팩터 f_i는 우도 함수나 사전 확률 분포에 해당한다.

2.3 비선형 최소 제곱 최적화로의 변환

실제 로봇 시스템에서 센서 측정 노이즈는 대부분 독립적인 가우시안 분포를 따른다고 가정할 수 있다. 이 가정 하에서 MAP 추정 문제는 비선형 최소 제곱(Nonlinear Least-Squares, NLS) 문제로 변환될 수 있다.

가우시안 분포의 확률 밀도 함수는 지수 함수를 포함한다. 이 함수의 음의 로그(negative log)를 취하면, 확률의 곱을 최대화하는 것은 지수 항에 있는 마할라노비스 거리(Mahalanobis distance)의 제곱합을 최소화하는 것과 같아진다.14

예를 들어, 선형 시스템 \mathbf{x}_{i+1} = \mathbf{A}_{i}\mathbf{x}_i + \mathbf{b}_i + \eta_i에서 가우시안 노이즈 \eta_i를 가정하면, MAP 추정은 다음과 같은 선형 최소 제곱 문제로 귀결된다.15

\mathcal{X}^{*} = \underset{\mathcal{X}}{\operatorname{argmin}} \sum_{i} \left\| \mathbf{A}_i \mathbf{x}_{i} + \mathbf{b}_{i} - \mathbf{x}_{i+1} \right\|^{2}_{\Sigma_i}
여기서 \left\| \cdot \right\|^2_{\Sigma_i}는 공분산 \Sigma_i로 정규화된 제곱 오차를 의미한다.

대부분의 실제 로봇 시스템은 비선형 동역학 \mathbf{x}_{i+1} = f(\mathbf{x}_i) + \eta_i을 따른다. 이 경우, MAP 추정은 다음과 같은 비선형 최소 제곱(NLS) 문제가 된다.15

\mathcal{X} = \underset{\mathcal{X}}{\operatorname{argmin}} \sum_i \left\| f(\mathbf{x}_i) - \mathbf{x}_{i+1} \right\|^2_{\Sigma_i}
이 NLS 문제는 일반적으로 닫힌 형태의 해가 존재하지 않으므로, Gauss-Newton이나 Levenberg-Marquardt과 같은 반복적 최적화 기법을 사용하여 해를 찾는다. 이러한 기법들은 각 반복 단계에서 현재 추정치 주변에서 비선형 함수를 선형화(linearization)하여 생성된 선형 최소 제곱 문제를 푸는 방식으로 동작한다.15

3. GTSAM 시작하기: 설치 및 환경 설정

3.1 시스템 요구사항 및 의존성 관리

GTSAM을 성공적으로 빌드하고 사용하기 위해서는 몇 가지 필수 및 선택적 구성 요소를 설치해야 한다.

• 필수 요구사항:

• 최신 컴파일러: C++17 표준을 지원하는 현대적인 컴파일러가 필요하다. Linux에서는 GCC 9 또는 Clang 11 이상, macOS에서는 Xcode 14.2 이상, Windows에서는 MSVC 14.2 이상이 요구된다.3

• CMake: 버전 3.9 이상의 CMake가 필요하다.3

• 의존성 관리:

• Boost: 과거에는 Boost 라이브러리에 대한 의존성이 높았으나, 최신 버전(4.3a0 이후)에서는 C++17 표준 라이브러리 기능으로 대부분 대체되어 선택 사항이 되었다.10 직렬화(serialization)나 일부 레거시 기능을 사용해야 할 경우에만 필요하며,

GTSAM_USE_BOOST_FEATURES와 GTSAM_ENABLE_BOOST_SERIALIZATION 플래그를 통해 사용 여부를 제어할 수 있다.3

• Intel TBB (Threaded Building Blocks): 병렬 처리를 통해 최적화 속도를 높이기 위한 라이브러리이다. 선택 사항이지만, 멀티코어 CPU 환경에서는 상당한 성능 향상을 기대할 수 있다.10

• Intel MKL (Math Kernel Library): Intel CPU에 최적화된 선형 대수 연산 라이브러리이다. 선택 사항이며, 모든 경우에 성능 향상을 보장하지는 않으므로 사용 전 벤치마킹이 권장된다.10

3.2 소스 코드 빌드 및 설치

GTSAM의 표준 빌드 절차는 간단하며, 터미널에서 다음 명령어를 순서대로 실행하면 된다.3

1. GitHub 저장소에서 소스 코드 복제: git clone https://github.com/borglab/gtsam.git

2. 빌드 디렉터리 생성 및 이동: mkdir build && cd build

3. CMake로 빌드 환경 구성: cmake..

4. (선택 사항) 단위 테스트 실행: make check

5. 컴파일 및 설치: make install

빌드 옵션을 세밀하게 조정하고 싶을 경우, ccmake.. (터미널 GUI) 또는 cmake-gui.. (그래픽 GUI) 명령어를 사용하면 편리하게 옵션을 확인하고 변경할 수 있다.17

3.3 성능 최적화를 위한 빌드 옵션

GTSAM의 성능을 최대한 끌어내기 위해서는 빌드 시 몇 가지 중요한 옵션을 고려해야 한다.

• CMAKE_BUILD_TYPE: 개발 및 디버깅 과정에서는 Debug 모드를 사용하는 것이 유용하지만, 실제 애플리케이션을 실행하거나 성능을 벤치마킹할 때는 반드시 Release 모드로 빌드해야 한다. Release 모드는 컴파일러 최적화를 최대로 활성화하여 Debug 모드 대비 최대 10배의 성능 향상을 가져올 수 있다.17

• TBB 활용: 멀티코어 프로세서 환경에서는 TBB를 활성화(-DGTSAM_WITH_TBB=ON)하면 선형화 및 소거 과정이 병렬로 처리되어 30-50%의 속도 향상을 기대할 수 있다. 단, 그래프의 크기가 매우 작은 경우에는 스레드 관리 오버헤드로 인해 오히려 성능이 저하될 수 있으므로, 실제 문제에 대한 벤치마킹이 필요하다.17

• 네이티브 아키텍처 최적화: GTSAM_CMAKE_CXX_FLAGS에 -march=native 플래그를 추가하면, 컴파일러가 현재 시스템의 CPU 아키텍처에 맞는 최적의 명령어를 사용하도록 하여 25-30%의 성능 향상을 얻을 수 있다. 하지만 이 옵션으로 생성된 바이너리는 다른 CPU 아키텍처를 가진 시스템에서는 실행되지 않을 수 있으므로 이식성이 저하된다.17

• MKL 사용에 대한 주의: Intel MKL은 특정 대규모 행렬 연산에서는 성능을 향상시킬 수 있지만, GTSAM이 주로 다루는 희소 행렬 연산에서는 오히려 오버헤드로 작용하여 성능을 저하시키는 경우가 많다. 따라서 MKL 사용은 특정 문제에 대해 벤치마킹을 통해 명확한 성능 향상이 확인된 경우에만 제한적으로 고려해야 한다.10

아래 표는 GTSAM의 주요 의존성과 빌드 옵션을 요약한 것이다.

4. GTSAM 핵심 사용법: 팩터 그래프 구축 및 최적화

4.1 기본 구성 요소: Values, Key, Symbol

GTSAM을 사용하여 최적화 문제를 구성하는 데에는 몇 가지 핵심적인 클래스가 사용된다.

• Values: 최적화할 모든 변수들의 현재 상태(추정치)를 저장하는 컨테이너 클래스이다. 이 컨테이너는 Pose2, Point3, Cal3_S2 등 다양한 타입의 변수들을 동적으로 저장할 수 있다.9

• Key: Values 컨테이너 내에 저장된 각 변수를 고유하게 식별하기 위한 정수 타입(typedef된 size_t)이다. 모든 변수는 고유한 Key 값을 가져야 한다.

• Symbol: 코드의 가독성을 높이기 위해 문자와 인덱스를 조합하여 Key를 생성하는 헬퍼 클래스이다. 예를 들어, Symbol('x', 1)은 첫 번째 로봇 포즈를, Symbol('l', 5)는 다섯 번째 랜드마크를 나타내는 Key를 생성한다. 이 방식은 디버깅 시 변수를 쉽게 식별할 수 있게 해주어 매우 유용하다.18

4.2 팩터 그래프 생성: NonlinearFactorGraph

NonlinearFactorGraph 클래스는 비선형 제약 조건들의 집합, 즉 팩터 그래프 자체를 나타낸다. graph.add() 멤버 함수를 사용하여 다양한 종류의 팩터들을 그래프에 추가함으로써 최적화 문제를 점진적으로 구축할 수 있다.19

4.3 주요 팩터 유형 분석

GTSAM은 다양한 로봇 공학 및 비전 문제를 모델링하기 위한 풍부한 팩터 라이브러리를 제공한다. 그중 가장 기본적이고 중요한 팩터는 다음과 같다.

• PriorFactor: 단일 변수에 대한 사전 정보(prior knowledge)를 모델링하는 단항(unary) 팩터이다. 이 팩터는 보통 SLAM 문제에서 첫 번째 로봇의 포즈를 원점 (0, 0, 0)에 고정하는 등, 전체 그래프의 절대적인 기준(anchor)을 설정하는 데 필수적으로 사용된다. PriorFactor가 없으면 전체 해에 대한 게이지 자유도(gauge freedom)가 발생하여 해가 유일하게 결정되지 않을 수 있다.19

• BetweenFactor: 두 변수 간의 상대적인 관계를 모델링하는 이진(binary) 팩터이다. 로봇의 주행 거리계(odometry) 측정으로 인한 연속된 포즈 간의 상대 변환이나, 루프 클로저(loop closure)로 인해 발생하는 두 비연속적인 포즈 간의 상대 변환을 표현하는 데 널리 사용된다.19

4.4 최적화 실행: LevenbergMarquardtOptimizer 및 기타 알고리즘

팩터 그래프와 변수들의 초기 추정치(Values)가 준비되면, 최적화기를 사용하여 해를 구한다. GTSAM은 다음과 같은 다양한 비선형 최적화 알고리즘을 제공한다.

• LevenbergMarquardtOptimizer

• GaussNewtonOptimizer

• DoglegOptimizer

이 중 Levenberg-Marquardt 알고리즘은 강인성(robustness)과 수렴 속도 면에서 균형이 잘 잡혀 있어 널리 사용된다.15

optimizer.optimize() 함수를 호출하면, 최적화기는 팩터 그래프에 정의된 오차의 총합을 최소화하는 변수들의 값을 반복적으로 찾아낸다.

4.5 결과 분석: Marginals를 이용한 불확실성 계산

최적화가 완료되어 최상의 상태 추정치를 얻은 후에는, 그 추정치가 얼마나 확실한지 정량적으로 파악하는 것이 중요하다. Marginals 클래스는 최적화된 해에 대한 각 변수의 사후 공분산(posterior covariance)을 계산하는 기능을 제공한다.15

이 공분산은 최적화 문제의 마지막 선형화 단계에서 계산된 피셔 정보 행렬(Fisher Information Matrix)의 역행렬을 통해 효율적으로 계산된다. 계산된 공분산은 각 변수의 불확실성을 나타내는 타원체(ellipsoid)로 시각화될 수 있으며, 이는 추정 결과의 신뢰도를 평가하는 데 중요한 지표가 된다.15

5. 주요 응용 사례 1: Pose-Graph SLAM

5.1 D Pose-Graph SLAM 문제 정의

Pose-Graph SLAM은 SLAM 문제의 한 형태로, 환경의 랜드마크 위치를 명시적으로 추정하지 않고 로봇의 궤적(trajectory), 즉 일련의 포즈(Pose2 또는 Pose3)만을 최적화 변수로 삼는다.20 맵은 최적화된 포즈들을 기준으로 사후에 재구성될 수 있다.

이 문제의 팩터 그래프는 두 종류의 제약 조건으로 구성된다 19:

1. 주행 거리계 제약: 로봇이 이동하면서 얻는 상대적인 변위 측정값(예: 휠 인코더, IMU, 또는 연속된 라이다 스캔 간의 ICP 결과)으로, 시간적으로 연속된 두 포즈를 연결하는 BetweenFactor로 모델링된다.

2. 루프 클로저 제약: 로봇이 이전에 방문했던 장소를 다시 인식했을 때 생성되는 제약으로, 시간적으로 멀리 떨어진 두 포즈를 연결하는 BetweenFactor로 모델링된다.

5.2 C++ 예제 코드 심층 분석

GTSAM 문서에 포함된 2D Pose-Graph SLAM 예제 코드는 이 문제의 해결 과정을 명확하게 보여준다.19

1. 그래프 및 초기값 생성:

NonlinearFactorGraph graph;와 Values initial;을 통해 빈 팩터 그래프와 초기 추정치 컨테이너를 생성한다.

2. 사전 팩터 추가:

첫 번째 포즈(Key: 1)를 원점에 고정하기 위해 PriorFactor를 추가한다. 이는 전체 맵의 기준 좌표계를 설정하는 역할을 한다. 노이즈 모델은 이 사전 정보에 대한 신뢰도를 나타낸다.

3. 주행 거리계 팩터 추가:

로봇이 x1에서 x5까지 순차적으로 이동하면서 얻은 주행 거리계 측정값을 BetweenFactor로 추가한다. 예를 들어, graph.add(BetweenFactor(1, 2, Pose2(2, 0, 0), odometryNoise));는 x1에서 x2로 x축 방향 2만큼 이동했다는 측정값을 나타낸다.

4. 루프 클로저 팩터 추가:

로봇이 x5 위치에서 x2 위치를 다시 관측했다고 가정하고, graph.add(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), loopNoise));와 같이 루프 클로저 제약을 추가한다.

5. 초기 추정치 설정:

initial.insert(1, Pose2(0.5, 0.0, 0.2));와 같이 각 포즈 변수에 대한 초기 추정치를 Values 컨테이너에 추가한다. 좋은 초기 추정치는 비선형 최적화가 올바른 지역 최솟값으로 수렴하는 데 도움을 준다.

6. 최적화 수행:

LevenbergMarquardtOptimizer optimizer(graph, initial);로 최적화기를 생성하고 Values result = optimizer.optimize();를 호출하여 최적의 포즈들을 계산한다.

5.3 루프 클로저의 역할과 구현

주행 거리계 측정에는 항상 작은 오차가 포함되며, 이 오차는 로봇이 이동함에 따라 계속 누적된다. 이로 인해 시간이 지남에 따라 추정된 궤적은 실제 궤적에서 점점 멀어지게 된다(drift). 루프 클로저는 이러한 누적 오차를 보정하고 전역적으로 일관된 맵과 궤적을 생성하는 데 결정적인 역할을 한다.20

로봇이 과거에 방문했던 위치를 다시 인식하고 그 두 포즈 간의 상대 변환을 측정하면, 이는 팩터 그래프 상에서 시간적으로 멀리 떨어진 노드들을 연결하는 강력한 제약 조건이 된다. 최적화 과정에서 이 제약은 전체 그래프에 걸쳐 오차를 분배하고 누적된 드리프트를 효과적으로 제거한다. GTSAM에서는 이를 BetweenFactor를 사용하여 간단하게 구현할 수 있다.19

6. 주요 응용 사례 2: Structure from Motion (SfM)

6.1 SfM 문제 개요

Structure from Motion(SfM)은 시간적 순서나 카메라의 움직임에 대한 사전 정보 없이, 정렬되지 않은 여러 장의 이미지로부터 3D 장면 구조(랜드마크 위치)와 이미지를 촬영한 카메라의 포즈를 동시에 복원하는 기술이다.21 SfM은 SLAM과 근본적으로 유사한 최적화 문제를 다루지만, 일반적으로 오프라인 배치(batch) 처리 방식으로 수행된다는 차이점이 있다.

6.2 카메라 모델과 투영 팩터: GenericProjectionFactor

SfM 문제의 핵심은 3D 공간상의 한 점이 특정 카메라 포즈에서 이미지 평면의 어디에 투영되는지를 모델링하는 것이다. GTSAM에서는 이 관계를 GenericProjectionFactor를 사용하여 팩터로 표현한다.21

이 팩터는 일반적으로 다음과 같은 변수들을 연결한다:

• 카메라의 3D 포즈 (Pose3)

• 3D 랜드마크의 위치 (Point3)

• 카메라의 내부 파라미터 (캘리브레이션 정보, 예: Cal3_S2)

팩터의 오차 함수는 ’재투영 오차(reprojection error)’로 정의된다. 즉, 현재 추정된 카메라 포즈와 3D 랜드마크 위치를 바탕으로 3D 점을 이미지 평면에 수학적으로 투영한 2D 좌표와, 실제 이미지에서 관측된 2D 특징점 좌표 간의 픽셀 거리 차이를 계산한다.7 SfM 최적화는 이 재투영 오차의 총합을 최소화하는 카메라 포즈와 3D 랜드마크 위치를 찾는 과정이다.

6.3 데이터 연관 및 초기화의 중요성

SfM(그리고 랜드마크 기반 SLAM)의 성공 여부는 비선형 최적화 알고리즘 자체의 성능보다 ‘프론트엔드(front-end)’ 처리 단계에 더 크게 좌우되는 경우가 많다. GTSAM은 강력한 ‘백엔드(back-end)’ 최적화 도구이지만, 프론트엔드 처리는 제공하지 않는다. 성공적인 SfM을 위해 사용자가 별도로 구현해야 하는 핵심적인 프론트엔드 단계는 다음과 같다.21

• 데이터 연관(Data Association): 서로 다른 이미지에서 관측된 특징점들 중 어떤 것들이 동일한 3D 랜드마크에 해당하는지를 정확히 식별하는 과정이다.

• 초기화(Initialization): 비선형 최적화는 초기 추정치에 민감하므로, 최적화가 올바른 해로 수렴할 수 있도록 합리적인 초기 카메라 포즈와 3D 랜드마크 위치를 제공해야 한다.

6.4 GTSfM: 파이썬 기반의 완전한 SfM 파이프라인

GTSfM은 GTSAM을 최적화 백엔드로 사용하는 완전한 End-to-End SfM 파이프라인 프로젝트이다.24 이 프로젝트는 GTSAM의 강력한 최적화 능력과 현대적인 컴퓨터 비전 기술을 결합하여 사용자가 쉽게 SfM을 수행할 수 있도록 돕는다.

GTSfM은 다음과 같은 전체 파이프라인을 포함한다:

• 프론트엔드: 딥러닝 기반 특징점 검출기 및 매칭기(예: SuperPoint, SuperGlue)를 사용하여 이미지 간의 정확한 대응점을 찾는다.

• 데이터 연관 및 초기화: 여러 이미지에 걸쳐 특징점 트랙을 생성하고, 이를 바탕으로 초기 카메라 포즈와 3D 포인트를 추정한다.

• 백엔드(번들 조정): 프론트엔드에서 얻은 정보를 바탕으로 팩터 그래프를 구성하고, GTSAM을 호출하여 모든 카메라 포즈와 3D 랜드마크 위치에 대한 전역 최적화(Global Bundle Adjustment)를 수행한다.

GTSfM은 전체 코드가 파이썬으로 작성되어 있으며, Dask를 이용한 병렬 처리를 지원하여 대규모 데이터셋도 효율적으로 처리할 수 있다. 이는 GTSAM의 파이썬 생태계가 단순한 프로토타이핑을 넘어 실제 복잡한 애플리케이션을 구축할 수 있을 만큼 성숙했음을 보여주는 좋은 사례이다.24

7. 고급 기능 및 심화 주제

7.1 증분 추론: iSAM2와 베이즈 트리

실시간 SLAM과 같이 지속적으로 새로운 데이터가 들어오는 온라인 애플리케이션에서는 iSAM2가 GTSAM의 핵심적인 경쟁력이 된다. iSAM2는 단순히 빠른 최적화기가 아니라, 추정 문제의 확률적 구조와 희소 선형 대수를 베이즈 트리(Bayes Tree)라는 데이터 구조를 통해 연결하는 근본적인 발전을 이룬 알고리즘이다.25 기존의 증분 방법(iSAM)이 주기적으로 전체 문제를 재최적화하는 배치 스텝을 필요로 했던 반면, iSAM2는 새로운 측정값으로 인해 영향을 받는 그래프의 일부만 지능적으로 업데이트하여 진정한 의미의 연속적인 온라인 처리를 가능하게 한다.27

이러한 고효율성은 GTSAM을 자율주행이나 드론 내비게이션과 같이 실시간, 장시간 운영이 필수적인 로보틱스 애플리케이션에서 특히 강력한 도구로 만든다. 하지만 이 성능에는 중요한 전제 조건이 따른다. iSAM2는 문제의 희소성을 유지하기 위해 변수 재정렬을 수행하는데, 이는 문제가 루프 클로저나 GPS 팩터 등에 의해 잘 제약되어 있을 때(well-constrained) 가장 효과적이다. 순수한 시각 주행 거리계처럼 게이지 자유도(gauge freedom)가 존재하는 문제에서는 성능이 저하되거나 불안정해질 수 있다.29 따라서 사용자는 iSAM2의 강력함과 그 한계를 명확히 이해하고, 문제의 특성에 맞게 사용 전략을 세워야 한다.

iSAM2의 핵심 메커니즘은 다음과 같다:

• 베이즈 트리: 팩터 그래프의 변수 소거(elimination) 과정에서 생성되는 조건부 확률 분포들의 관계를 트리 구조로 표현한 데이터 구조이다.25 이는 행렬 인수분해 과정을 확률적으로 해석할 수 있게 해준다.

• 증분 변수 재정렬: 새로운 변수가 추가될 때, 전체 행렬을 재구성하지 않고 베이즈 트리의 일부만 수정하여 효율적인 소거 순서를 동적으로 유지한다.27

• 유동적 재선형화(Fluid Relinearization): 매번 모든 변수를 재선형화하는 대신, 현재 추정치에서 큰 변화가 발생하여 선형화 오차가 커진 변수들만 선택적으로 재선형화하여 계산 비용을 획기적으로 줄인다.28

7.2 자동 미분: Expression의 활용

사용자 정의 팩터를 구현할 때 가장 어렵고 오류가 발생하기 쉬운 부분은 오차 함수의 자코비안(Jacobian) 행렬을 해석적으로 계산하고 코드로 구현하는 것이다. GTSAM 4.0에 도입된 Expression 기능은 이 과정을 자동화하여 개발 생산성을 크게 향상시킨다.9

Expression은 변수, 상수, 또는 다른 Expression들을 입력으로 받는 함수를 표현하는 객체이다. 사용자는 기본적인 연산(덧셈, 곱셈, 행렬 연산 등)과 함수(예: Pose3::transformFrom)를 Expression 객체로 조합하여 복잡한 오차 함수를 구성할 수 있다. 이 과정에서 GTSAM은 연쇄 법칙(chain rule)을 내부적으로 적용하여 최종 오차 함수에 대한 자코비안을 자동으로 계산해준다.9

7.3 사용자 정의 타입 확장: Traits

GTSAM은 Pose2, Point3, SO(3) 등 로봇 공학에서 자주 사용되는 다양한 타입을 기본적으로 제공한다. 하지만 때로는 사용자가 정의한 고유한 타입을 최적화 변수로 사용해야 할 필요가 있다. Traits는 이를 가능하게 하는 C++ 메타프로그래밍 기법이다.9

사용자는 자신의 커스텀 타입에 대해 몇 가지 특성(trait), 예를 들어 해당 타입의 차원(dimension), 영벡터에 해당하는 항등원(identity), 국소 좌표계로의 변환(localCoordinates), 국소 좌표계로부터의 복원(retract) 등을 정의해주기만 하면 된다. 이렇게 Traits를 정의하면, 해당 타입은 GTSAM의 Values 컨테이너에 저장될 수 있으며, 최적화 엔진에서 다른 기본 타입과 마찬가지로 원활하게 처리된다. 이는 라이브러리의 유연성과 확장성을 극대화하는 강력한 기능이다.

8. GTSAM 생태계와 다른 도구와의 비교

8.1 주요 최적화 라이브러리 비교: GTSAM vs. g2o vs. Ceres Solver

GTSAM의 핵심인 팩터 그래프 최적화는 SLAM이나 SfM에 국한되지 않는 매우 일반적인 수학적 도구이다. 이는 제약 조건의 합으로 인수분해될 수 있는 모든 종류의 최적화 문제에 적용될 수 있다. 실제로 GTSAM은 최적 제어 문제인 선형 2차 조절기(Linear Quadratic Regulator, LQR)를 해결하는 데에도 사용될 수 있다.32 LQR의 비용 함수와 동역학 제약을 팩터 그래프로 모델링하면, MAP 추정에 사용되는 것과 동일한 변수 소거 알고리즘을 통해 최적 제어 이득(gain)을 구할 수 있다.

이는 GTSAM이 단순한 ’상태 추정 라이브러리’를 넘어 ’범용 로보틱스 최적화 프레임워크’임을 시사한다. 예를 들어, 하나의 로봇 시스템이 SLAM을 이용한 자기 위치 추정(Perception)과 LQR을 이용한 경로 추종(Control)을 모두 필요로 할 때, 두 문제를 모두 GTSAM이라는 단일 프레임워크 내에서 모델링하고 해결할 수 있다. 이는 소프트웨어 스택을 통합하고, 코드 재사용성을 높이며, 개발 및 유지보수 비용을 절감하는 실질적인 이점을 가져온다.

로봇 공학 및 컴퓨터 비전 분야에는 GTSAM 외에도 g2o와 Ceres Solver와 같은 널리 사용되는 최적화 라이브러리가 있다. 각 라이브러리는 고유한 철학과 장단점을 가지고 있으며, 문제의 특성에 따라 적합한 도구를 선택하는 것이 중요하다.33

아래 표는 세 가지 주요 라이브러리의 특징을 비교한 것이다.

8.2 확장 프로젝트 소개

GTSAM의 유연하고 강력한 아키텍처는 다양한 확장 프로젝트의 기반이 되고 있다.

• gtsam_points 6:

포인트 클라우드 기반 SLAM을 위한 포괄적인 팩터 및 최적화기 모음이다. ICP, GICP, LOAM 등 널리 사용되는 스캔 매칭 알고리즘을 팩터 형태로 제공하여 포인트 클라우드 데이터를 팩터 그래프 최적화 프레임워크에 쉽게 통합할 수 있게 한다.

• GTSfM 24:

앞서 소개한 바와 같이, 완전한 기능을 갖춘 Python 기반 SfM 파이프라인이다.

• mav_gtsam_estimator 35:

소형 무인 항공기(MAV)를 위한 GTSAM 기반 상태 추정 프레임워크로, ROS(Robot Operating System)와 긴밀하게 연동된다.

• vertigo 36:

SLAM에서 발생하는 잘못된 루프 클로저(false positive)에 강인한(robust) 최적화를 수행하기 위한 g2o 및 GTSAM 확장 라이브러리이다. 스위처블 제약(switchable constraints)과 같은 기법을 구현하여 이상치(outlier)의 영향을 효과적으로 제거한다.

9. 결론: GTSAM의 현재와 미래

9.1 GTSAM의 핵심 가치 요약

GTSAM은 로봇 공학 및 컴퓨터 비전 분야의 복잡한 추정 문제를 해결하기 위한 강력하고 현대적인 도구이다. 그 핵심 가치는 다음 세 가지 요소의 결합에 있다.

1. 직관적인 모델링: 팩터 그래프라는 추상화를 통해 문제의 본질적인 구조를 코드로 직접 표현할 수 있게 하여, 복잡한 시스템의 모델링을 단순화한다.

2. 최첨단 성능: iSAM2 알고리즘을 통해 실시간 온라인 애플리케이션에 필수적인 고효율 증분 최적화 성능을 제공한다.

3. 현대적인 확장성: Expression을 통한 자동 미분과 Traits를 통한 사용자 정의 타입 지원은 개발 생산성을 높이고, 라이브러리를 거의 모든 종류의 최적화 문제에 적용할 수 있도록 유연성을 극대화한다.

9.2 최신 동향 및 향후 발전 방향

GTSAM은 활발하게 개발이 진행 중인 프로젝트이며, 최신 릴리즈 노트를 통해 그 발전 방향을 엿볼 수 있다.16

• 하이브리드 추론(Hybrid Inference) 강화: 이산(discrete) 변수(예: 데이터 연관 가설)와 연속(continuous) 변수(예: 포즈)가 혼합된 복잡한 추론 문제에 대한 지원을 지속적으로 강화하고 있다.

• Python 생태계 성숙: 타입 힌트(.pyi) 파일 자동 생성, cibuildwheel을 통한 패키지 배포 간소화 등 Python 래퍼의 사용성과 편의성을 꾸준히 개선하여 더 넓은 사용자층을 포용하고 있다.

• 현대 C++로의 전환: Boost 라이브러리에 대한 의존성을 점진적으로 제거하고 C++17 표준 기능을 적극적으로 활용함으로써, 코드 베이스를 현대화하고 경량화하여 유지보수성과 이식성을 높이고 있다.

이러한 동향은 GTSAM이 고성능 코어는 유지하면서도, 더 복잡한 차세대 로보틱스 및 비전 문제를 해결하고 더 넓은 개발자 커뮤니티와 함께 성장하기 위해 끊임없이 진화하고 있음을 보여준다. 결론적으로, GTSAM은 복잡한 추정 및 최적화 문제에 직면한 연구자와 엔지니어에게 필수적인 도구로 자리매김하고 있다.

10. 참고 자료

1. GTSAM | GTSAM is a BSD-licensed C++ library that implements sensor fusion for robotics and computer vision using factor graphs., https://gtsam.org/
2. Overview — GTSAM 4.0.2 documentation, https://gtsam-jlblanco-docs.readthedocs.io/en/latest/Overview.html
3. GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices. - GitHub, https://github.com/borglab/gtsam
4. What are Factor Graphs? | GTSAM, https://gtsam.org/2020/06/01/factor-graphs.html
5. Factor Graphs and GTSAM: A Hands-on Introduction - GT Digital Repository, https://repository.gatech.edu/server/api/core/bitstreams/b3606eb4-ce55-4c16-8495-767bd46f0351/content
6. koide3/gtsam_points: A collection of GTSAM factors and optimizers for point cloud SLAM - GitHub, https://github.com/koide3/gtsam_points
7. GTSAM factor for FRC - Chief Delphi, https://www.chiefdelphi.com/t/gtsam-factor-for-frc/477303
8. Factor Graphs and GTSAM, https://gtsam.org/tutorials/intro.html
9. GTSAM 4.0 Tutorial Theory, Programming, and … - Jing Dong, https://dongjing3309.github.io/files/gtsam-tutorial.pdf
10. gtsam - ROS Package Overview - ROS Index, https://index.ros.org/p/gtsam/
11. Docs | GTSAM, https://gtsam.org/docs/
12. Practical introduction to Factor Graphs with GTSAM - Canal UNED, https://canal.uned.es/uploads/material/627dfa496f3c000c823dce02/Presentacion_JoseLuisBlanco_10_05_2022.pdf
13. Structure from Motion - CMSC426 Computer Vision, https://cmsc426.github.io/gtsam/
14. Factor Graphs for Navigation Applications: A Tutorial, https://navi.ion.org/content/navi/71/3/navi.653.full.pdf
15. Reducing the uncertainty about the uncertainties, part 1: Linear and …, https://gtsam.org/2021/02/23/uncertainties-part1.html
16. Releases · borglab/gtsam - GitHub, https://github.com/borglab/gtsam/releases
17. Build | GTSAM, https://gtsam.org/build/
18. Landmark-based SLAM — GTSAM 4.0.2 documentation - Read the Docs, https://gtsam-jlblanco-docs.readthedocs.io/en/latest/LandmarkBasedSLAM.html
19. PoseSLAM — GTSAM 4.0.2 documentation - Read the Docs, https://gtsam-jlblanco-docs.readthedocs.io/en/latest/PoseSLAM.html
20. 1 SLAM with LIDAR Measurements, https://dellaert.github.io/20S-3630/notes/5-Lidar-SLAM.pdf
21. Structure from Motion — GTSAM 4.0.2 documentation, https://gtsam-jlblanco-docs.readthedocs.io/en/latest/StructureFromMotion.html
22. 7.4. Visual SLAM - Introduction to Robotics and Perception, https://www.roboticsbook.org/S74_drone_perception.html
23. SFM with OpenCV + GTSAM + PMVS - Nghia Ho, https://nghiaho.com/?p=2379
24. borglab/gtsfm: End-to-end SFM framework based on GTSAM - GitHub, https://github.com/borglab/gtsfm
25. iSAM2: Incremental smoothing and mapping using the Bayes tree, https://dspace.mit.edu/handle/1721.1/78894
26. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree, https://www.cvlibs.net/projects/autonomous_vision_survey/slides/Kaess2012IJRR/top.pdf
27. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree - ResearchGate, https://www.researchgate.net/publication/254098771_iSAM2_Incremental_Smoothing_and_Mapping_Using_the_Bayes_Tree
28. MIT Open Access Articles iSAM2: Incremental Smoothing and Mapping with Fluid Relinearization and Incremental Variable Reordering - DSpace@MIT, https://dspace.mit.edu/bitstream/handle/1721.1/64749/Leonard_iSAM2%20Incremental.pdf?sequence=1&isAllowed=y
29. G2O vs GTSAM vs Ceres Solver from a programmer’s perspective | by Jianzhu Huai, https://medium.com/@jianzhuhuai0108/g2o-vs-gtsam-vs-ceres-solver-from-a-programmers-perspective-f45ac68a90fd
30. [PDF] iSAM2: Incremental smoothing and mapping using the Bayes tree | Semantic Scholar, https://www.semanticscholar.org/paper/iSAM2%3A-Incremental-smoothing-and-mapping-using-the-Kaess-Johannsson/1e7ed145df3268c35a89df9ddea879b02f050f06
31. gtsam 4.3.0 documentation, https://docs.ros.org/en/rolling/p/gtsam/
32. LQR Control Using Factor Graphs - GTSAM, https://gtsam.org/2019/11/07/lqr-control.html
33. g2o vs. Ceres: Optimizing Scan Matching in Cartographer SLAM - arXiv, https://arxiv.org/html/2507.07142v1
34. A Comparison of Graph Optimization Approaches for Pose Estimation in SLAM - Laboratory for Autonomous Systems and Mobile Robotics, https://lamor.fer.hr/images/50036607/2021-ajuric-comparison-mipro.pdf
35. ethz-asl/mav_gtsam_estimator: A GTSAM based state estimation framework. - GitHub, https://github.com/ethz-asl/mav_gtsam_estimator
36. haidai/vertigo - GitHub, https://github.com/haidai/vertigo