[KR] Pose Graph-based SLAM

1. Pose Graph-based SLAM
pdf 보기 탭에서 한 페이지 씩 보기로 설정하시면 ppt 슬라이드 넘기듯이 보실 수 있습니다
개인적으로 공부하기 위해 작성한 자료입니다
내용 중 틀린 부분이나 빠진 내용이 있다면 gyurse@gmail.com 으로 말씀해주시면 감사하겠습니다 :-)

2. References: 1) SLAMKR study - SLAMKR Study Season 1
2) Robot Mapping Course – Freiburg
3) ETH CVG 3D Vision – lecture link
4) FastCampus SLAM Workshop (Dong-Won Shin)
5) Kwonheejeong blog - Kwonheejeong Blog
2019/10/19
1st: 2019/07/20
2nd: 2019/08/03
3rd: 2019/08/09
4th: 2019/08/16
Last modified:

3. Contents
1. Pose Graph
2. Pose Graph-based SLAM
3. Least Squares
4. Bundle Adjustment (BA)
5. Pose Graph Optimization (PGO)

4. 1. Pose Graph

5. N1
N2
N3
N4
N5
N6
E12
E13
E35
E34
E25
E24
E45
E46
E56
1. Pose Graph
• 단순히 노드(N, node)와 그 노드를 연결하는 간선(E, edge)을
하나로 모아 놓은 자료 구조
• 연결되어 있는 객체 간의 관계를 표현할 수 있는 자료구조이다.
Graph
https://www.researchgate.net/figure/A-schematic-depiction-of-a-pose-graph-The-vertices-in-the-graph-correspond-to-poses_fig2_302146617
https://gmlwjd9405.github.io/2018/08/13/data-structure-graph.html

6. N1
N2
N3
N4
N5
N6
E12
E13
E35
E34
E25
E24
E45
E46
E56
1. Pose Graph
• 단순히 노드(N, node)와 그 노드를 연결하는 간선(E, edge)을
하나로 모아 놓은 자료 구조
• 연결되어 있는 객체 간의 관계를 표현할 수 있는 자료구조이다.
Graph Pose Graph
• Graph 자료구조에서 노드를 로봇의 위치(Pose)로,
간선을 노드 간 상대 위치(Relative Pose)로 표현한 자료구조
https://www.researchgate.net/figure/A-schematic-depiction-of-a-pose-graph-The-vertices-in-the-graph-correspond-to-poses_fig2_302146617
https://gmlwjd9405.github.io/2018/08/13/data-structure-graph.html

7. https://www.cc.gatech.edu/~dellaert/pub/Carlone16tro.pdf
Pose Graph
• 각각의 노드는 Pose를 의미하며 간선은 두 노드 간 Relative Pose를 의미한다.
• 2D, 3D 차원에 따라 Pose의 차원이 달라진다.
2D case
Node
Edge
1. Pose Graph

8. Pose Graph
• 각각의 노드는 Pose를 의미하며 간선은 두 노드 간 Relative Pose를 의미한다.
• 2D, 3D 차원에 따라 Pose의 차원이 달라진다. 3D case
Edge
Node
2D case
Node
Edge
1. Pose Graph
https://www.cc.gatech.edu/~dellaert/pub/Carlone16tro.pdf

9. Pose Graph
• 각각의 노드는 Pose를 의미하며 간선은 두 노드 간 Relative Pose를 의미한다.
• 2D, 3D 차원에 따라 Pose의 차원이 달라진다.
• 해당 자료에서는 3D case만을 다룬다.
2D case
Node
Edge
3D case
1. Pose Graph
https://www.cc.gatech.edu/~dellaert/pub/Carlone16tro.pdf
Edge
Node

10. 2. Pose Graph-based SLAM

11. 2. Pose Graph-based SLAM
Graph-based SLAM
• Graph SLAM은1997년 Lu and Milios에 의해 발표된 알고리즘이다.
• 로봇이 주행을 Pose Graph를 사용하여 표현하고 이를 통해 간선에 누적되는 에러를 최적화를 통해 최소화하는 방식이다.
• 그래프를 생성하는 Front-End와 그래프를 최적화하는 Back-End로 구분된다.
: Robot Pose
: Constraint (error)
: Information Matrix
: Error Function

12. Process pipeline
2. Pose Graph-based SLAM
• Pose Graph SLAM의 Pipeline은 다음과 같이 Front-End와 Back-End로 구성되어 있다.

13. Process pipeline
2. Pose Graph-based SLAM
• Pose Graph SLAM의 Pipeline은 다음과 같이 Front-End와 Back-End로 구성되어 있다.
• Front-End: 사용하는 센서의 종류에 따라 다양한 알고리즘을 사용하며 Pose Graph를 생성한다.
• 입력: Sensor Data
• 출력: Pose Graph

14. 2. Pose Graph-based SLAM
• Pose Graph SLAM의 Pipeline은 다음과 같이 Front-End와 Back-End로 구성되어 있다.
• Front-End: 사용하는 센서의 종류에 따라 다양한 알고리즘을 사용하며 Pose Graph를 생성한다.
• 입력: Sensor Data
• 출력: Pose Graph
• Back-End: Pose Graph의 누적된 오차를 비선형 최적화 방법을 통해 최적화한다.
• 입력: Pose Graph
• 출력: Optimized Pose Graph
Process pipeline

15. 2. Pose Graph-based SLAM
• Pose Graph SLAM의 Pipeline은 다음과 같이 Front-End와 Back-End로 구성되어 있다.
• Front-End: 사용하는 센서의 종류에 따라 다양한 알고리즘을 사용하며 Pose Graph를 생성한다.
• 입력: Sensor Data
• 출력: Pose Graph
• Back-End: Pose Graph의 누적된 오차를 비선형 최적화 방법을 통해 최적화한다.
• 입력: Pose Graph
• 출력: Optimized Pose Graph
• Front-End의 경우 다양한 센서에 따라 Graph를 생성하는 방법이 달라진다. e.g., Visual SLAM  Visual Odometry
Process pipeline

16. 2. Pose Graph-based SLAM
• Pose Graph SLAM의 Pipeline은 다음과 같이 Front-End와 Back-End로 구성되어 있다.
• Front-End: 사용하는 센서의 종류에 따라 다양한 알고리즘을 사용하며 Pose Graph를 생성한다.
• 입력: Sensor Data
• 출력: Pose Graph
• Back-End: Pose Graph의 누적된 오차를 비선형 최적화 방법을 통해 최적화한다.
• 입력: Pose Graph
• 출력: Optimized Pose Graph
• Front-End의 경우 다양한 센서에 따라 Graph를 생성하는 방법이 달라진다. e.g., Visual SLAM  Visual Odometry
• Back-End의 경우 다양한 비선형 최적화 방법(Gauss-Newton, Gradient Descent, Levenberg-Marquardt,…) 이 적용될 수 있다.
Process pipeline

17. 2. Pose Graph-based SLAM
• Edge는 다음과 같은 2가지 상황에서 생성된다.
1. 순차적(temporal)으로 생성되는 노드 사이에 Edge가 생성된다. (by Front-End)
1. temporal case
Edge Construction

18. 2. Pose Graph-based SLAM
• Edge는 다음과 같은 2가지 상황에서 생성된다.
1. 순차적(temporal)으로 생성되는 노드 사이에 Edge가 생성된다. (by Front-End)
2. 시간 순서에 관계없이(non-temporal) 근접한 두 노드 사이에도 Edge가 생성된다. (by Back-End)
1. temporal case 2. non-temporal case
Edge Construction

19. 3. Least Squares

20. Error Function Derivation
• 최적화를 수행하기 위해서는 Error Function을 수학적으로 정의해야 한다.
3. Least Squares

21. Error Function Derivation
• 최적화를 수행하기 위해서는 Error Function을 수학적으로 정의해야 한다.
• 근접한 위치에 있는 두 노드 가 비슷한 센서 데이터를 취득하면 이를 바탕으로 두 노드 사이의 상대 위치를 구할 수 있다.
• 이를 실제로 측정한 정확한 Edge 정보가 아니므로 Virtual Measurement라고 한다.
3. Least Squares

22. Error Function Derivation
• 최적화를 수행하기 위해서는 Error Function을 수학적으로 정의해야 한다.
• 근접한 위치에 있는 두 노드 가 비슷한 센서 데이터를 취득하면 이를 바탕으로 두 노드 사이의 상대 위치를 구할 수 있다.
• 이를 실제로 측정한 정확한 Edge 정보가 아니므로 Virtual Measurement라고 한다.
• Virtual Measurement를 통해 Pose Graph Optimization에 사용되는 Error Function을 유도할 수 있다.
3. Least Squares

23. Error Function Derivation
• 최적화를 수행하기 위해서는 Error Function을 수학적으로 정의해야 한다.
• 근접한 위치에 있는 두 노드 가 비슷한 센서 데이터를 취득하면 이를 바탕으로 두 노드 사이의 상대 위치를 구할 수 있다.
• 이를 실제로 측정한 정확한 Edge 정보가 아니므로 Virtual Measurement라고 한다.
• Virtual Measurement를 통해 Pose Graph Optimization에 사용되는 Error Function을 유도할 수 있다.
• Error Function은 예상된 Virtual Measurement와 실제 측정한 Virtual Measurement 간의 차이로 표현된다.
3. Least Squares

24. • 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
3. Least Squares
Error Function Derivation
Error 에러
관측값
virtual measurement
예측값
prediction of
virtual measurement

25. 3. Least Squares
Error Function Derivation
Error 에러
관측값
virtual measurement
예측값
prediction of
virtual measurement
• 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.

26. 3. Least Squares
Error Function Derivation
• 위는 Euler Angle로 표현된 에러를 의미한다. 변환행렬로 표현하는 에러는 다음과 같다.
Euler angle
• 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.

27. 3. Least Squares
Error Function Derivation
• 위는 Euler Angle로 표현된 에러를 의미한다. 변환행렬로 표현하는 에러는 다음과 같다.
Euler angle
• 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.

28. 3. Least Squares
Error Function Derivation
• 에러가 ‘0’인 경우 역행렬과 행렬의 곱은 항등행렬 가 되어야 하지만 실제 상황에서 항상 에러는 존재하므로 항등행렬이 되지 않는다.
• 위는 Euler Angle로 표현된 에러를 의미한다. 변환행렬로 표현하는 에러는 다음과 같다.
Euler angle
• 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.

29. 3. Least Squares
Error Function Derivation
• 에러가 ‘0’인 경우 역행렬과 행렬의 곱은 항등행렬 가 되어야 하지만 실제 상황에서 항상 에러는 존재하므로 항등행렬이 되지 않는다.
• 위는 Euler Angle로 표현된 에러를 의미한다. 변환행렬로 표현하는 에러는 다음과 같다.
Euler angle
• 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.

30. 3. Least Squares
Error Function Derivation
• 에러가 ‘0’인 경우 역행렬과 행렬의 곱은 항등행렬 가 되어야 하지만 실제 상황에서 항상 에러는 존재하므로 항등행렬이 되지 않는다.
• 이를 변환행렬에서 에러로 정의하고 실제 최적화 수행 시 자주 사용된다.
• 위는 Euler Angle로 표현된 에러를 의미한다. 변환행렬로 표현하는 에러는 다음과 같다.
Transformation Matrix
• 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.
Euler angle

31. • 다음과 같이 두 노드가 근접한 위치에 있어서 Virtual Measurement를 구했다고 가정해보자.
• 이 때 관측값과 예측값의 차이를 에러 로 정의한다.
3. Least Squares
Error Function Derivation
• 에러가 ‘0’인 경우 역행렬과 행렬의 곱은 항등행렬 가 되어야 하지만 실제 상황에서 항상 에러는 존재하므로 항등행렬이 되지 않는다.
• 이를 변환행렬에서 에러로 정의하고 실제 최적화 수행 시 자주 사용된다.
• 위는 Euler Angle로 표현된 에러를 의미한다. 변환행렬로 표현하는 에러는 다음과 같다.
Transformation Matrix
Euler angle

32. 3. Least Squares
Toy Example of Error Function
Visual Odometry를 통해 계산된
i,j 노드의 keypoints

33. 3. Least Squares
Toy Example of Error Function
Loop Closing or Bundle Adjustment Occurred

34. • 지금까지 에러함수를 유도했다.
• 다음으로 에러가 정규분포를 따른다고 가정하고 최적화 수식으로 변환하는 작업을 수행한다.
3. Least Squares
Error Function for Optimization

35. • 지금까지 에러함수를 유도했다.
• 다음으로 에러가 정규분포를 따른다고 가정하고 최적화 수식으로 변환하는 작업을 수행한다.
• 평균이 이고 분산이 인 다변수 정규분포를 통해 모델링할 수 있고 해당 식에 Log-Likelihood 는 다음과 같다.
3. Least Squares
단일변수 정규분포
다변수 정규분포Error Function for Optimization

36. • 지금까지 에러함수를 유도했다.
• 다음으로 에러가 정규분포를 따른다고 가정하고 최적화 수식으로 변환하는 작업을 수행한다.
• 평균이 이고 분산이 인 다변수 정규분포를 통해 모델링할 수 있고 해당 식에 Log-Likelihood 는 다음과 같다.
3. Least Squares
Error Function for Optimization
단일변수 정규분포
다변수 정규분포

37. • 지금까지 에러함수를 유도했다.
• 다음으로 에러가 정규분포를 따른다고 가정하고 최적화 수식으로 변환하는 작업을 수행한다.
• 평균이 이고 분산이 인 다변수 정규분포를 통해 모델링할 수 있고 해당 식에 Log-Likelihood 는 다음과 같다.
3. Least Squares
• Log-Likelihood가 최대가 되는 를 찾으면 다변수 정규분포의 확률이 최대가 된다. (Maximum Likelihood Estimation)(MLE)
(=negative log-likelihood가 최소가 되는)
Error Function for Optimization

38. • 지금까지 에러함수를 유도했다.
• 다음으로 에러가 정규분포를 따른다고 가정하고 최적화 수식으로 변환하는 작업을 수행한다.
• 평균이 이고 분산이 인 다변수 정규분포를 통해 모델링할 수 있고 해당 식에 Log-Likelihood 는 다음과 같다.
3. Least Squares
• Log-Likelihood가 최대가 되는 를 찾으면 다변수 정규분포의 확률이 최대가 된다. (Maximum Likelihood Estimation)(MLE)
(=negative log-likelihood가 최소가 되는)
• 이를 모든 노드에 대하여 표현하면 다음과 같다.
Error Function for Optimization

39. • 최종적으로 우리가 풀어야하는 최적화 문제는 다음과 같다.
3. Least Squares
non-linear Least Square

40. • 최종적으로 우리가 풀어야하는 최적화 문제는 다음과 같다.
3. Least Squares
non-linear Least Square
• 위 공식에서 에러를 줄이기 위해서는 에러의 파라미터 값을 변경하면서 최소값이 되는 최적의 파라미터를 찾아야 한다.
• 하지만 위 공식은 비선형이므로 closed-form solution이 존재하지 않는다.

41. • 최종적으로 우리가 풀어야하는 최적화 문제는 다음과 같다.
3. Least Squares
non-linear Least Square
• 위 공식에서 에러를 줄이기 위해서는 에러의 파라미터 값을 변경하면서 최소값이 되는 최적의 파라미터를 찾아야 한다.
• 하지만 위 공식은 비선형이므로 closed-form solution이 존재하지 않는다.
• 따라서 비선형 최적화 방법(Gauss-Newton, Gradient Descent, Levenberg-Marquardt)을 사용해서 문제를 풀 수 있다.
• 해당 자료에서는 Gauss-Newton 방법을 통해 푸는 방법을 소개한다.

42. • 최종적으로 우리가 풀어야하는 최적화 문제는 다음과 같다.
3. Least Squares
non-linear Least Square
• 위 공식에서 에러를 줄이기 위해서는 에러의 파라미터 값을 변경하면서 최소값이 되는 최적의 파라미터를 찾아야 한다.
• 하지만 위 공식은 비선형이므로 closed-form solution이 존재하지 않는다.
• 따라서 비선형 최적화 방법(Gauss-Newton, Gradient Descent, Levenberg-Marquardt)을 사용해서 문제를 풀 수 있다.
• 해당 자료에서는 Gauss-Newton 방법을 통해 푸는 방법을 소개한다.
• Gauss-Newton 방법은 다음과 같다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다
• 1번 Error Function은 위에 정의하였으므로 2번부터 소개한다.

43. 3. Least Squares
Gauss-Newton Method 1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
벡터화

44. 3. Least Squares
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

45. 3. Least Squares
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

46. 3. Least Squares
Error function에 위 식을 대입해보면 아래와 같다.
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

47. 3. Least Squares
Error function에 위 식을 대입해보면 아래와 같다.
해당 식을 전개하면 아래와 같다.
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

48. 3. Least Squares
Error function에 위 식을 대입해보면 아래와 같다.
해당 식을 전개하면 아래와 같다.
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

49. 3. Least Squares
Error function에 위 식을 대입해보면 아래와 같다.
해당 식을 전개하면 아래와 같다.
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

50. 3. Least Squares
Error function에 위 식을 대입해보면 아래와 같다.
해당 식을 전개하면 아래와 같다.
모든 노드들에 대한 공식은 다음과 같다.
Gauss-Newton Method
에러 파라미터의 변화를 통해 에러 함수를 최소화할 수 있다.
1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

51. 3. Least Squares
모든 노드들에 대한 공식은 다음과 같다.
Gauss-Newton Method 1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

52. 3. Least Squares
모든 노드들에 대한 공식은 다음과 같다.
위 식은 Quadratic form이므로 미분 후 0으로 설정하여 최소값을 찾을 수 있다.
Gauss-Newton Method 1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

53. 3. Least Squares
모든 노드들에 대한 공식은 다음과 같다.
위 식은 Quadratic form이므로 미분 후 0으로 설정하여 최소값을 찾을 수 있다.
이를 통해 얻은 최적의 x 변화량 값을 기존 x에 대입한다. 위의 1~4까지 과정을 값이 수렴할 때까지 반복한다.
Gauss-Newton Method 1. Error function을 정의한다
2. 테일러 전개로 근사 선형화한다
3. 1차 미분 후 0으로 설정한다
4. 이 때 값을 구하고 이를 Error function에 대입한다
5. 값이 수렴할 때 까지 반복한다

54. 3. Least Squares
지금까지 수행한 Gauss-Newton 방법을 정리하면 다음과 같다.
Gauss-Newton Method

55. 4. Bundle Adjustment
mostly refer to
SLAMKR Online Study BA 발표 – 최종호: https://youtu.be/EavQ_PodpWQ

56. 4. Bundle Adjustment
• 본 자료는 Feature-based Visual Odometry를 기반으로 설명한다 (feature extractor & descriptor)
Introduction

57. 4. Bundle Adjustment
• 본 자료는 Feature-based Visual Odometry를 기반으로 설명한다 (feature extractor & descriptor)
• 두 개의 카메라 영상 + 특징점들의 매칭 쌍이 주어졌을 때 매칭 쌍들로부터 상대적인 카메라의 pose를 구할 수 있었다
• Fundamental Matrix 또는 Essential Matrix를 사용하여 두 카메라의 상대적인 pose를 계산할 수 있었다
Introduction

58. 4. Bundle Adjustment
• 본 자료는 Feature-based Visual Odometry를 기반으로 설명한다 (feature extractor & descriptor)
• 두 개의 카메라 영상 + 특징점들의 매칭 쌍이 주어졌을 때 매칭 쌍들로부터 상대적인 카메라의 pose를 구할 수 있었다
• Fundamental Matrix 또는 Essential Matrix를 사용하여 두 카메라의 상대적인 pose를 계산할 수 있었다
• 이렇게 구한 두 개의 카메라 pose를 바탕으로 각각의 매칭 쌍들에 대한 3차원 점을 계산할 수 있었다 (Triangulation)
Introduction

59. 4. Bundle Adjustment
• 본 자료는 Feature-based Visual Odometry를 기반으로 설명한다 (feature extractor & descriptor)
• 두 개의 카메라 영상 + 특징점들의 매칭 쌍이 주어졌을 때 매칭 쌍들로부터 상대적인 카메라의 pose를 구할 수 있었다
• Fundamental Matrix 또는 Essential Matrix를 사용하여 두 카메라의 상대적인 pose를 계산할 수 있었다
• 이렇게 구한 두 개의 카메라 pose를 바탕으로 각각의 매칭 쌍들에 대한 3차원 점을 계산할 수 있었다 (Triangulation)
• Bundle Adjustment는 2개의 카메라 뿐만 아니라 연속적인 카메라 영상 + 매칭 쌍이 주어졌을 때
카메라 pose 전체와 3차원 점 전체의 최적의 위치를 찾는 방법을 말한다
bundle of light rays
Introduction

60. 4. Bundle Adjustment
• 본 자료는 Feature-based Visual Odometry를 기반으로 설명한다 (feature extractor & descriptor)
• 두 개의 카메라 영상 + 특징점들의 매칭 쌍이 주어졌을 때 매칭 쌍들로부터 상대적인 카메라의 pose를 구할 수 있었다
• Fundamental Matrix 또는 Essential Matrix를 사용하여 두 카메라의 상대적인 pose를 계산할 수 있었다
• 이렇게 구한 두 개의 카메라 pose를 바탕으로 각각의 매칭 쌍들에 대한 3차원 점을 계산할 수 있었다 (Triangulation)
• Bundle Adjustment는 2개의 카메라 뿐만 아니라 연속적인 카메라 영상 + 매칭 쌍이 주어졌을 때
카메라 pose 전체와 3차원 점 전체의 최적의 위치를 찾는 방법을 말한다
bundle of light rays
Introduction
: 최적화 대상

61. 4. Bundle Adjustment
• i 번째 카메라 pose 와 j 번째 3차원 공간 상의 한 점 가 있을 때 는 다음과 같은 변환을 통해 이미지 평면 상에 투영된다
Error function derivation

62. 4. Bundle Adjustment
• i 번째 카메라 pose 와 j 번째 3차원 공간 상의 한 점 가 있을 때 는 다음과 같은 변환을 통해 이미지 평면 상에 투영된다
Error function derivation

63. 4. Bundle Adjustment
3차원 점
2차원 이미지 픽셀
Projection model
Extrinsic Intrinsic
• i 번째 카메라 pose 와 j 번째 3차원 공간 상의 한 점 가 있을 때 는 다음과 같은 변환을 통해 이미지 평면 상에 투영된다
Error function derivation

64. 4. Bundle Adjustment
3차원 점
2차원 이미지 픽셀
• i 번째 카메라 pose 와 j 번째 3차원 공간 상의 한 점 가 있을 때 는 다음과 같은 변환을 통해 이미지 평면 상에 투영된다
• 위와 같이 카메라 Intrinsic/Extrinsic 파라미터를 활용한 모델을 Projection Model이라고 한다.
• 에러는 다음과 같이 정의되고 이를 reprojection error라고 한다
Error function derivation
Projection model

65. 4. Bundle Adjustment
3차원 점
2차원 이미지 픽셀
• i 번째 카메라 pose 와 j 번째 3차원 공간 상의 한 점 가 있을 때 는 다음과 같은 변환을 통해 이미지 평면 상에 투영된다
• 위와 같이 카메라 Intrinsic/Extrinsic 파라미터를 활용한 모델을 Projection Model이라고 한다.
• 에러는 다음과 같이 정의되고 이를 reprojection error라고 한다
Triangulation을 통해 측정한 measurement 값
Projection model을 통한 estimation 값
Error function derivation
Projection model

66. 4. Bundle Adjustment
3차원 점
2차원 이미지 픽셀
• 위와 같이 카메라 Intrinsic/Extrinsic 파라미터를 활용한 모델을 Projection Model이라고 한다.
• 에러는 다음과 같이 정의되고 이를 reprojection error라고 한다
• 모든 카메라 pose, 모든 3차원 점들에 대한 cost function은 다음과 같이 정의된다.
• i 번째 카메라 pose 와 j 번째 3차원 공간 상의 한 점 가 있을 때 는 다음과 같은 변환을 통해 이미지 평면 상에 투영된다
Error function derivation
Projection model

67. 4. Bundle Adjustment
• Cost function에서 최적화할 대상 는 다음 그림과 같다.
Least Squares for BA

68. 4. Bundle Adjustment
• Cost function에서 최적화할 대상 는 다음 그림과 같다.
Least Squares for BA

69. 4. Bundle Adjustment
• Cost function에서 최적화할 대상 는 다음 그림과 같다.
• 카메라 pose와 3차원 점들을 둘로 나누면 다음과 같다
6DOF camera poses
3DOF points
Least Squares for BA

70. 4. Bundle Adjustment
• Cost function에서 최적화할 대상 는 다음 그림과 같다.
• 카메라 pose와 3차원 점들을 둘로 나누면 다음과 같다
• 최적화 공식이 비선형이므로 non-linear least square method를 이용하여 문제를 해결해야 한다 (해당 슬라이드에서 이미 설명했다)
: Jacobian에서 (카메라 pose) 성분
: Jacobian에서 (3차원 점) 성분
Least Squares for BA

71. 4. Bundle Adjustment
• Cost function에서 최적화할 대상 는 다음 그림과 같다.
• 카메라 pose와 3차원 점들을 둘로 나누면 다음과 같다
• 최적화 공식이 비선형이므로 non-linear least square method를 이용하여 문제를 해결해야 한다 (해당 슬라이드에서 이미 설명했다)
: Jacobian에서 (카메라 pose) 성분
: Jacobian에서 (3차원 점) 성분
• 다음과 같이 를 구하고 에러가 수렴할 때 까지 반복적으로(Iterative) 수행한다
Least Squares for BA

72. 4. Bundle Adjustment
• 이 때 Hessian Matrix 는 다음과 같이 구성되어 있다
Sparsity of Hessian Matrix

73. 4. Bundle Adjustment
• 이 때 Hessian Matrix 는 다음과 같이 구성되어 있다
• Jacobian은 카메라 pose와 3차원 점에 따라 모양이 변한다.
• 만약 카메라 pose, 3차원 점이 한 개씩 있는 경우 Jacobian은 아래와 같다
Sparsity of Hessian Matrix

74. 4. Bundle Adjustment
• 이 때 Hessian Matrix 는 다음과 같이 구성되어 있다
카메라 pose (R,t)
3차원 점
3 3 3
2
• Jacobian은 카메라 pose와 3차원 점에 따라 모양이 변한다.
• 만약 카메라 pose, 3차원 점이 한 개씩 있는 경우 Jacobian은 아래와 같다
Sparsity of Hessian Matrix

75. 4. Bundle Adjustment
• 이 때 Hessian Matrix 는 다음과 같이 구성되어 있다
카메라 pose (R,t)
3차원 점
3 3 3
2
• Jacobian은 카메라 pose와 3차원 점에 따라 모양이 변한다.
• 만약 카메라 pose, 3차원 점이 한 개씩 있는 경우 Jacobian은 아래와 같다
• 카메라 pose, 3차원 점이 여러 개인 일반적인 경우 Jacobian은 다음과 같다
Sparsity of Hessian Matrix

76. 4. Bundle Adjustment
• 이 때 Hessian Matrix 는 다음과 같이 구성되어 있다
카메라 pose (R,t)
3차원 점
3 3 3
2
• Jacobian은 카메라 pose와 3차원 점에 따라 모양이 변한다.
• 만약 카메라 pose, 3차원 점이 한 개씩 있는 경우 Jacobian은 아래와 같다
• 카메라 pose, 3차원 점이 여러 개인 일반적인 경우 Jacobian은 다음과 같다
• 3차원 점 는 카메라의 위치 에서만 보이고 해당 블록만 값을 갖는다.
• 보이지 않는 나머지 부분은 위와 같이 영행렬이 된다. (Jacobian Sparsity)
Sparsity of Hessian Matrix

77. 4. Bundle Adjustment
• Jacobian의 희소성(Sparsity) 특징으로 인해 Hessian Matrix 또한 Sparsity를 갖는다
Image from Robot Mapping course - Freiburg Univ. https://youtu.be/wVsfCnyt5jA
• Hessian Matrix를 전개하면 다음과 같다 camera points
camera
points
Sparsity of Hessian Matrix

78. 4. Bundle Adjustment
• 하지만 일반적인 경우 3차원 점의 개수가
카메라 pose보다 훨씬 많으므로 다음과 같은
거대한 matrix가 생성된다.
• 따라서 역행렬을 계산할 때 매우 큰 연산시간이
소요되므로 이를 빠르게 개선해 나갈 다양한 방법이
필요하다
large computational cost!
real world data
Sparsity of Hessian Matrix

79. 4. Bundle Adjustment
• 이 때, Schur Complement를 사용하면 computational cost를 대폭 낮출 수 있다
Schur Complement

80. 4. Bundle Adjustment
• 이 때, Schur Complement를 사용하면 computational cost를 대폭 낮출 수 있다
• 1. 를 먼저 계산한다 (전진 대입: Forward Substitution)
• 우선 선형시스템에서 를 제거하기 위해 양변에 아래와 같은 행렬곱을 한다
Schur Complement

81. 4. Bundle Adjustment
• 이 때, Schur Complement를 사용하면 computational cost를 대폭 낮출 수 있다
• 1. 를 먼저 계산한다 (전진 대입: Forward Substitution)
• 우선 선형시스템에서 를 제거하기 위해 양변에 아래와 같은 행렬곱을 한다
• 위 행렬을 전개하면 아래와 같이 (1,2) 원소가 0이 되는 행렬이 만들어진다. 이를 marginalization이라고 한다
Schur Complement

82. 4. Bundle Adjustment
• 이 때, Schur Complement를 사용하면 computational cost를 대폭 낮출 수 있다
• 1. 를 먼저 계산한다 (전진 대입: Forward Substitution)
• 우선 선형시스템에서 를 제거하기 위해 양변에 아래와 같은 행렬곱을 한다
• 위 행렬을 전개하면 아래와 같이 (1,2) 원소가 0이 되는 행렬이 만들어진다. 이를 marginalization이라고 한다
• 위 노란색 부분만을 곱하면 없이 오직 카메라 pose incremental 값만 구할 수 있다
Schur Complement

83. 4. Bundle Adjustment
• 2. 를 계산한다 (후진 대입: Backward Substitution)
• 를 계산하고 계산한 값을 토대로 를 계산한다.
• 위 파란색 부분을 계산할 때 이전 단계에서 이미 모든 변수의 값을 구했으므로 단순하게 연산이 가능하다
Linear Solvers & Robust Cost function

84. 4. Bundle Adjustment
• 다음과 같은 Schur Complement를 통해 얻은 선형방정식을 계산할 때 다양한 방법이 적용될 수 있다.
• LU Factorization, Cholesky Factorization을 통해서 빠르게 계산할 수 있다.
• 또한 huber 같은 Robust Cost function을 사용하면 outlier의 영향력을 줄일 수 있다
Linear Solvers & Robust Cost function