12.19 인수 그래프 기반 최적화

1. 인수 그래프 기반 최적화의 개요

인수 그래프 기반 최적화(factor graph-based optimization)는 인수 그래프로 표현된 문제의 최적 해를 찾는 알고리즘이다. 인수 그래프는 변수들 사이의 제약을 명시적으로 표현하며, 그 구조를 활용한 효율적인 최적화가 가능하다. 로봇공학에서는 SLAM, 비주얼-관성 항법, 캘리브레이션, 다중 센서 융합 등 다양한 추정 문제가 인수 그래프 위의 최적화로 풀린다. 본 절에서는 인수 그래프 최적화의 원리, 알고리즘, 그리고 응용을 다룬다.

2. 최적화 문제의 정식화

2.1 MAP 추정

인수 그래프 기반 최적화의 가장 일반적인 형태는 MAP(Maximum A Posteriori) 추정이다. 모든 변수의 결합 사후 확률을 최대화한다.

\hat{\mathbf{X}} = \arg\max_{\mathbf{X}}P(\mathbf{X} | \mathbf{Z})

여기서 \mathbf{X}는 모든 변수, \mathbf{Z}는 모든 측정값이다.

2.2 인수의 곱

인수 그래프에서 결합 분포는 인수의 곱으로 표현된다.

P(\mathbf{X}) \propto \prod_{i}f_i(\mathbf{X}_i)

각 인수 f_i는 변수의 부분 집합 \mathbf{X}_i에 의존한다.

2.3 음의 로그 우도

수치적 안정성과 최적화의 편의를 위해 음의 로그 우도가 사용된다.

J(\mathbf{X}) = -\log P(\mathbf{X}) = -\sum_{i}\log f_i(\mathbf{X}_i)

가우시안 인수의 경우 이는 잔차 제곱의 가중합이 된다.

J(\mathbf{X}) = \sum_{i}\frac{1}{2}\mathbf{r}_i(\mathbf{X}_i)^T\Omega_i\mathbf{r}_i(\mathbf{X}_i)

여기서 \Omega_i는 정보 행렬(공분산의 역)이다.

2.4 비선형 최소제곱

이는 비선형 최소제곱(nonlinear least squares) 문제이다. 비용 함수가 잔차의 제곱합 형태이다.

3. 가우스-뉴턴법

3.1 알고리즘

가우스-뉴턴법은 비선형 최소제곱 문제의 표준 알고리즘이다.

3.1.1 단계

1. 현재 추정 \mathbf{X}_k 주위에서 잔차를 선형화한다.

\mathbf{r}_i(\mathbf{X}_k + \Delta\mathbf{X}) \approx \mathbf{r}_i(\mathbf{X}_k) + \mathbf{J}_i\Delta\mathbf{X}

2. 선형 최소제곱 문제를 푼다.

\Delta\mathbf{X}^* = \arg\min_{\Delta\mathbf{X}}\frac{1}{2}\sum_{i}(\mathbf{r}_i(\mathbf{X}_k) + \mathbf{J}_i\Delta\mathbf{X})^T\Omega_i(\mathbf{r}_i(\mathbf{X}_k) + \mathbf{J}_i\Delta\mathbf{X})

3. 정상 방정식을 푼다.

\left(\sum_{i}\mathbf{J}_i^T\Omega_i\mathbf{J}_i\right)\Delta\mathbf{X}^* = -\sum_{i}\mathbf{J}_i^T\Omega_i\mathbf{r}_i(\mathbf{X}_k)

4. 변수를 갱신한다.

\mathbf{X}_{k+1} = \mathbf{X}_k + \Delta\mathbf{X}^*

5. 수렴 검사 후 반복한다.

3.2 정상 방정식

정상 방정식은 다음과 같이 표현된다.

\mathbf{H}\Delta\mathbf{X} = -\mathbf{b}

여기서

• \mathbf{H} = \sum_{i}\mathbf{J}_i^T\Omega_i\mathbf{J}_i: 헤시안 근사
• \mathbf{b} = \sum_{i}\mathbf{J}_i^T\Omega_i\mathbf{r}_i: 경사

3.3 수렴

가우스-뉴턴법은 잘 조건화된 문제에서 빠르게 수렴한다. 잔차가 0에 가까워지면 2차 수렴 속도를 보인다.

4. 레벤버그-마쿼트법

4.1 댐핑

레벤버그-마쿼트법은 가우스-뉴턴법에 댐핑을 추가한다.

(\mathbf{H} + \lambda\mathbf{D})\Delta\mathbf{X} = -\mathbf{b}

여기서 \lambda는 댐핑 매개변수, \mathbf{D}는 대각 행렬(일반적으로 \mathbf{H}의 대각)이다.

4.2 적응적 댐핑

댐핑 매개변수는 반복 과정에서 적응적으로 조정된다.

• 비용이 감소하면 \lambda를 줄여 가우스-뉴턴에 가깝게 한다.
• 비용이 증가하면 \lambda를 늘려 경사 하강에 가깝게 한다.

4.3 강건성

레벤버그-마쿼트법은 가우스-뉴턴법보다 강건하며, 초기 추정이 좋지 않거나 잘 조건화되지 않은 문제에서도 수렴한다.

5. 희소성의 활용

5.1 희소 헤시안

인수 그래프의 헤시안은 매우 희소하다. 각 인수가 적은 수의 변수에만 의존하기 때문이다.

5.2 희소 선형 솔버

희소 선형 시스템 \mathbf{H}\Delta\mathbf{X} = -\mathbf{b}의 풀이에 효율적인 알고리즘이 사용된다.

5.2.1 콜레스키 분해

\mathbf{H}가 양의 정부호이면 콜레스키 분해 \mathbf{H} = \mathbf{L}\mathbf{L}^T를 사용할 수 있다. 희소 콜레스키는 매우 효율적이다.

5.2.2 QR 분해

자코비안 \mathbf{J}를 직접 QR 분해하는 방법도 있다. 수치적으로 더 안정적이다.

5.2.3 켤레 기울기

큰 시스템에서는 켤레 기울기(conjugate gradient)법이 사용된다. 반복적이며 메모리 효율적이다.

5.3 변수 순서

변수의 순서가 희소 분해의 효율성에 영향을 준다. 좋은 변수 순서가 채움(fill-in)을 최소화한다.

5.3.1 COLAMD

COLAMD(Column Approximate Minimum Degree)는 변수 순서를 결정하는 휴리스틱 알고리즘이다.

5.3.2 메티스

METIS는 그래프 분할 기반 변수 순서 알고리즘이다.

6. 자코비안 계산

6.1 분석 자코비안

분석 자코비안(analytical Jacobian)은 손으로 유도한 자코비안 식이다. 효율적이지만 구현이 복잡하다.

6.2 자동 미분

자동 미분(automatic differentiation)은 자코비안을 자동으로 계산한다. 다음의 두 가지 모드가 있다.

• 순방향 모드: 변수의 수가 적을 때 효율적
• 역방향 모드: 출력의 수가 적을 때 효율적

비선형 최소제곱에서는 순방향 모드가 일반적으로 사용된다.

6.3 수치 미분

수치 미분(numerical differentiation)은 유한 차분으로 자코비안을 근사한다. 단순하지만 부정확할 수 있다.

6.4 라이브러리 지원

Ceres Solver, GTSAM 등의 라이브러리는 자동 미분을 지원한다.

7. 리 군 변수의 최적화

7.1 매개변수화

자세 변수가 SE(3)의 원소일 때 매개변수화가 필요하다. 일반적으로 명목 자세 주위의 작은 변화로 매개변수화한다.

\mathbf{T} = \hat{\mathbf{T}}\exp(\hat{\boldsymbol{\delta}})

여기서 \boldsymbol{\delta}가 최적화 변수이다.

7.2 자코비안

자코비안은 \boldsymbol{\delta}에 대해 계산된다. 리 군의 좌측 또는 우측 야코비안이 등장한다.

7.3 갱신

최적화의 한 단계 후 명목 자세가 갱신된다.

\hat{\mathbf{T}} \leftarrow \hat{\mathbf{T}}\exp(\hat{\boldsymbol{\delta}}^*)

이러한 매개변수화는 자세를 다양체 위에 유지한다.

8. 비선형 최적화의 도전

8.1 국소 최소

비선형 최적화는 국소 최소(local minimum)에 빠질 수 있다. 좋은 초기 추정이 중요하다.

8.2 수렴 속도

수렴 속도는 문제의 조건수에 의존한다. 잘 조건화된 문제는 빠르게 수렴한다.

8.3 발산

매우 비선형적인 문제에서는 알고리즘이 발산할 수 있다. 댐핑이 도움이 된다.

9. 강건 최적화

9.1 이상치의 영향

이상치(outlier) 측정은 표준 최소제곱 최적화의 결과를 크게 왜곡할 수 있다. 큰 잔차의 제곱이 비용 함수에 큰 기여를 하기 때문이다.

9.2 강건 손실 함수

강건 손실 함수는 큰 잔차의 영향을 줄인다.

9.2.1 Huber 손실

\rho(r) = \begin{cases}\frac{1}{2}r^2 & \text{if } |r| \leq \delta \\ \delta(|r| - \frac{\delta}{2}) & \text{otherwise}\end{cases}

작은 잔차는 제곱으로, 큰 잔차는 선형으로 처리한다.

9.2.2 Cauchy 손실

\rho(r) = \frac{c^2}{2}\log\!\left(1 + \frac{r^2}{c^2}\right)

큰 잔차의 영향이 더 강하게 감쇠된다.

9.2.3 Tukey 손실

\rho(r) = \begin{cases}\frac{c^2}{6}\!\left(1 - \!\left(1 - \frac{r^2}{c^2}\right)^3\right) & \text{if } |r| \leq c \\ \frac{c^2}{6} & \text{otherwise}\end{cases}

매우 큰 잔차는 완전히 무시된다.

9.3 가중 최소제곱으로의 변환

강건 손실 함수의 최적화는 반복 가중 최소제곱(Iteratively Reweighted Least Squares, IRLS)으로 변환될 수 있다. 각 반복에서 잔차에 따라 가중치가 조정된다.

10. 점진적 최적화

10.1 동기

새로운 측정이 도착할 때마다 전체 그래프를 다시 최적화하는 것은 비효율적이다. 점진적 최적화는 부분적으로만 갱신한다.

10.2 iSAM

iSAM(incremental Smoothing and Mapping)은 점진적 최적화 알고리즘이다. QR 분해의 점진적 갱신을 사용한다.

10.3 iSAM2

iSAM2는 iSAM의 후속이며, 베이즈 트리(Bayes tree) 자료 구조를 사용한다. 더 효율적이고 유연하다.

10.3.1 베이즈 트리

베이즈 트리는 인수 그래프의 정보 행렬을 트리 구조로 표현한다. 부분 갱신이 효율적이다.

10.3.2 변수 재정렬

iSAM2는 필요에 따라 변수를 재정렬하여 효율성을 유지한다.

11. 마진화

11.1 마진화의 필요성

큰 SLAM 시스템에서 모든 변수를 유지할 수 없다. 오래된 변수를 마진화(marginalize)하여 그래프의 크기를 제어한다.

11.2 슈어 보충

마진화는 슈어 보충(Schur complement)을 사용하여 효율적으로 수행된다.

11.3 정보 행렬의 갱신

마진화 후 정보 행렬이 채워지지만, 정보가 보존된다. 마진화된 변수의 영향이 새로운 인수에 통합된다.

11.4 응용

비주얼-관성 SLAM의 슬라이딩 윈도우 최적화에서 마진화가 사용된다. 윈도우를 벗어나는 변수가 마진화된다.

12. 미리 조정

12.1 동기

정상 방정식의 풀이는 시스템의 조건수에 의존한다. 미리 조정(preconditioning)은 조건수를 개선한다.

12.2 미리 조정자

미리 조정자는 시스템을 다음과 같이 변환한다.

\mathbf{P}^{-1}\mathbf{H}\Delta\mathbf{X} = -\mathbf{P}^{-1}\mathbf{b}

좋은 미리 조정자는 \mathbf{P}^{-1}\mathbf{H}가 항등 행렬에 가깝게 한다.

12.3 종류

• 자코비 미리 조정: 대각 원소만 사용
• 불완전 콜레스키: 콜레스키의 근사
• 블록 미리 조정: 변수의 블록 구조 활용

13. 라이브러리

13.1 GTSAM

GTSAM은 인수 그래프 최적화의 표준 라이브러리이다.

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
gtsam::LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial, params);
gtsam::Values result = optimizer.optimize();

13.2 Ceres Solver

Ceres Solver는 Google의 비선형 최소제곱 라이브러리이다.

ceres::Problem problem;
// 잔차 블록 추가
problem.AddResidualBlock(cost_function, loss_function, parameters);
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

13.3 g2o

g2o는 그래프 최적화에 특화된 라이브러리이다.

13.4 다른 라이브러리

• iSAM/iSAM2
• Eigen Levenberg-Marquardt

14. 실용적 고려사항

14.1 초기 추정

좋은 초기 추정이 수렴에 중요하다. 일반적으로 오도메트리 추정이나 점진적 솔버의 결과가 사용된다.

14.2 수렴 기준

수렴 기준은 다음과 같이 정의될 수 있다.

• 비용 함수의 감소율
• 변수의 변화량
• 잔차의 노름
• 최대 반복 횟수

14.3 디버깅

복잡한 인수 그래프의 디버깅은 어렵다. 시각화와 단계별 검증이 도움이 된다.

14.4 매개변수 튜닝

알고리즘 매개변수(댐핑, 수렴 기준 등)의 튜닝이 필요할 수 있다.

15. 응용

15.1 SLAM

SLAM은 인수 그래프 최적화의 가장 직접적인 응용이다. 자세, 랜드마크, 측정이 그래프를 형성한다.

15.2 비주얼-관성 항법

VIO에서 카메라 자세, 속도, 바이어스, 3차원 점이 변수이며, 시각 측정과 IMU 측정이 인수이다.

15.3 캘리브레이션

다중 센서 캘리브레이션, 매니퓰레이터의 운동학 캘리브레이션 등이 인수 그래프로 표현된다.

15.4 다중 로봇 SLAM

다중 로봇이 협력하여 SLAM을 수행할 때 통합된 인수 그래프가 사용된다.

15.5 상태 추정

일반적인 상태 추정 문제도 인수 그래프 최적화로 풀 수 있다.

16. 응용 예시: 시각적 SLAM의 번들 조정

번들 조정은 비주얼 SLAM의 핵심 최적화 문제이다. 카메라 자세와 3차원 점이 변수, 재투영 오차가 인수이다.

// 변수
for (int i = 0; i < num_cameras; ++i) {
    initial.insert(C(i), camera_poses[i]);
}
for (int j = 0; j < num_points; ++j) {
    initial.insert(P(j), points[j]);
}

// 인수
for (auto& obs : observations) {
    graph.add(ProjectionFactor(obs.pixel, noise, C(obs.camera_id), P(obs.point_id), K));
}

// 최적화
auto result = LevenbergMarquardtOptimizer(graph, initial).optimize();

17. 응용 예시: 그래프 SLAM

자세 그래프 SLAM에서 자세가 변수, 자세 사이의 측정이 인수이다.

for (int i = 0; i < num_poses; ++i) {
    initial.insert(X(i), poses[i]);
}

graph.add(PriorFactor<Pose3>(X(0), prior_pose, prior_noise));
for (int i = 0; i < num_poses - 1; ++i) {
    graph.add(BetweenFactor<Pose3>(X(i), X(i+1), odometry[i], odometry_noise));
}

for (auto& lc : loop_closures) {
    graph.add(BetweenFactor<Pose3>(X(lc.from), X(lc.to), lc.transform, lc.noise));
}

auto result = LevenbergMarquardtOptimizer(graph, initial).optimize();

18. 응용 예시: 비주얼-관성 항법

VIO에서 시각 측정과 IMU 측정이 인수 그래프로 통합된다.

// 자세, 속도, 바이어스 변수
initial.insert(X(i), pose_i);
initial.insert(V(i), velocity_i);
initial.insert(B(i), bias_i);

// IMU 미리 적분 인수
graph.add(ImuFactor(X(i), V(i), X(i+1), V(i+1), B(i), preintegrated_imu));

// 시각 측정 인수
graph.add(ProjectionFactor(pixel, noise, X(i), L(j), K));

// 최적화
auto result = LevenbergMarquardtOptimizer(graph, initial).optimize();

19. 응용 예시: 매니퓰레이터 캘리브레이션

매니퓰레이터의 운동학 매개변수를 캘리브레이션할 때, 매개변수가 변수, 측정이 인수이다.

20. 응용 예시: 다중 센서 융합

GPS, IMU, 카메라, 라이다 등의 측정이 통합된 인수 그래프가 자율 주행에서 사용된다.

21. 학습의 가치

인수 그래프 기반 최적화를 깊이 이해하는 것은 다음과 같은 이점을 제공한다.

• 현대 SLAM의 핵심 알고리즘을 익힌다.
• 비선형 최적화의 응용을 학습한다.
• GTSAM, Ceres 등의 라이브러리를 효과적으로 활용할 수 있다.
• 다양한 추정 문제를 통합적으로 다룰 수 있다.
• 학술 연구에 기여할 수 있다.

22. 학습 권장사항

22.1 직접 구현

작은 문제에서 가우스-뉴턴법을 직접 구현해 본다. 단순한 1차원 또는 2차원 예제부터 시작한다.

22.2 라이브러리 활용

GTSAM이나 Ceres Solver의 튜토리얼을 따라가며 사용법을 익힌다.

22.3 응용 학습

실제 SLAM 시스템을 인수 그래프로 모델링하고 최적화한다.

22.4 디버깅 연습

복잡한 그래프의 디버깅 기법을 익힌다.

23. 본 절의 의의

본 절은 인수 그래프 기반 최적화를 다루었다. 이는 현대 SLAM과 상태 추정의 핵심 알고리즘이며, 다양한 로봇공학 응용에서 광범위하게 활용된다. 인수 그래프 최적화의 이해는 학술 연구와 산업 응용 모두에서 필수적이다.

24. 참고 문헌

• Dellaert, F., & Kaess, M. (2017). “Factor graphs for robot perception.” Foundations and Trends in Robotics, 6(1-2), 1–139.
• Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J. J., & Dellaert, F. (2012). “iSAM2: Incremental smoothing and mapping using the Bayes tree.” International Journal of Robotics Research, 31(2), 216–235.
• Triggs, B., McLauchlan, P. F., Hartley, R. I., & Fitzgibbon, A. W. (2000). “Bundle adjustment—a modern synthesis.” International Workshop on Vision Algorithms, 298–372.
• Agarwal, S., Mierle, K., et al. (2024). Ceres Solver. http://ceres-solver.org
• Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., & Burgard, W. (2011). “g2o: A general framework for graph optimization.” IEEE International Conference on Robotics and Automation, 3607–3613.
• Nocedal, J., & Wright, S. (2006). Numerical Optimization (2nd ed.). Springer.

version: 1.0