입문 Visual SLAM 14강 : 4장. Lie군과 Lie 대수

Preface

이 문서는 중국어 원서인 “입문 Visual SLAM 이론에서 연습까지 14 강(视觉SLAM十四讲 从理论到实践)” 책의  원저자로부터 한글 번역 허가를 받고 구글 번역기를 이용하여 작성된 문서입니다. 본 문서는 아래의 Contribution을 특징으로 합니다.

 

• 중국어 전공 서적을 구글 번역기를 이용해 한글로 초벌 번역했습니다.
• 초벌 번역 후 매끄럽지 않은 문장은 문맥에 맞게 수정되었습니다. 
• 문서 내용 중 참고할만한 웹문서를 코멘트로 추가했습니다.  
• SLAM 연구에서 주로 사용되는 용어는 한글로 번역된 용어보다 주로 사용되는 영어로 된 용어 그대로 표시하였습니다.

그럼에도 불구하고 부정확하거나 매끄럽지 않은 부분이 있을 수 있습니다. 그런 부분은 코멘트로 제안해주시면 반영하도록 노력하겠습니다. 또한 읽으시다가 잘 이해가 가지 않는 부분도 코멘트로 질문해주시면 답변해드리도록 하겠습니다.

 

번역 참가자: 

신동원 (前 광주과학기술원 박사과정)

김선호 (前 VIRNECT 선임연구원)

조원재 (前 일본국립농업기술혁신공학센터 연구원)

장형기 (前 Imperial College London 석사과정)

박준영 (前 광주과학기술원 석사과정)

 

2018년 10월 1일

신동원 드림

---

제4장. Lie군과 Lie 대수

주요 목표 1. Lie 그룹과 Lie 대수의 개념을 이해하고 SO (3), SE (3) 및 해당 Lie 대수의 표현을 익 힙니다. 2. BCH 근사법의 의미를 이해합니다. 3. Lie 대수 (Lie algebra)에서 섭동 모델을 배웁니다. 4. Sophus를 사용하여 Lie 대수에 대한 연산을 수행합니다.

  지난 강의에서는 회전 행렬, 회전 벡터, 오일러 각, 쿼터니언 등 3차원 세계에서의 강체 운동에 대한 설명을 소개했습니다. 앞에서는 회전의 표현에 초점을 맞추었지만, SLAM에서는 회전과 이동이 포함된 카메라의 포즈 값을 조금씩 업데이트하면서 현재 관측치와 가장 잘 일치하는 카메라 포즈의 최적 값을 찾아야 합니다. 일반적인 방법은 최적화 문제로 그것을 구성하고, 최적의 R을 풀고, t를 최소화하여 오류를 최소화하는 것입니다. 

  앞에서 언급했듯이 회전 행렬 자체는 제약조건이 있습니다 (직교 행렬이어야 하며 행렬식의 값이 1이어야 함). 이는 회전 행렬이 최적화 변수로 사용될 때 최적화를 어렵게 하는 추가적인 제약 조건을 야기합니다. Lie 군과 Lie 대수의 변환 관계를 통해 우리는 포즈 추정을 제약 없는(unconstrained) 최적화 문제로 전환하고 솔루션을 단순화할 수 있습니다. 독자가 Lie 군과 Lie 대수의 기본 지식을 가지고 있지 않을 수도 있다는 것을 고려하여, 우리는 가장 기본적인 지식으로 시작할 것입니다.

역자주: Lie 군과 Lie 대수는 대수학에 관련된 것으로서 카메라의 포즈와 포인트의 위치에 대한 식을 최적화 할 때 유용하게 쓰입니다. 제가 느끼기에 수식과 이론적인 내용이 많아 이 책에서도 어려운 부분에 속하는 것 같습니다. 한 번만에 이해하려고 하시기보다는 여러 다른 자료들도 같이 보면서 이해하시는 것이 좋을 것 같습니다 :-).  

이해하는데 도움이 되는 참고문헌:
edward0im.github.io/mathematics/2020/05/01/lie-theory/#org8cdcc4b
STATE ESTIMATION FOR ROBOTICS - Timothy D. Barfoot의 7장 Matrix Lie Groups
gtsam.org/2021/02/23/uncertainties-part1.html
A micro Lie theory for state estimation in robotics

4.1 Lie군과 Lie 대수 기초

  지난 강의에서는 회전 행렬과 변환 행렬의 정의를 도입했습니다. 당시 우리는 3차원 회전 행렬이 특수 직교 군(Special Orthogonal group) S0(3)을 구성하고 변환 행렬이 특수 유클리드 군 (Special Euclidean Group) SE (3)를 구성한다고 소개했습니다.

  그러나 당시 우리는 군의 의미를 자세히 설명하지 않았습니다. 사실, 임의의 두 회전 행렬 $\mathbf{R}_1, \mathbf{R}_2$, 가 있을 때, 행렬 덧셈의 정의에 따르면 이는 더 이상 회전 행렬이 아닙니다. 

  변환 행렬에서도 마찬가지입니다. 우리는 이 두 행렬에 잘 정의된 덧셈이 없는 것을 발견했습니다. 그나마 더 나은 연산은 곱셈입니다. S0(3)과 SE(3)은 곱셈과 관련하여 닫힌 연산입니다 :

  곱셈은 회전 또는 변환 행렬의 합성에 해당하며 두 회전 행렬의 곱은 두 개의 회전이 연속적으로 이루어졌음을 의미합니다. 이 작업 집합 하나에 대해서만 군이라고 부릅니다.

4.1.1 군

  다음으로 우리는 추상적인 대수에 대한 지식을 갖게 될 것입니다. 이것이 Lie 군과 Lie 대수를 이야기하는 데 필요한 조건이라고 생각하지만 실제로 수학 및 물리학과의 학생들을 제외하고 대부분의 학생들은 학부 연구에서 이러한 지식을 배우지 않을 것입니다. 먼저 기본적인 지식을 먼저 살펴보겠습니다.

  군은 집합과 연산으로 이루어진 대수적 구조입니다. 어떤 집합을 A라고 하고 어떤 연산을 ·로 표시하면 군은 G = (A, ·)로 나타낼 수 있습니다. 군은 다음 조건을 충족해야 합니다.

( 1. 닫힘성(Closure), 2. 결합성(Associativity), 3. 항등성 (identity element), 4. 역(inverse) )

 

  회전 행렬 집합과 행렬 곱셈이 군을 형성하고 또한 변환 행렬과 행렬 곱셈이 군을 형성한다는 것을 알 수 있습니다. (따라서 회전 행렬 군 및 변환 행렬 군이라고 할 수 있습니다.) 행렬의 공통적인 군은 다음과 같습니다.

 

• 일반 선형 군 GL(n)은 행렬 곱셈에 의해 그룹화된 n x n의 가역 행렬을 나타냅니다.
• 특수 직교 군 S0(n)은 S0(2) 및 S0(3)이 가장 많이 사용되는 회전 행렬 군이라고도 합니다.
• 특별 유클리드 군 SE(n)는 SE(2) 및 SE(3)와 같은 앞서 언급 한 n 차원 유클리드 변환입니다.

  군의 구조는 해당 군에 대한 특정 연산이 좋은 속성을 갖는 것을 보장합니다. 군 이론은 군의 다양한 구조와 속성에 대한 이론이지만 여기서 자세히 소개하지 않습니다. 관심 있는 독자는 최근의 대수 교과서를 참조할 수 있습니다. Lie 군은 연속적인(continuous) 속성을 가진 군을 나타냅니다. 정수 군 $\mathbb{Z}$와 같은 이산적인 군에는 연속성이 없으므로 Lie 군이 아닙니다. SO(n)과 SE(n)은 실제 공간에서 연속적입니다. 우리는 3차원 공간에서 강체가 연속적으로 계속 움직일 수 있다는 것을 직관적으로 상상할 수 있습니다. 그래서 그들은 모두 Lie 군입니다. S0(3)과 SE(3)는 카메라 포즈 추정에 특히 중요하기 때문에, 우리는 주로 이 두 가지 Lie 군에 대해 논의합니다. 독자가 Lie 군의 이론적인 특성에 관심이 있다면, [21]을 참고하십시오.

  아래에서 우리는 보다 간단한 SO(3) 토론으로 시작하고, 각 Lie 군에는 상응하는 Lie 대수가 있음을 알게 될 것입니다. 먼저 SO(3)에 대한 Lie 대수 so(3)을 소개합니다.

4.1.2  Lie 군의 유도

  임의의 회전 행렬 R만을 고려하면, 우리는 그것이 다음을 만족함을 알 수 있습니다:

  이제, R은 시간에 따라 계속 변화하는 카메라의 회전으로 생각하며 시간의 함수 $\mathbf{R}(t)$로 표기합니다.

  이는 여전히 회전 행렬이기 때문에 다음의 성질을 만족합니다.

  방정식의 양측에서 시간에 대해서 미분을 수행하면 다음과 같이 됩니다. (곱의 미분)

  따라서 $\dot{\mathbf{R}} (t) \mathbf{R} {(t)^\mathrm{T}}$가 skew-symmetric(반대칭)행렬임을 알 수 있습니다. 식 (3.3)에서 외적을 소개할 때, $^\wedge$ 연산자를 소개했는데 이는 벡터를 반대칭 행렬로 바꾸어 줍니다. 반대로 생각하면, 어떤 반대칭 행렬에 대해서도, 그것에 대응하는 벡터를 찾을 수 있습니다. 즉, 연산자 $^{\vee}$를 넣음으로써 다음을 알 수 있습니다. 

  따라서, $\dot{\mathbf{R}} (t) \mathbf{R} {(t)^\mathrm{T}}$ 가 반대칭 행렬이기 때문에 이에 상응하는 3차원 벡터 $\boldsymbol{\phi} (t) \in \mathbb{R}^3$ 를 찾을 수 있습니다.

  다음 이 방정식의 양쪽에 $\mathbf{R}(t)$을 곱합니다. R은 직교 행렬이므로 다음과 같습니다.

  이는 회전 행렬에 시간에 대한 미분을 취하고 $\boldsymbol{\phi}^\wedge (t)$을 왼쪽에 곱한 것처럼 보입니다. 다음으로 시간 $t_0=0$이라고 할 때, 회전 행렬을 $\mathbf{R}(0) = \mathbf{I}$ 로 정의할 수 있습니다. 그리고 $\mathbf{R}(t)$는 t=0 주변의 1 차 테일러 전개로 근사할 수 있습니다. ($\mathbf{R}(t)$ 의 1차 테일러 전개 수행)

역자주:

  $\boldsymbol{\phi}$는 R의 미분을 반영하므로 SO(3) 원점에 대해 접선 공간이라고 합니다. 만약 시간 $t$가 $t_0$에 가까워진다면 $\boldsymbol{\phi}(t)$가 $\boldsymbol{\phi}(t_0) = \boldsymbol{\phi}_0$에 가까워진다고 할 수 있습니다. 그런 다음 수식 (4.8)에 따라 다음을 알 수 있습니다.

  위 공식은 $\mathbf{R}$에 대한 미분 방정식이며 $\mathbf{R}(0) = \mathbf{I}$ 의 초기 값을 가지며 솔루션은 다음과 같습니다. (벡터 선형 미분방정식의 해)

  독자는 위의 방정식이 미분 방정식과 초기 값 모두에 대해 유지되는지 확인할 수 있습니다. 이것은 t = 0 주위에서 회전 행렬이 $\exp \left( \boldsymbol{\phi}_0^\wedge t \right)$을 통해 계산될 수 있음을 의미합니다. 우리는 회전 행렬 R이 지수 관계를 통해 또 다른 반대칭 행렬 $\boldsymbol{\phi}_0^\wedge t$ 와 관련되어 있음을 알 수 있습니다. 그러나 행렬의 지수 승(exponential)은 무엇입니까? 여기서 우리는 명확히 해야 할 두 가지 질문이 있습니다.

 

1. 특정 순간에 $\mathbf{R}$이 주어지면 $\mathbf{R}$의 국부 미분 관계를 나타내는 $\boldsymbol{\phi}$를 찾을 수 있습니다. $\mathbf{R}$에 연관되는 $\boldsymbol{\phi}$는 무엇을 의미합니까? 우리는 $\boldsymbol{\phi}$가 $\mathrm{SO}(3)$에서의 Lie 대수 $\mathfrak{so}(3)$에 해당한다고 말할 수 있습니다.
2. 둘째, 벡터 $\boldsymbol{\phi}$가 주어질 때, 행렬 $\exp (\boldsymbol{\phi} ^\wedge )$ 는 어떻게 계산되는가? 반대로, $\mathbf{R}$이 주어지면 $\boldsymbol{\phi}$를 계산하기 위해 반대의 연산이 가능합니까? 사실, 이것은 Lie 군과 Lie 대수사이의 지수 / 로그 매핑입니다.

아래에서 두 가지 문제에 대해 설명해보겠습니다.

4.1.3 리 대수의 정의

이제 Lie 대수에 대한 엄격한 정의를 하겠습니다. 각 Lie 군에는 그에 상응하는 Lie 대수가 있습니다. Lie 대수는 원점 근처에서 Lie 군의 지역적 구조(Tangent space)를 설명합니다. 일반적인 Lie 대수의 정의는 다음과 같습니다.

  Lie 대수는 벡터 공간 $\mathbb{V}$, 실수 집합 $\mathbb{F}$ 및 이항 연산 $[,]$ (두 개의 항을 가지는 연산)으로 구성됩니다. 다음 성질을 만족하면 $(\mathbb{V}, \mathbb{F}, [,])$는 Lie대수라고 불리며 $\mathfrak{g}$로 표시됩니다.

이항 연산 $[,]$은 Lie Brackets이라고 합니다. 우리는 Lie bracket의 연산자에 대한 많은 속성을 알아야 합니다. 일반적인 군에서 더 간단한 이항 연산과 비교할 때, Lie bracket은 두 원소의 차이를 나타냅니다. 여기서는 결합 법칙을 필요로 하지는 않지만 자기 자신으로 Lie Bracket을 수행했을 때 0이 되어야 합니다. 예를 들어, 3차원 벡터 $\mathbb{R}^3$에 정의된 외적 $\times$ 는 일종의 Lie Bracket이므로 $\mathfrak{g} = (\mathbb{R}^3, \mathbb{R}, \times)$ 는 Lie 대수를 구성합니다. 독자는 외적 연산자에 대해 위의 네 가지 성질을 증명해볼 수 있습니다.

4.1.4 리대수 so(3)

  앞에서 언급한 $\boldsymbol{\phi}$는 일종의 Lie 대수입니다. $\mathrm{SO}(3)$에 해당하는 Lie 대수는 $\mathbb{R}^3$에 정의된 벡터이며 우리는 $\boldsymbol{\phi}$라고 부릅니다. 이전의 유도에 따르면, 각 $\boldsymbol{\phi}$는 반대칭 행렬을 생성할 수 있습니다 :

이 정의에서 $\boldsymbol{\phi}_1, \boldsymbol{\phi}_2$의 Lie bracket 연산은 다음과 같습니다.

  독자는 이 정의에 따른 Lie bracket이 위의 속성을 충족시키는지 확인할 수 있습니다. $\boldsymbol{\phi}$는 반대칭 행렬과 밀접한 관계가 있기 때문에, 모호성이 없는 경우, $\mathfrak{so}(3)$의 요소는 3차원 벡터 또는 3차원 반대칭 행렬로 나타낼 수 있습니다.

어떤 책들은 기호 $\widehat{\boldsymbol{\phi}}$를 기호 $\boldsymbol{\phi}^\wedge$로 표시하기도 하지만 의미는 동일합니다. 이 시점에서, 우리는 이미 $\mathfrak{so}(3)$의 내용을 이해했습니다. 그것들은 회전 행렬의 미분을 표현하는 반 대칭 행렬에 각각 대응하는 3차원 벡터의 모음입니다. $\mathrm{SO}(3)$과의 관계는 exponential map에 의해 다음과 같이 주어집니다.

Exponential map은 나중에 더 자세히 소개될 것입니다. 우리는 $\mathfrak{so}(3)$을 도입했으므로 먼저 $\mathrm{SE}(3)$에 대응하는 대수 (Lie algebra)를 살펴볼 것입니다.

4.1.5 리 대수 se(3)

  $\mathrm{SE}(3)$의 경우 이와 관련한 Lie 대수 $\mathfrak{se}(3)$도 있습니다. $\mathfrak{so}(3)$와 유사하게 $\mathfrak{se}(3)$은 $\mathbb{R}^6$공간에 있습니다.

각 $\mathfrak{se}(3)$요소를 6차원 벡터 인 $\boldsymbol{\xi}$로 나타냅니다. 앞의 3차원은 translation을 나타내며 $\boldsymbol{\rho}$로 표시되고 그 뒤의 3차원은 회전을 나타내는 $\boldsymbol{\phi}$이며, 이는 본질적으로 $\mathfrak{so}(3)$입니다. 동시에 $^\wedge$ 기호의 의미를 확장했습니다. $\mathfrak{se}(3)$ 에서 6차원 벡터는 $^\wedge$ 기호를 사용하여 다음의 4차원 행렬로 변환됩니다.

 

우리는 여전히 $^\wedge$와 $^\vee$기호를 사용하여 "벡터에서 행렬"과 "행렬에서 벡터"사이의 관계를 참조하여 $\mathfrak{so}(3)$과 일관성을 유지합니다. 그들은 여전히 일대일 대응입니다. 독자는 단순히 $\mathfrak{se}(3)$를 translation과 rotation($\mathfrak{so}(3)$) 요소로 구성된 벡터로 해석할 수 있습니다. 비슷하게, Lie 대수 $\mathfrak{se}(3)$도 $\mathfrak{so}(3)$과 비슷한 Lie 대괄호 연산을 가집니다.

독자는 여기서 Lie 대수의 정의를 충족하는지 확인할 수 있습니다. 지금까지 우리는 두 개의 중요한 Lie 대수 $\mathfrak{so}(3)$와 $\mathfrak{se}(3)$를 보았습니다.

4.2 Exponential and Logarithmic Mapping

4.2.1 Exponential map of $\mathrm{SO}(3)$

  이제 두 번째 질문 인 $\exp ( \boldsymbol{\phi}^{\wedge} )$을 계산하는 방법을 생각해 봅시다. 이는 행렬의 지수화를 나타냅니다. Lie 군과 Lie 대수에서는 Exponential map이라고 합니다. 먼저 우리는 $\mathfrak{so}(3)$의 Exponential map을 논의한 다음 $\mathfrak{se}(3)$의 사례를 논의할 것입니다.

  임의의 행렬의 Exponential map은 Taylor expansion으로 쓰일 수 있지만, 수렴하는 경우 그 결과는 여전히 행렬입니다. 

  마찬가지로, $\boldsymbol{\phi} \in \mathfrak{so}(3)$에 대해서도 다음과 같이 Exponential map을 정의할 수 있습니다.

  이 정의에 대해 자세히 살펴보겠습니다. 

  그러나 이 정의는 매트릭스의 무한한 지수 승을 계산해야 하기 때문에 바로 계산할 수 없습니다. 아래에서 우리는 Exponential map을 계산하는 편리한 방법을 도출합니다. $\boldsymbol{\phi}$는 3차원 벡터이기 때문에 $\theta$와 $\mathbf{n}$으로 각각 표시되는 길이와 방향을 정의한 다음 $\boldsymbol{\phi} = \theta \mathbf{n}$로 정의 할 수 있습니다. 여기서 $\mathbf{n}$은 길이가 1인 방향 벡터, 즉 $\| \mathbf{n} \| =1$인 벡터입니다. 첫번째로 단위 벡터 $\mathbf{n}$에 대해서 다음의 2가지 성질이 있습니다.

  그리고

  이 두 방정식은 $\mathbf{n}^\wedge$의 고차 항을 처리하는 방법을 제공합니다. 따라서 Exponential map은 다음과 같이 작성할 수 있습니다.

마침내 다음을 얻을 수 있습니다.

  이전의 강좌에서 Rodrigues 수식인 수식 (3.15)와 정확히 같은 것을 알 수 있습니다. 이것은 $\mathfrak{so}(3)$이 실제로 회전 벡터로 구성된 공간이고 Exponential map은 Rodrigues 수식임을 보여줍니다. 이들을 통해 $\mathfrak{SO}(3)$에 있는 회전 행렬에 $\mathfrak{so}(3)$ 중 하나를 대응시킬 수 있습니다. 반대로, Logarithm map을 정의하면 $\mathfrak{SO}(3)$의 요소를 $\mathfrak{so}(3)$에 매핑할 수 있습니다.

Exponential map과 마찬가지로 Taylor를 사용하여 Logarithm map을 확장할 필요가 없습니다. 앞의 제3강에서 회전 행렬을 기반으로 한 대응되는 Lie 대수를 계산하는 방법, 즉 수식 (3.17)을 사용하여 trace의 특성을 사용하여 회전 각도와 회전축을 개별적으로 해결하는 방법을 이미 소개했습니다.

  여기서 우리는 Exponential map의 계산 방법을 소개했습니다. Exponential map의 성질은 무엇입니까? 어떤 회전 행렬 $\mathbf{R}$에 대해 고유한 $\boldsymbol{\phi}$를 찾을 수 있습니까? 불행하게도, 지수 맵은 1대1 맵핑 함수가 아니라 1대 다 맵핑 함수입니다. 이것은 $\mathrm{SO}(3)$의 각 요소가 이에 해당하는 $\mathrm{so}(3)$ 요소를 찾을 수 있지만 동일한 $\mathrm{SO}(3)$에 해당하는 $\mathrm{so}(3)$의 요소가 여러 개 있을 수 있음을 의미합니다. 최소한 회전 각 $\theta$에 대해서, 우리는 $360^\circ$로 여러 번 회전한 것이 회전을 하지 않은 것과 동일하다는 것을 압니다. 이것은 주기성을 가지고 있습니다. 그러나 회전 각을 $\pm \pi$사이로 고정하면, Lie 군과 Lie 대수 요소는 일대일로 대응합니다.

  $\mathrm{SO}(3)$, $\mathrm{so}(3)$의 결론은 우리가 기대하는 바와 같은 것처럼 보입니다. 이것은 이전에 이야기한 회전 벡터와 매우 유사하며, Exponential map은 Rodrigue 공식입니다.

4.2.2 SE (3)의 Exponential Map

  $\mathrm{se}(3)$의 Exponential map은 아래에 설명되어 있습니다. 공간을 절약하기 위해 Exponential map을 $\mathrm{so}(3)$처럼 세밀하게 유도하지 않았습니다. $\mathrm{se}(3)$의 Exponential map의 형식은 다음과 같습니다.

  약간의 인내심을 가지고 있다면, $\mathrm{so}(3)$처럼 Taylor expansion을 이용하여 이 공식을 유도할 수 있습니다. 다음과 같이 $\boldsymbol{\phi}=\theta \mathbf{a}$라고하면 (a는 단위 벡터) 아래의 식을 유도할 수 있습니다.

  결과로부터, $\boldsymbol{\xi}$에 대한 Exponential map의 좌상단에 있는 R은 이미 우리가 잘 알고 있는 $\mathrm{SO}(3)$의 원소이며, 이는 $\mathfrak{so}(3)$에 속하는 $\boldsymbol{\xi}$의 회전 부분에 해당합니다. 오른쪽 상단의 $\mathbf{J}$는 자코비안 행렬입니다.

  이 수식은 로드리게스 수식과 다소 유사하지만 정확하게 동일하지는 않습니다. Exponential map을 거친 후에는 이동 부분이 선형 자코비안 매트릭스 J와 곱해진 것을 알 수 있습니다.

  유사하게, Logarithm map의 유도를 분석적으로 도출할 수는 있지만 변환 행렬 T에서 $\mathfrak{so}(3)$에 대응하는 벡터를 찾는 좀 더 쉬운 방법이 있습니다. 

  J는 $\boldsymbol{\phi}$로부터 얻을 수 있기 때문에, $\boldsymbol{\rho}$는 이 선형 방정식으로도 풀 수 있습니다. 이제 우리는 그림 4-1에 요약된 것처럼 Lie 군과 Lie 대수의 정의와 상호 변환 관계를 명확히 했습니다. 독자가 이해할 수 없는 경우 몇 페이지로 위로 돌아가서 그에 해당하는 수식 유도를 볼 수 있습니다.

그림 4-1 : SO(3), SE(3), so(3), se(3) 사이의 대응.

4.3 리 대수 미분과 섭동 모델

4.3.1 BCH 공식 및 근사법

  Lie 대수를 사용하는 주된 동기 중 하나는 최적화를 위한 것이며, 미분은 최적화 과정에서 매우 필요한 정보입니다 (이는 강의 6에서 자세히 다룰 것입니다). 이제 아래의 문제를 고려해 보겠습니다. $\mathrm{SO}(3)$와 $\mathrm{SE}(3)$에 대한 Lie 군과 Lie 대수  관계를 이미 이해했지만, $\mathrm{SO}(3)$에서 두 행렬 곱셈이 수행될 때 Lie 대수로 어떻게 바꿔줄까요? 반대로, $\mathrm{SE}(3)$에서 두 개의 Lie 대수를 추가할 때 $\mathrm{SO}(3)$은 두 행렬의 곱에 해당합니까?

만약 $\boldsymbol{\phi}_1, \boldsymbol{\phi}_2$가 스칼라라면, 이것은 분명히 사실이지만 여기에서는 스칼라가 아닌 행렬의 지수 함수를 계산합니다. 즉, 우리는 다음 공식이 성립하는지를 알고 싶습니다 :

불행히도 이 수식은 행렬에서 유효하지 않습니다. 이 곱셈의 완전한 형태는 Baker-Campbell-Hausdorff 공식 (BCH 공식)에 의해 주어집니다. 전체 공식의 복잡성으로 인해 처음 몇 개의 항만 표현하면 다음과 같습니다.

여기서 $[]$ 는 앞에서 설명한 대괄호 연산입니다. $\mathrm{SO}(3)$에 대한 대수 $\ln { \left( {\exp \left( { \boldsymbol{\phi} _1^ \wedge } \right)\exp \left ( {\boldsymbol{\phi} _2^ \wedge } \right)} \right) ^ \vee }$를 생각해 봅시다. $\boldsymbol{\phi_1}$ 또는 $\boldsymbol{\phi_2}$가 0에 가까운 아주 작은 값일 때 우리는 이 값을 무시할 수 있습니다. 이 시점에서 BCH는 선형 근삿값을 갖습니다.

첫 번째 근사식을 예로 들어 보겠습니다. 이 방정식은 회전 행렬 $\mathbf{R}_2$ (Lie 대수가 $\boldsymbol{\phi_2}$인 것에 해당하는 회전 행렬)에 아주 작은(infinitesimal) 회전 행렬 $\mathbf{R}_1$ (Lie 대수가 $\boldsymbol{\phi_1}$인 것에 해당하는 회전 행렬)을 곱하면 원래의 Lie 대수 $\boldsymbol{\phi_2}$로 근사 될 수 있음을 알 수 있습니다. 원래의 Lie 대수 $\boldsymbol{\phi_2}$에 $\mathbf{J}_l \left( {\boldsymbol{\phi} _2} \right)^{ - 1} { \boldsymbol{\phi} _1}$ 가 추가된 형태입니다. 마찬가지로, 두 번째 근사값(두번째 줄의 식)은 오른쪽에 작은 행렬이 곱해진 경우를 나타냅니다. 따라서 BCH 근사법에서 Lie 대수는 왼쪽 곱셈 근사와 오른쪽 곱셈 근사로 나누어집니다. 따라서 사용 시 왼쪽 곱셈 모델을 사용하는지 또는 오른쪽 곱셈 모델을 사용하는지 주의해야 합니다.이 책은 왼쪽 곱셈 모델을 예로 들어 설명합니다. 왼쪽 모델에서의 자코비안 행렬 $\mathbf{J}_l$는 실제로 수식 (4.27)의 내용입니다.

역행렬은 다음과 같습니다:

  만약 $\theta$가 0이 아니라면 (그러한 경우 우리는 $\mathbf{J}_{l}$과 이의 역행렬을 단위행렬로 취한다). 오른쪽 자코비안을 구하기 위해서는 마이너스 부호만 취하면 됩니다.

  이 방법으로, 우리는 Lie 군 곱셈과 Lie 대수 추가 사이의 관계에 대해 이야기할 수 있습니다.

  독자의 편의를 위해 BCH 근사법의 의미를 다시 설명합니다. 특정 회전 R에 대해 대응하는 Lie 대수는 $\boldsymbol{\phi}$라고 가정합니다. 우리는 왼쪽에 곱해지는 작은 회전을 $\Delta \mathbf{R}$로 표시하고 이에 대응하는 Lie 대수는 $\Delta \boldsymbol{\phi}$라고 가정합니다. 그런 다음 Lie 군에서 결과는 $ \Delta \mathbf{R} \cdot \mathbf{R}$이고 이것이 리 대수에서는 BCH 근사에 따라 $\mathbf{J}_l^{-1 } (\boldsymbol{\phi}) \Delta \boldsymbol{\phi} + \boldsymbol{\phi}$가 됩니다. 결합하면 다음과 같이 간단히 작성할 수 있습니다.

반대로, Lie 대수 $\boldsymbol{\phi}$에 $\Delta \boldsymbol{\phi}$를 더하면, Lie 군의 왼쪽과 오른쪽 Jacobian의 곱셈을 근사 할 수 있습니다.

  이것은 Lie 대수에 대한 후속 계산에 대한 이론적 근거를 제공합니다. 유사하게, $\mathrm{SE}(3)$에 대해서도 유사한 BCH 근사가 있습니다 :

여기서 $\boldsymbol{\mathcal{J}}_l$ 와 $\boldsymbol{\mathcal{J}}_r$의 형식은 더 복잡하며 6 x 6 행렬입니다. 독자는 [5]에서 더 자세한 내용을 참조할 수 있습니다. 우리는 이 두 개의 자코비안 행렬을 계산에 사용하지 않았기 때문에 여기에서 실제 양식을 생략했습니다.

4.3.2 SO(3) 대수의 유도

  이제 우리의 목표 함수가 회전이나 변환과 관련된 수식을 포함하는 경우 미분을 계산하는 방법에 대해 이야기해 봅시다. 이것은 일반적으로 SLAM 문제를 해결할 때 최적화하기 위해 일반적으로 이러한 함수들을 가지고 있기 때문에 매우 강력한 실용적인 의미를 가집니다. 우리가 $\mathrm{SO}(3)$ 또는 $\mathrm{SE}(3)$로 설명되는 카메라의 위치와 자세를 추정한다고 가정해봅시다. 우리의 로봇은 세계 좌표계에서의 점 p를 관찰하고 관찰값 z를 생성하여 다음과 같은 식을 쓸 수 있습니다.

여기서 w는 랜덤 노이즈입니다. 랜덤 노이즈로 인해, 실제로 관찰된 데이터는 관찰 모델로부터 계산된 값과 정확히 같지 않습니다. 따라서 우리는 일반적으로 이상적인 관찰과 실제 데이터 간의 오차를 계산합니다 :

 그러한 랜드 마크 포인트와 관측치가 N개 있다고 가정하면 위의 식이 N개가 있습니다. 그러면 로봇의 위치 추정은 최적의 T를 찾는 것과 동일하며, 이는 전체 오차를 최소화합니다 :

  이 문제를 해결하기 위해 변환 행렬 T에 대한 목적 함수 J의 미분을 계산해야 합니다.  다음 장에서 설명할 최소 자승 문제에 대한 설명을 남겨둡시다. 여기에서 요점은 포즈 종속 함수를 작성한 다음 함수의 미분 함수를 논의하여 현재 추정치를 조정한다는 것입니다. 우리는 더 나은 / 최선의 추정을 찾기 위해 회전이나 변환 행렬을 조정해야 합니다. 그러나 $\mathrm{SO}(3)$ 와 $\mathrm{SE}(3)$에는 잘 정의된 증분 연산이 없으며 단지 군입니다. 최적화를 처리하기 위해 R 또는 T를 정규 행렬로 취급하면 우리는 최적화 시에 제약조건들을 포함해야 합니다.

  그러나 Lie 대수의 관점에서, Lie 대수는 벡터로 구성되어 있기 때문에 증분 연산이 정의되어 있습니다. 따라서 Lie 대수 을 사용하여 해당 문제를 해결하는 두 가지 방법이 있습니다.

 

1. Lie 대수에서의 아주 작은 크기의 항을 추가하는 것을 가정하고 목적 함수의 변화를 계산
2. Lie 군에서 아주 작은 섭동량을 왼쪽 곱셈 또는 오른쪽 곱셈을 활용하여 곱하고 섭동을 설명하기 위해 Lie 대수를 사용. 그리고 이 섭동에 대한 미분을 계산. 이는 왼쪽 섭동 또는 오른쪽 섭동 모델로 이야기함

  첫 번째 방법은 Lie 대수의 미분 모델에 해당하고 두 번째 방법은 섭동 모델에 해당합니다. 두 아이디어의 유사점과 차이점에 대해 논의해 보겠습니다.

4.3.3 미분 모델

  첫째, $\mathrm{SO}(3)$의 상황을 고려해봅시다. 우리가 공간 점 $\mathbf{p}$를 회전시켜 $\mathbf{R} \mathbf{p}$를 얻는다고 가정해봅시다. 이제 회전 후의 점 좌표에 대해 회전에 대한 미분 값을 계산하려면 다음과 같이 쓸 수 있습니다.

 $\mathrm{SO}(3)$가 증분 연산을 가지지 않기 때문에, 이는 일반적인 미분 정의로는 계산될 수 없습니다. 회전 R에 관련한 Lie 대수를 $\boldsymbol{\phi}$라고 하면, 일반적인 미분 대신에 다음을 계산할 수 있습니다.

  미분의 정의에 따르면 다음과 같습니다.

  두 번째 행의 근사는 BCH의 선형 근사치이며 세 번째 행은 Taylor 확장의 근사값이며 네 번째 행에서 다섯 번째 행은 반 대칭 행렬로 만드는 기호(^)를 외적(outer product)으로 취급하여 교환 후에 부호가 변경됩니다.(A^B = -B^A) 따라서 우리는 Lie 대수와 관련된 회전된 점의 미분을 유도할 수 있습니다.

  그러나 여전히 더 복잡한 형태의 $\mathbf{J}_l$를 포함하기 때문에 이를 계산하고 싶지는 않습니다. 아래에 설명된 섭동 모델은 미분을 계산하는 더 간단한 방법을 제공합니다.

4.3.4 섭동 모델

  미분의 또 다른 유형은 $\mathbf{R}$에서 섭동 $\Delta \mathbf{R}$을 수행하는 것입니다. 이 섭동은 왼쪽에서 곱해질 수도 있고 오른쪽에서 곱해질 수도 있습니다. 최종 결과는 약간 다릅니다. 왼쪽 섭동 모델을 예로 들어 봅시다. 왼쪽 섭동 $\Delta \mathbf{R}$에 해당하는 Lie 대수를 $\boldsymbol{\varphi}$라고합시다. 그러면 $\boldsymbol{\varphi}$에 대해서 미분을 수행하면

이 수식의 미분은 위의 것보다 간단합니다.

  Lie 대수를 직접 찾는 것과 비교하여 Jacobian $\mathbf{J}_l$의 계산이 생략된 것을 볼 수 있습니다. 이것은 섭동 모델을 보다 실용적으로 만듭니다. 독자는 여기에서 미분 작업을 이해해야 하며 이는 포즈 추정에 큰 의미가 있습니다.

4.3.5 SE(3)에서의 미분 연산

  마지막으로, 우리는 $\mathrm{SE}(3)$에 섭동 모델을 제시하고 직접적인 Lie 대수에 대한 유도는 소개하지 않겠습니다. 공간 점 p가 변환 T (Lie 대수 $\boldsymbol{\xi}$에 해당)라고 가정하면, Tp가 얻어집니다. 이제 T에 왼쪽 섭동 $\Delta \mathbf{T} = \exp \left( \delta \boldsymbol{\xi}^\wedge \right)$을 곱함으로써, 섭동 항의 Lie 대수를 $\delta \boldsymbol{\xi} = [\delta \boldsymbol{\rho}, \delta \boldsymbol{\phi}]^\mathrm{T}$로 설정하면 다음을 얻을수 있습니다.

  우리는 최종 결과를 Homogeneous coordinate의 공간 점을 4 × 6 행렬로 변환하는 연산자 $^\odot$로 정의합니다. 이 수식은 행렬 유도의 순서를 약간 설명할 필요가 있습니다. a, b, x, y를 열 벡터라고 가정하면 기호 표기법에 따라 다음과 같은 규칙이 적용됩니다.

지금까지 우리는 Lie group, Lie algebra에 대해 미분 연산을 도입했습니다. 다음 장에서는 이 지식을 실제 문제를 해결하는 데 적용할 것입니다. Lie 군, Lie 대수의 몇 가지 중요한 수학적 특성에 관해서 우리는 그것을 연습문제로 독자에게 남겨 둡니다.

4.4 실습: Sophus

  우리는 앞에서 Lie 대수에 대한 수학적 지식을 소개했으며, 이제는 실제 연습을 통해 배운 것을 테스트할 때입니다. 프로그램에서 Lie 대수를 조작하는 방법에 대해 알아보겠습니다. 강의 3에서는 Eigen 라이브러리가 기하학 모듈을 제공하지만 Lie 대수를 지원하지 않는다는 것을 알았습니다. 더 나은 Lie 대수 라이브러리는 Strasdat가 관리하는 Sophus라이브러리입니다 (https://github.com/strasdat/Sophus). Sophus 라이브러리는 이 장에서 주로 다루는 $\mathrm{SO}(3)$과 $\mathrm{SE}(3)$ 를 지원하며 2차원 공간에서의 움직임 $\mathrm{SO}(2), \mathrm{SE} (2) $ 및 유사변환 $\mathrm{SO}(2), \mathrm{SE} (2) $의 내용도 포함합니다. 이는 Eigen 라이브러리를 기반으로 하여 개발되었으며 그 이외의 추가 종속성을 설치할 필요가 없습니다. 독자는 GitHub 또는 이 책의 예제 코드에서 Sophus를 직접 얻을 수 있습니다. Sophus 소스 코드는 책의 코드 디렉토리 "slambook2/3rdparty"에서도 볼 수 있습니다. 역사적인 이유로 Sophus의 초기 버전은 double precision(배정밀도)의 Lie 군 / Lie 대수 클래스만을 제공했습니다. 이후 버전은 템플릿 클래스로 다시 작성되었습니다. 다른 정밀도의 Lie 군 / Lie 대수는 템플릿 클래스의 Sophus에서 사용할 수 있지만 동시에 사용의 어려움이 커집니다. 

  이 책의 두 번째 버전에서는 템플릿과 함께 Sophus 라이브러리를 사용합니다. 이 책의 코드 예제의 3rdparty에서 제공하는 Sophus는 템플릿 버전이며이 책의 코드를 다운로드할 때 컴퓨터에 복사해야 합니다. Sophus 자체도 CMake 프로젝트입니다. 아마도 여러분은 이미 CMake 프로젝트를 컴파일하는 방법을 알고 있을 것이므로 여기서는 자세히 설명하지 않겠습니다. Sophus 라이브러리는 컴파일 만하면 되며 설치할 필요가 없습니다.

  Sophus 라이브러리에서 SO (3) 및 SE (3) 작업을 수행해봅시다.

 

slambook/ch4/useSophus/useSophus.cpp

 

  데모는 두 부분으로 나뉩니다. 전반부는 $\mathrm{SO}(3)$에 대한 연산을 소개하고 후반부는 $\mathrm{SE}(3)$ 입니다. 우리는 $\mathrm{SO}(3)$, $\mathrm{SE}(3)$ 객체를 생성하고, Exponential map과 Logarithm map을 수행하고, Lie 군 요소를 업데이트하는 방법을 보여줍니다. 독자가 이 강좌의 내용을 잘 이해한다면 이 프로그램은 당신에게 어려움이 없어야 합니다. 그것을 컴파일하기 위해 CMakeLists.txt에 다음 줄을 추가하십시오 :

  find_package 명령은 헤더와 라이브러리 파일을 찾기 위해 CMake에 의해 제공된 명령어입니다. CMake가 찾을 수 있으면 헤더 및 라이브러리 파일이 있는 디렉터리에 대한 변수를 제공합니다. Sophus의 예에서는 Sophus_INCLUDE_DIRS입니다. 템플릿 기반 Sophus 라이브러리에는 Eigen과 마찬가지로 헤더 파일과 소스 파일만 있습니다. 이를 기반으로 Sophus 라이브러리를 자체 CMake 프로젝트에 도입할 수 있습니다. 

4.4.2 예 : 궤적의 오차 추정

  실용 공학에서는 종종 알고리즘의 추정 궤적과 알고리즘의 정확도를 평가하기 위한 실제 궤적 간의 차이를 평가해야 합니다. 실제 궤적은 종종 일부 고정밀 시스템에 의해 얻어지며 추정된 추정치는 평가할 알고리즘에 의해 계산됩니다. 이전 장에서는 파일에 저장된 트랙을 표시하는 방법을 보여주었습니다. 이 섹션에서는 두 궤적의 오류를 계산하는 방법을 고려할 것입니다. 추정된 궤도 $\mathbf{T}_{\mathrm{esti}, i}$와 실제 궤도 $\mathbf{T}_{\mathrm{gt},i}$를 생각해 봅시다 ($i=1,\cdots, N$). 그다음에 우리는 그들 사이의 차이를 설명하기 위한 몇 가지 평가 방법을 정의할 수 있습니다.

  평가 방법에는 여러 가지가 있으며, 일반적인 ATE (Absolute Trajectory Error)는 다음과 같습니다.

  이것은 실제로 각 자세에 대한 lie대수의 RMSE (Root-Mean-Squared Error)입니다. 이 오류는 두 궤적의 회전 및 변환 오류를 특성화할 수 있습니다. 동시에, 일부 문헌에서는 이동 오차만을 고려하므로 평균 이동 오차 (Average Translation Error)를 정의할 수 있습니다.

  여기서 "trans"는 괄호의 내부 변수의 이동 부분을 의미합니다. 전체 궤적 관점에서 볼 때 회전에 오류가 있는 경우 후속 궤적도 변환에 오류가 있으므로 두 지표를 실제로 적용할 수 있습니다.

  이 외에도 상대적 오류를 정의할 수 있습니다. 예를 들어 i에서 i + Δt까지의 움직임을 고려하면 RPE (Relative Pose Error)는 다음과 같이 정의 할 수 있습니다.

마찬가지로 이동 부분만 사용할 수 있습니다.

  이 부분은 Sophus 라이브러리로 쉽게 구현할 수 있습니다. 아래에서 우리는 절대 궤적 오차의 계산을 설명합니다. 이 예제에서는 groundtruth.txt와 estimated.txt의 두 궤적을 가지고 있는데, 다음 코드는 두 궤적을 읽고 오류를 계산 한 다음 3D 창에 표시합니다.

 

slambook/ch4/example/trajectoryError.cpp

 

  이 프로그램 출력의 결과 값은 2.207이며 이미지는 그림 4-2에 나와 있습니다. 회전 부분을 제거하고 변환 부분의 오류만 계산할 수도 있습니다. 실제로 이 예에서 우리는 독자가 궤적의 시간 정렬 및 외부 매개 변수 추정을 포함하여 몇 가지 전처리 작업을 수행하도록 도왔습니다. 이러한 내용은 아직 언급되지 않았으며 향후 강의에서 논의할 예정입니다.

그림 4-2 : 추정 된 궤적과 실제 궤적 사이의 오류를 계산합니다.

 

4.5 유사 변환 군과 리 대수

  마지막으로 우리는 단안 시각에 사용된 유사 변환 군 $\mathrm{Sim}(3)$과 상응하는 Lie 대수 $\mathfrak{sim}(3)$을 언급하고자 합니다. 양안 SLAM 또는 RGB-D SLAM에만 관심이 있는 경우 이 절을 건너뛸 수 있습니다.

  우리는 앞에서 크기 변환의 불확실성을 이야기했습니다. 단안 SLAM에서 포즈를 나타 내기 위해 $\mathrm{SE}(3)$를 사용할 때 크기 변환에 대한 불확실성 및 크기 변환 드리프트로 인해 스케일이 지속적으로 변화할 것이며 이는 $\mathrm{SE}(3)$로는 처리되지 않습니다. 따라서 단안의 경우 일반적으로 스케일 팩터를 명시적으로 처리해야 합니다. 수학적 용어로, 공간에 위치한 점 p에 대해서 유클리드 변환 대신 카메라 좌표계에서 유사 변환이 전달됩니다.

  유사 변환에서 우리는 스케일을 s로 표현합니다. $\mathrm{SO}(3)$, $\mathrm{SE}(3)$와 유사하게 유사 변환은 유사 변환 군 $\mathrm{Sim}(3)$이라고하는 행렬 곱셈을 위한 군을 형성합니다.

  유사하게, $\mathrm{Sim}(3)$은 그에 상응하는 대수 (Lie algebra), Exponential map, Logarithm map 등을 가지고 있습니다. Lie 대수 $\mathfrak{sim}(3)$ 요소는 7차원 벡터 $\boldsymbol{\zeta}$입니다. 첫 번째 6 개의 차원은 $\mathfrak{se}(3)$과 동일하며 새로운 인자 $\sigma$가 추가됩니다.

  $\mathfrak{se}(3)$ 와 비교하여 추가적인 항 $\sigma$을 가집니다. $\mathrm{Sim}(3)$과 $\mathfrak{sim}(3)$에 대해서도 Exponential map과 Logarithm map이 존재합니다. Exponential map은 다음과 같습니다.

  Exponential map을 통해 Lie 대수와 Lie 군 사이의 관계를 찾을 수 있습니다. Lie 대수의 경우, Lie군과의 대응은 다음과 같습니다.

  회전 부분은 $\mathrm{SO}(3)$와 동일합니다. 이동 부분에서는 $\mathfrak{se}(3)$에 속하는 자코비안 $\boldsymbol{\mathcal{J}}$를 곱해야하며 유사 변환된 Jacobi는 더 복잡합니다. 스케일 인자에 대해서, 우리는 Lie 군의 s가 Lie 대수에서 σ의 지수 함수임을 알 수 있습니다.

  $\mathrm{Sim}(3)$의 BCH 근사는 $\mathrm{SE}(3)$와 유사합니다. 우리는 $\mathbf{S}$에 대해 $\mathbf{S} \mathbf{p}$의 유사 변환 후에 $\mathbf{S}$의 미분에 대해 논의할수 있습니다. 유사하게, 미분 모델과 섭동 모델의 두 가지 방법이 있으며, 섭동 모델이 좀 더 간단합니다. 유도 과정을 생략하고 섭동 모델의 결과를 직접 제공합니다. $\mathbf{S} \mathbf{p}$의 왼쪽에 작은 섭동 $\exp( \boldsymbol{\zeta} ^\wedge )$를주고, 섭동에 대해 $\mathbf{S} \mathbf{p}$의 미분을 찾습니다. $\mathbf{S} \mathbf{p}$는 4 차원 동차 좌표이고 $\boldsymbol{\zeta}$는 7 차원 벡터이기 때문에 미분은 4 × 7 자코비안이어야합니다. 편의상 $\mathbf{S} \mathbf{p}$의 첫 번째 3차원이 벡터 $\mathbf{q}$ 를 형성한다는 것을 기억하십시오.

  $\mathrm{Sim}(3)$에 관해서는 여기까지 소개 할 것입니다. $\mathrm{Sim}(3)$에 대한 자세한 내용은 독자가 문헌 [24]을 참조하는 것이 좋습니다.

4.6 요약

  이 강의에서는 Lie 군 $\mathrm{SO}(3)$ 와 $\mathrm{SE}(3)$ 및 해당 Lie 대수 $\mathfrak{so}(3)$와 $\mathfrak{se }(3)$를 소개했습니다. 포즈의 표현과 변형을 소개하고, BCH의 선형 근사화를 통해 포즈의 값을 예측할 수 있습니다. 우리는 포즈의 추정치를 조정하여 대응하는 오차를 줄여야 하기 때문에 포즈의 최적화를 위한 이론적 토대를 소개했습니다. 포즈를 조정하고 업데이트하는 방법을 알아낸 후에야 다음 단계로 넘어갈 수 있습니다.

  이 강의의 내용은 매우 이론적 일 수 있습니다. Lie 군과 Lie 대수를 설명하는 수학 교과서와 비교할 때 실용적인 내용만 다루기 때문에 과정이 매우 간소화되었습니다. 독자는 이 장 이후의 많은 후속 문제, 특히 포즈 추정 부분을 해결하기 위한 기초로써 해당 장의 내용을 이해해야 합니다.

  Lie 대수 이외에 회전은 쿼터니언, 오일러 각 등으로 표현될 수 있지만 후속 처리는 번거로운 점을 언급해야 합니다. 실용적인 응용 프로그램에서는 일부 Jacobian 계산을 피하기 위해 SE(3) 대신 SO(3)와 함께 translation을 사용할 수 있습니다.