11.17 리 군 위의 최적화 기법

1. 리 군 위 최적화의 개요

리 군 위의 최적화(optimization on Lie groups)는 변수가 리 군의 원소이며 비용 함수가 리 군 위에서 정의된 최적화 문제를 다루는 기법이다. 이러한 문제는 자세 추정, 시각적 SLAM, 매니퓰레이터 운동 계획, 캘리브레이션 등 로봇공학의 다양한 영역에서 등장한다. 일반적인 유클리드 공간 위의 최적화와 달리, 리 군 위의 최적화는 변수가 다양체 위에 머물러야 하는 제약을 가진다. 이를 효율적으로 다루기 위해서는 리 군의 구조를 활용한 특별한 기법이 필요하다.

2. 일반 유클리드 공간 위의 최적화

2.1 비제약 최적화

표준적인 비제약 최적화 문제는 다음과 같이 표현된다.

\min_{\mathbf{x} \in \mathbb{R}^n}f(\mathbf{x})

여기서 f : \mathbb{R}^n \to \mathbb{R}은 매끄러운 비용 함수이고, \mathbf{x}는 자유 변수이다.

2.2 표준 알고리즘

표준 알고리즘에는 경사 하강법, 뉴턴법, 가우스-뉴턴법, 레벤버그-마쿼트법 등이 있다. 모두 변수의 갱신이 다음의 형태이다.

\mathbf{x}_{k+1} = \mathbf{x}_k + \mathbf{d}_k

여기서 \mathbf{d}_k는 갱신 방향이다. 이러한 가산 갱신은 유클리드 공간에서 자연스럽다.

3. 리 군 위 최적화의 특수성

3.1 가산 갱신의 문제

리 군 변수 \mathbf{R} \in SO(3)에 대해 가산 갱신은 일반적으로 리 군을 벗어난다.

\mathbf{R}_{k+1} = \mathbf{R}_k + \mathbf{d}_k \notin SO(3)

따라서 가산 갱신을 직접 적용할 수 없다.

3.2 다양체 구조의 활용

해결책은 리 군의 다양체 구조를 활용하는 것이다. 갱신을 리 대수의 원소(접선 벡터)로 표현하고, 지수 사상을 통해 리 군 위에 적용한다.

\mathbf{R}_{k+1} = \mathbf{R}_k\exp([\mathbf{d}_k]_\times)

이 갱신은 리 군 안에 머문다.

3.3 좌측 곱과 우측 곱의 선택

갱신을 좌측 곱 또는 우측 곱으로 적용할 수 있다.

• 우측 곱: \mathbf{R}_{k+1} = \mathbf{R}_k\exp([\mathbf{d}_k]_\times)
• 좌측 곱: \mathbf{R}_{k+1} = \exp([\mathbf{d}_k]_\times)\mathbf{R}_k

두 형태는 본체 좌표계와 공간 좌표계의 차이에 대응된다. 응용에 따라 적절한 형태를 선택한다.

4. 리 군 위의 최적화 문제

4.1 일반적 형태

리 군 위의 최적화 문제는 다음과 같이 표현된다.

\min_{\mathbf{T} \in G}f(\mathbf{T})

여기서 G는 리 군이고 f : G \to \mathbb{R}은 매끄러운 비용 함수이다.

4.2 매개변수화

최적화를 수행하려면 변수의 매개변수화가 필요하다. 일반적으로 명목 자세 \hat{\mathbf{T}}를 유지하고, 작은 변화를 리 대수의 원소로 매개변수화한다.

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

여기서 \boldsymbol{\delta}가 새로운 최적화 변수이다. \boldsymbol{\delta} = \mathbf{0}이 명목 자세에 해당한다.

4.3 비용 함수의 매개변수화

비용 함수도 \boldsymbol{\delta}의 함수로 표현된다.

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

이는 매끄러운 함수이며, \boldsymbol{\delta} = \mathbf{0} 근방에서 최적화가 수행된다.

5. 야코비안의 계산

5.1 명목 자세에서의 야코비안

비용 함수의 자코비안은 \boldsymbol{\delta} = \mathbf{0}에서 계산된다.

\mathbf{J}(\boldsymbol{\delta}) = \frac{\partial f}{\partial\boldsymbol{\delta}}\bigg|_{\boldsymbol{\delta} = \mathbf{0}}

5.2 연쇄 법칙

복잡한 비용 함수의 자코비안은 연쇄 법칙을 사용하여 계산된다. 각 단계에서 리 군 위의 미분이 등장한다.

5.3 좌측과 우측 야코비안

매개변수화가 좌측 또는 우측 곱이냐에 따라 자코비안의 형태가 다르다. 일관된 사용이 중요하다.

6. 가우스-뉴턴법

6.1 기본 형태

비선형 최소제곱 문제는 다음과 같이 표현된다.

\min_{\mathbf{T} \in G}\frac{1}{2}\sum_i\lVert\mathbf{r}_i(\mathbf{T})\rVert^2

여기서 \mathbf{r}_i(\mathbf{T})는 i번째 잔차이다.

6.2 갱신 단계

가우스-뉴턴법의 갱신은 다음과 같다.

\boldsymbol{\delta}^* = -(\mathbf{J}^T\mathbf{J})^{-1}\mathbf{J}^T\mathbf{r}

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

여기서 \mathbf{J}는 잔차 벡터의 자코비안, \mathbf{r}은 모든 잔차의 결합이다.

6.3 수렴

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

7. 레벤버그-마쿼트법

7.1 댐핑

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

\boldsymbol{\delta}^* = -(\mathbf{J}^T\mathbf{J} + \lambda\mathbf{I})^{-1}\mathbf{J}^T\mathbf{r}

여기서 \lambda는 댐핑 매개변수이다. 큰 \lambda는 경사 하강법에 가깝고, 작은 \lambda는 가우스-뉴턴법에 가깝다.

7.2 적응적 댐핑

댐핑 매개변수는 반복 과정에서 적응적으로 조정된다. 비용이 감소하면 \lambda를 줄이고, 증가하면 늘린다.

7.3 안정성

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

8. 신뢰 영역 방법

8.1 개념

신뢰 영역 방법(trust region method)은 갱신의 크기를 제한하는 방법이다. 신뢰 영역 내에서 비용 함수의 2차 모형을 최소화한다.

8.2 부분 문제

부분 문제는 다음과 같다.

\min_{\boldsymbol{\delta}}\mathbf{g}^T\boldsymbol{\delta} + \frac{1}{2}\boldsymbol{\delta}^T\mathbf{H}\boldsymbol{\delta} \quad \text{s.t.} \quad \lVert\boldsymbol{\delta}\rVert \leq \Delta

여기서 \mathbf{g}는 경사, \mathbf{H}는 헤시안 근사, \Delta는 신뢰 영역 반지름이다.

8.3 갱신

해 \boldsymbol{\delta}^*를 사용하여 명목 자세를 갱신한다.

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

신뢰 영역 반지름 \Delta는 비용 감소의 정도에 따라 조정된다.

9. 야코비안 자동 미분

9.1 자동 미분의 도전

리 군 위의 변수에 대해 자동 미분을 적용하는 것은 어렵다. 변수가 다양체 위에 머물러야 하기 때문이다.

9.2 매개변수화 클래스

Ceres Solver, GTSAM 등은 매개변수화 클래스를 통해 리 군 위의 변수를 다룬다. 매개변수화는 리 대수의 원소(접선 벡터)와 리 군 원소 사이의 변환을 정의한다.

9.3 리 군 자동 미분

Sophus, manif 등의 라이브러리는 리 군의 자동 미분을 지원한다. 자동 미분 결과가 리 대수의 원소로 표현된다.

10. 비제약 vs 제약 최적화

10.1 비제약 처리

리 군의 매개변수화를 통해 다양체 제약을 자동으로 처리할 수 있다. 명목 자세 주위의 매개변수화로 비제약 문제로 변환된다.

10.2 제약의 명시적 처리

다른 접근법은 단위 노름 제약 등을 명시적으로 처리하는 것이다. 라그랑주 곱셈자나 페널티 방법이 사용된다.

10.3 비교

매개변수화 방법이 더 우아하고 효율적이며, 일반적으로 선호된다.

11. 시각적 SLAM의 번들 조정

11.1 문제 정의

번들 조정은 시각적 SLAM의 핵심 최적화 문제이다. 카메라 자세와 3차원 점 위치를 동시에 최적화한다.

\min_{\{\mathbf{T}_i\}, \{\mathbf{p}_j\}}\sum_{i, j}\lVert\mathbf{u}_{ij} - \pi(\mathbf{T}_i, \mathbf{p}_j)\rVert^2

여기서 \mathbf{T}_i는 i번째 카메라의 자세, \mathbf{p}_j는 j번째 3차원 점, \mathbf{u}_{ij}는 측정된 픽셀 좌표, \pi는 투영 함수이다.

11.2 변수의 처리

카메라 자세는 SE(3)의 원소이며, 매개변수화를 통해 최적화된다. 3차원 점은 일반적으로 유클리드 공간의 원소이다.

11.3 알고리즘

번들 조정에는 가우스-뉴턴법이나 레벤버그-마쿼트법이 사용된다. 큰 규모의 문제에서는 희소 행렬 구조를 활용한다.

11.4 라이브러리

g2o, GTSAM, Ceres Solver 등의 라이브러리가 번들 조정을 지원한다.

12. 그래프 SLAM

12.1 문제 정의

그래프 SLAM은 노드(로봇의 자세)와 엣지(상대 자세)로 구성된 그래프 위의 최적화 문제이다.

\min_{\{\mathbf{T}_i\}}\sum_{(i, j) \in E}\lVert\log(\mathbf{T}_{ij}^{-1}\mathbf{T}_i^{-1}\mathbf{T}_j)\rVert^2

여기서 \mathbf{T}_{ij}는 i와 j 사이의 측정된 상대 자세, E는 엣지의 집합이다.

12.2 잔차의 계산

각 엣지의 잔차는 측정된 상대 자세와 계산된 상대 자세의 차이이다. 차이는 로그 사상으로 트위스트로 표현된다.

12.3 알고리즘

그래프 SLAM도 비선형 최소제곱 문제이며, 가우스-뉴턴법이나 레벤버그-마쿼트법이 사용된다.

13. 자세 평균화

13.1 문제 정의

여러 자세 측정값의 평균을 계산하는 문제는 다음과 같이 표현된다.

\min_{\mathbf{R} \in SO(3)}\sum_i\lVert\log(\mathbf{R}^T\mathbf{R}_i)\rVert^2

이는 측지 거리의 제곱합을 최소화하는 자세를 찾는 문제이다.

13.2 알고리즘

자세 평균화는 반복 알고리즘으로 풀린다. 카르치 평균(Karcher mean)이라고도 한다.

13.3 응용

자세 평균화는 캘리브레이션, 자세 추정의 후처리, 분포 추정 등에서 활용된다.

14. 캘리브레이션

14.1 손-눈 캘리브레이션

손-눈 캘리브레이션은 매니퓰레이터의 손과 카메라 사이의 변환을 추정하는 문제이다. 측정된 변환들로부터 미지의 변환을 추정한다.

\mathbf{A}_i\mathbf{X} = \mathbf{X}\mathbf{B}_i

여기서 \mathbf{A}_i와 \mathbf{B}_i는 측정된 변환들, \mathbf{X}는 미지의 변환이다. 이는 리 군 위의 비선형 문제이며, 리 군 기반 알고리즘이 사용된다.

14.2 카메라 캘리브레이션

카메라의 내부 매개변수와 외부 매개변수의 캘리브레이션도 리 군 위의 최적화이다.

15. 매니퓰레이터의 운동 계획

15.1 작업 공간 경로 계획

매니퓰레이터의 작업 공간 경로 계획은 SE(3) 위의 경로를 찾는 문제이다. 시작 자세에서 목표 자세까지의 매끄러운 경로를 계산한다.

15.2 최적 제어

매니퓰레이터의 최적 제어 문제는 시간, 에너지, 부드러움 등을 최소화하는 경로를 찾는다. 변수가 리 군 위에 정의된다.

15.3 예측 제어

모델 예측 제어(MPC)는 미래 상태를 예측하여 최적 제어 입력을 계산한다. 자세 변수의 처리에 리 군 위의 최적화가 활용된다.

16. 학습 기반 방법

16.1 신경망과 리 군

신경망의 출력으로 자세를 표현할 때 리 군의 구조가 고려된다. 일반적으로 단위 쿼터니언이나 6D 표현이 사용된다.

16.2 손실 함수

학습의 손실 함수는 자세 사이의 측지 거리이다.

L = \lVert\log(\mathbf{R}_{\text{pred}}^T\mathbf{R}_{\text{true}})\rVert

16.3 미분 가능성

학습을 위해서는 손실 함수가 미분 가능해야 하며, 리 군 위의 자동 미분이 활용된다.

17. 다양체 최적화의 일반화

17.1 리만 다양체

리 군은 리만 다양체의 특수한 경우이다. 일반 리만 다양체 위의 최적화도 활발히 연구되고 있다.

17.2 일반화된 알고리즘

리만 경사 하강법, 리만 뉴턴법 등의 일반화된 알고리즘이 있다. 이들은 리 군 위의 알고리즘의 자연스러운 일반화이다.

17.3 라이브러리

Manopt, Pymanopt 등의 라이브러리는 일반 다양체 위의 최적화를 지원한다.

18. 라이브러리와 도구

18.1 Ceres Solver

Google의 Ceres Solver는 비선형 최소제곱 최적화 라이브러리이며, 리 군 위의 변수를 지원한다.

ceres::Problem problem;
problem.AddParameterBlock(quaternion, 4, new ceres::QuaternionParameterization);

18.2 GTSAM

Georgia Tech의 GTSAM은 그래프 기반 SLAM 라이브러리이며, 리 군 변수의 자동 미분을 내장한다.

18.3 g2o

g2o는 그래프 최적화 라이브러리이며, SLAM과 번들 조정에 사용된다.

18.4 Sophus

Sophus는 리 군 라이브러리이며, 다른 최적화 라이브러리와 함께 사용된다.

18.5 manif

manif도 리 군 라이브러리이며, 자코비안과 자동 미분을 지원한다.

19. 수치적 안정성

19.1 작은 회전의 처리

작은 회전에 대한 지수 사상과 로그 사상의 계산은 수치적으로 주의가 필요하다. 테일러 급수 전개나 특수한 알고리즘이 사용된다.

19.2 \pi 회전 근처

\pi 회전 근처에서는 로그 사상이 특이점을 가진다. 이 영역에서는 별도의 처리가 필요하다.

19.3 정규화

장시간 반복 후 자세의 단위성이 손상될 수 있다. 정기적인 정규화가 필요하다.

20. 학습의 가치

리 군 위의 최적화를 깊이 이해하는 것은 다음과 같은 이점을 제공한다.

• 자세 추정과 시각적 SLAM의 핵심 알고리즘을 이해할 수 있다.
• 매니퓰레이터의 운동 계획을 효율적으로 다룰 수 있다.
• 캘리브레이션 문제를 정확히 풀 수 있다.
• 학습 기반 방법에서 자세를 적절히 처리할 수 있다.
• 학술 문헌의 표준 알고리즘을 이해할 수 있다.

21. 응용 예시: 자세 추정의 ESKF

오차 상태 칼만 필터(ESKF)는 자세 변수를 명목 상태와 작은 오차 상태로 분리한다. 명목 상태는 SE(3)의 원소이고, 오차 상태는 리 대수의 원소이다. 측정 갱신 후 오차가 명목 상태로 통합된다.

22. 응용 예시: 매니퓰레이터의 역기구학

수치적 역기구학에서 자코비안의 의사역행렬을 사용한 갱신이 리 군 위의 가우스-뉴턴법의 일종이다. 자세 오차가 트위스트로 표현되고, 관절 변수가 갱신된다.

23. 응용 예시: 손-눈 캘리브레이션

손-눈 캘리브레이션의 비선형 최소제곱 문제는 리 군 위의 최적화로 풀린다. 미지의 변환이 SE(3)의 원소이며, 잔차가 리 대수의 형태로 표현된다.

24. 참고 문헌

• Absil, P. A., Mahony, R., & Sepulchre, R. (2008). Optimization Algorithms on Matrix Manifolds. Princeton University Press.
• Boumal, N. (2023). An Introduction to Optimization on Smooth Manifolds. Cambridge University Press.
• Forster, C., Carlone, L., Dellaert, F., & Scaramuzza, D. (2017). “On-manifold preintegration for real-time visual-inertial odometry.” IEEE Transactions on Robotics, 33(1), 1–21.
• Sola, J., Deray, J., & Atchuthan, D. (2018). “A Micro Lie Theory for State Estimation in Robotics.” arXiv:1812.01537.
• 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

version: 1.0