arXiv preprint
English version
Notes on Various Errors and Jacobian Derivations for SLAM
This paper delves into critical concepts and meticulous calculations pertinent to Simultaneous Localization and Mapping (SLAM), with a focus on error analysis and Jacobian matrices. We introduce various types of errors commonly encountered in SLAM, includi
arxiv.org
1. Introduction
본 포스트에서는 SLAM에서 사용하는 다양한 에러에 대한 정의 및 이를 최적화하기 위해 사용하는 자코비안에 대해 설명한다.
본 포스트에서 다루는 에러는 다음과 같다.
- Reprojection error
- Photometric error
- Relative pose error
- Line reprojection error
- IMU measurement error :
카메라 포즈는 회전행렬
본 포스트에서 다루는 자코비안은 다음과 같다.
- Camera pose (SO(3)-based)
- Camera pose (SE(3)-based)
- Map point
- Relative pose (SE(3)-based)
- 3D plücker line
- Quaternion representation
- Camera intrinsics
- Inverse depth
- IMU error-state system kinematics :
- IMU measurement :
2. Optimization formulation
2.1. Error derivation
SLAM에서 에러는 센서 데이터에 의한 관측값(measurement)
-
위와 같이 관측값과 예측값의 차이를 에러로 정하고 에러를 최소로 하는 최적의 상태변수
2.2. Error function derivation
일반적으로 다량의 센서 데이터가 들어오면 이에 대한 수십~수백개의 에러가 벡터 형태로 계산된다. 이 때, 에러가 정규분포를 따른다고 가정하고 에러 함수로 변환하는 작업을 수행한다.
에러 함수를 모델링하기 위한 확률변수에 대한 다변수 정규분포는 다음과 같다.
-
-: mean vector
-: covariance matrix
-: determinant of
-: information matrix (inverse of covariance matrix)
에러를 평균이
위 식으로부터 다음과 같은 비례식이 성립한다.
위 식에 log-likelihood를 적용한
log-likelihood
단일 에러가 아닌 모든 에러를 더하여 표현하면 다음과 같고 이를 에러 함수
2.3. Non-linear least squares
최종적으로 풀어야 하는 최적화 수식은 다음과 같다.
위 공식에서 에러를 최소화하는 최적 파라미터
실제 SLAM의 최적화 수식을 유도하는 과정은 크게 두 방법이 존재한다. 첫 번째로 앞서 설명한 것처럼 MLE를 통해 information matrix를 고려하여
예를 들어, GN 방법을 사용해서 해당 문제를 푼다고 가정해보자. 문제를 푸는 순서는 다음과 같다.
- 에러함수를 정의한다
- 테일러 전개로 근사 선형화한다
- 1차 미분 후 0으로 설정한다.
- 이 때 값을 구하고 이를 에러함수에 대입한다
- 값이 수렴할 때 까지 반복한다.
에러함수
이 때,
이 때,
위 식을 전개한 후 치환하면 아래와 같다.
위 전체 에러에 적용하면 다음과 같다.
이를 정리하면 다음과 같은 공식이 도출된다.
이렇게 구한
지금까지 과정을 반복적(iterative)으로 수행하는 알고리즘을 Gauss-Newton 방법이라고 한다. LM 방법은 GN 방법과 비교했을 때 전체적인 프로세스는 동일하나 증분량을 구하는 공식에서 damping factor
3. Reprojection error
Reprojection 에러는 feature-based Visual SLAM에서 주로 사용되는 에러이다. 주로 feature-based method 기반의 visual odometry(VO) 또는 bundle adjustment(BA)를 수행할 때 사용된다. BA에 대한 자세한 내용은 [SLAM] Bundle Adjustment 개념 리뷰 포스트를 참조하면 된다.
NOMENCLATURE of reprojection error
- 이미지 평면에 프로젝션하기 위해 3차원 공간 상의 점
를 non-homogeneous 변환한 점
- 이미지 평면에 프로젝션하기 위해 3차원 공간 상의 점
- 렌즈 왜곡을 보정한 후 이미지 평면에 프로젝션한 점. 만약 왜곡 보정이 입력 단계에서 이미 수행된 경우
이 된다.
- 렌즈 왜곡을 보정한 후 이미지 평면에 프로젝션한 점. 만약 왜곡 보정이 입력 단계에서 이미 수행된 경우
: 카메라 내부(intrinsic) 파라미터 : 로 프로젝션하기 위해 내부 파라미터의 마지막 행을 생략했다. : 모델의 상태변수 : 카메라 포즈의 개수 : 3차원 점의 개수 : 간략한 표기를 위해 생략하기도 한다 : 관측된(observed) 특정짐의 픽셀 좌표 : 예측된(estimated) 특징점의 픽셀 좌표 : Transformation, 3차원 점 를 카메라 좌표계 로 변환,
: SO(3) 회전행렬 과 3차원 벡터 를 한 번에 업데이트할 수 있는 연산자. : 각속도 : 각속도 의 반대칭 행렬

i번째 핀홀카메라

위와 같이 카메라 내부/외부(intrinsic/extrinsic) 파라미터를 활용한 모델을 projection model이라고 한다. 이를 통한 reprojection 에러는 다음과 같이 정의한다.
모든 카메라 포즈, 3차원 점들에 대한 에러 함수는 다음과 같이 정의된다.
엄밀하게 말하면 상태 증분량
위 식은 테일러 1차 근사를 통해 다음과 같이 표현이 가능하다.
이를 미분하여 최적의 증분량
위 식은 선형시스템
왼쪽 곱셈
3.1. Jacobian of the reprojection error
3.1.1. Jacobian of camera pose
포즈에 대한 자코비안
Chain rule을 사용하여 위 식을 정리하면 다음과 같다. 이 때, 편의를 위해
이 때, 회전행렬
렌즈에 의한 왜곡(distortion)은 일반적으로 계산의 복잡성으로 인해 자코비안 계산 시 고려하지 않는다. 또한 렌즈 왜곡 보정(undistortion)이 이미지 입력 과정에서 이미 수행되었다고 가정하면 이를 고려할 필요가 없어진다.
따라서
다음으로
다음으로
3.1.2. Lie theory-based SO(3) optmization
마지막으로
- 중복되는 파라미터를 계산해야 하기 때문에 최적화 수행 시 연산량이 증가한다.
- 추가적인 자유도로 인해 수치적인 불안정성(numerical instability) 문제가 야기될 수 있다.
- 파라미터가 업데이트될 때마다 항상 제약조건을 만족하는 지 체크해줘야 한다.
lie theory를 사용하면 제약조건으로부터 자유롭게 최적화를 수행할 수 있다. 따라서 lie group SO(3)
하지만
는 각속도 를 exponential mapping하여 3차원 회전행렬 로 변환하는 연산을 말한다. exponential mapping에 대한 자세한 내용은 해당 링크를 참조하면 된다.
이 때, 작은 lie algebra 증분량
위 두 방법 사이에는 다음과 같은 관계가 존재한다. 자세한 내용은 해당 링크의 4.3.3 챕터를 참조하면 된다.
위 식에서 두 번째 행은 BCH 근사를 사용하여 왼쪽 자코비안(left jacobian)이 유도된 형태이고 세 번째 행은 작은 회전량 에 대해 1차 테일러 근사가 적용된 형태이다. 에 대한 자세한 내용은 Lie theory 개념 리뷰 포스팅을 참조하면 된다.
세 번째 행의 근사를 이해하기 위해 임의의 회전 벡터가 주어졌을 때 회전행렬을 exponential mapping 형태로 전개하면 다음과 같이 나타낼 수 있다.
작은 크기의 회전행렬에 대해서는 2차 이상의 고차항을 무시하여 다음과 같이 근사적으로 나타낼 수 있다.
위 식 또한 두 번째 행에서 작은 회전행렬에 대한 근사
따라서 기존의 자코비안은
최종적인 포즈에 대한 자코비안
3.2. Jacobian of Map Point
3차원 점
Chain rule을 사용하여 위 식을 정리하면 다음과 같다.
이 중,
따라서
일반적으로
3.3. Code implementations
- g2o 코드: edge_project_xyz.cpp#L82
- g2o 코드: edge_project_xyz.cpp#L80
4. Photometric error
Phtometric 에러는 direct Visual SLAM에서 주로 사용되는 에러이다. 주로 direct method 기반의 visual odometry(VO) 또는 bundle adjustment(BA)를 수행할 때 사용된다. direct method에 대한 자세한 내용은 [SLAM] Optical Flow와 Direct Method 개념 및 코드 리뷰 포스트를 참조하면 된다.
NOMENCLATURE of photometric error
- 이미지 평면에 프로젝션하기 위해 3차원 공간 상의 점
를 non-homogeneous 변환한 점
- 이미지 평면에 프로젝션하기 위해 3차원 공간 상의 점
- 렌즈 왜곡을 보정한 후 이미지 평면에 프로젝션한 점. 만약 왜곡 보정이 입력 단계에서 이미 수행된 경우
이 된다.
- 렌즈 왜곡을 보정한 후 이미지 평면에 프로젝션한 점. 만약 왜곡 보정이 입력 단계에서 이미 수행된 경우
: 카메라 내부(intrinsic) 파라미터 : 로 프로젝션하기 위해 내부 파라미터의 마지막 행을 생략했다. : 이미지 내의 모든 특징점들의 집합 : 일반적으로 표기를 생략하여 간단하게 나타내기도 한다. : 첫번째 이미지와 두번째 이미지에서 i번째 특징점의 픽셀 좌표 : 두 SE(3) 군을 결합(composition)하는 연산자 : Transformation, 3차원 점 를 카메라 좌표계로 변환, : 3차원 각속도와 속도로 구성된 벡터. twist라고 불린다. : hat 연산자가 적용된 twist의 lie algebra (4x4 행렬)

위 그림에서 3차원 점

direct method의 특징 중 하나는 feature-based와 달리 어떤
photometric 에러는 grayscale 불변성 가정에 기반하며 스칼라 값을 가진다. photometric 에러를 통해 non-linear least squares를 풀기 위해 다음과 같은 에러 함수
엄밀하게 말하면 상태 증분량
이는 1차 테일러 근사를 통해 다음과 같이 표현이 가능하다.
이를 미분하여 최적의 증분량
위 식은 선형시스템
왼쪽 곱셈
4.1. Jacobian of the photometric error
(
이를 자세히 풀어서 보면 다음과 같다.
Chain rule을 적용하여 위 식을 다시 표현하면 다음과 같다.
이 때, 변환행렬
렌즈에 의한 왜곡(distortion)은 일반적으로 계산의 복잡성으로 인해 자코비안 계산 시 고려하지 않는다. 또한 렌즈 왜곡 보정(undistortion)이 이미지 입력 과정에서 이미 수행되었다고 가정하면 이를 고려할 필요가 없어진다.
따라서
다음으로
4.1.1. Lie theory-based SE(3) optimization
마지막으로
- 중복되는 파라미터를 계산해야 하기 때문에 최적화 수행 시 연산량이 증가한다.
- 추가적인 자유도로 인해 수치적인 불안정성(numerical instability) 문제가 야기될 수 있다.
- 파라미터가 업데이트될 때마다 항상 제약조건을 만족하는 지 체크해줘야 한다.
따라서 제약조건으로 부터 자유로운 최소 파라미터(minimal parameter) 표현법인 lie theory 기반 최적화 방식을 일반적으로 사용한다. lie group SE(3) 기반 최적화 방법은 비선형의 회전행렬을 포함하는
이를 통해 기존의 식은 다음과 같이 변경된다.
-
-
는 twist 를 exponential mapping하여 3차원 포즈로 변환하는 연산을 말한다. exponential mapping에 대한 자세한 내용은 해당 링크를 참조하면 된다.
지금까지 자코비안들은 계산하기 용이했던 반면에
이 때, 작은 lie algebra 증분량
위 두 방법 중
두 방법 사이에는 다음과 같은 변환이 존재하며 이를 BCH 근사라고 한다. 자세한 내용은 Lie theory 개념 정리 포스팅을 참조하면 된다.
-
위 식에서 보다시피
따라서
위 식에서 두 번째 행은 작은 twist 증분량에 대해 1차 테일러 근사가 적용된 형태이다. 두 번째 행의 근사를 이해하기 위해 임의의 twist 가 주어졌을 때 변환행렬 를 exponential mapping 형태로 전개하면 다음과 같이 나타낼 수 있다.
작은 크기의 twist 증분량에 대해서는 2차 이상의 고차항을 무시하여 다음과 같이 근사적으로 나타낼 수 있다.
최종적인 포즈에 대한 자코비안
이 때,
4.2. Code implementations
- Visual SLAM 입문 챕터8 코드: direct_sparse.cpp#L111
- DSO 코드: CoarseInitializer.cpp#L430
- DSO 코드2: CoarseTracker.cpp#L320
5. Relative pose error
Relative pose 에러는 주로 pose graph optimization(PGO)에서 사용하는 에러이다. PGO에 대한 자세한 내용은 [SLAM] Pose Graph Optimization 개념 설명 및 예제 코드 분석 포스트를 참조하면 된다.
NOMENCLATURE of relative pose error
: 예측값 : 관측값 (virtual measurement) : pose graph의 모든 포즈 노드 : 표현의 편의를 위해 생략하여 표기하기도 한다. : 두 SE(3) 군을 결합(composition)하는 연산자 : SE(3)를 twist 로 변환하는 연산자. Logarithm mapping에 대한 자세한 내용은 해당 포스트를 참조하면 된다.

Pose graph 상에서 두 노드
relative pose 에러를 최적화하는 과정을 pose graph optimization(PGO)라고 하며 graph-based SLAM의 back-end 알고리즘으로도 불린다. Front-end의 visual odometry(VO) 또는 lidar odometry(LO)에 의해 순차적으로 계산되는 노드
즉, PGO는 일반적으로 loop closing과 같은 특수한 상황이 발생할 때 수행된다. 로봇이 이동하면서 같은 장소를 재방문하는 경우 loop detection 알고리즘이 동작하여 루프를 판별한다. 이 때 루프가 탐지되면 기존 노드
Pose graph 상의 모든 노드에 대한 relative pose 에러는 다음과 같이 정의할 수 있다.
엄밀하게 말하면 상태 증분량
위 식은 테일러 1차 근사를 통해 다음과 같이 표현이 가능하다.
이를 미분하여 모든 노드에 대한 최적의 증분량
위 식은 선형시스템
오른쪽 곱셈
5.1. Jacobian of relative pose error
(
이를 자세히 풀어서 보면 다음과 같다.
5.1.1. Lie theory-based SE(3) optimization
위 자코비안을 구할 때, 위치에 관련된 항
- 중복되는 파라미터를 계산해야 하기 때문에 최적화 수행 시 연산량이 증가한다.
- 추가적인 자유도로 인해 수치적인 불안정성(numerical instability) 문제가 야기될 수 있다.
- 파라미터가 업데이트될 때마다 항상 제약조건을 만족하는 지 체크해줘야 한다.
따라서 제약조건으로 부터 자유로운 최소 파라미터(minimal parameter) 표현법인 lie theory 기반 최적화 방식을 일반적으로 사용한다. lie group SE(3) 기반 최적화 방법은 비선형의 회전행렬을 포함하는
이를 통해 기존의 식은 다음과 같이 변경된다.
-
-
는 twist 를 exponential mapping하여 3차원 포즈로 변환하는 연산을 말한다. exponential mapping에 대한 자세한 내용은 해당 링크를 참조하면 된다.
이 때,
이를 자세히 풀어쓰면 다음과 같다.
위 식을 보면
위 식에서 증분량 항을 왼쪽 또는 오른쪽으로 이동시켜야꼴로 항이 정리된다. 이를 수행하기 위해 아래와 같은 adjoint matrix of SE(3)의 성질을 이용해야 한다. Adjoint martix에 대한 자세한 내용은 해당 포스트를 참조하면 된다.
위 식을에 대한 식으로 변형하면 다음과 같다.
그리고 정리하면 다음과 같은 공식을 얻을 수 있다.
(
이를 간단하게 표현하기 위해 치환하여 표시하면
-
-
-
위 식은 오른쪽 BCH 근사를 사용하여 정리할 수 있다.
오른쪽 BCH 근사는 다음과 같다.
자세한 내용은 Lie theory 개념 정리 포스팅을 참조하면 된다.
BCH 근사를 사용하여 (
최종적으로 치환을 풀고
따라서 최종적인 relative pose 에러의 SE(3) 버전 자코비안은 다음과 같다.
이 때,
만약
5.2. Code implementations
- g2o 코드: edge_se3_expmap.cpp#L55
- 위 g2o 코드에서는 에러를
로 정의하여 자코비안이 위 설명과 약간 달라지게 된다. - 이는 (
)에서 는 왼쪽으로 항을 넘겨서 정리하고 는 오른쪽으로 항을 넘겨서 정리한 후 합쳐준 형식과 동일하다. - 또한
으로 근사한 것으로 보인다. 따라서 실제 구현된 코드는 다음과 같다.
- 위 g2o 코드에서는 에러를
6. Line reprojection error
Line reprojection 에러는 plücker coordinate로 표현한 3차원 공간 상의 직선을 최적화할 때 사용하는 에러이다. Plücker coordinate에 대한 자세한 내용은 Plücker Coordinate 개념 정리 포스트를 참조하면 된다.
NOMENCLATURE of line reprojection error
: Plücker 직선의 변환 행렬 : 직선의 내부 파라미터 행렬(line intrinsic matrix) : 3차원 직선의 회전 행렬 : 3차원 직선이 원점과 떨어진 거리 정보를 포함하는 행렬 : SO(3) 회전행렬에 대응하는 파라미터 : SO(2) 회전행렬에 대응하는 파라미터 : 번째 열벡터(column vector) : 상태 변수 : orthonormal 표현법의 상태 변수 : Lie theory를 통한 업데이트 방법은 해당 링크를 참조하면 된다 : 상태 변수 를 한 번에 업데이트할 수 있는 연산자.

3차원 공간 상의 직선은 Plücker Coordinate를 사용하여 6차원 열벡터로 표현할 수 있다.
앞서 설명한
6.1. Line Transformation and projection
월드 좌표계에서 본 직선을
해당 직선을 이미지 평면 상에 프로젝션시키면 다음과 같다.
6.2. Line reprojection error

직선의 reprojection 에러
이는 점과 직선 사이의 거리 공식을 통해 나타낼 수 있다. 이 때,
6.3. Orthonormal representation
앞서 구한
- 중복되는 파라미터를 계산해야 하기 때문에 최적화 수행 시 연산량이 증가한다.
- 추가적인 자유도로 인해 수치적인 불안정성(numerical instability) 문제가 야기될 수 있다.
- 파라미터가 업데이트될 때마다 항상 제약조건을 만족하는 지 체크해줘야 한다.
따라서 직선을 최적화 할 때는 일반적으로 최소 파라미터인 4자유도로 변경하기 위해 orthonormal 표현법을 사용한다. 즉, 직선을 표현할 때는 Plücker Coordinate를 사용하지만 최적화를 수행할 때는 orthonormal 표현법으로 변형한 뒤 최적값을 업데이트하고 다시 Plücker Coordinate로 돌아오는 방식을 취한다.
Orthonormal 표현법은 다음과 같다. 3차원 공간 상의 직선은 항상 다음과 같이 표현 가능하다.
임의의 Plücker 직선
이 때, 상삼각행렬(upper triangle matrix)
실제 최적화를 수행할 때는
6.4. Error function formulation
직선에 대한 reprojection 에러
엄밀하게 말하면 상태 증분량
위 식은 테일러 1차 근사를 통해 다음과 같이 표현이 가능하다.
이를 미분하여 최적의 증분량
6.4.1. The analytical jacobian of 3d line
이전 섹션에서 설명한 것처럼 비선형 최적화를 수행하기 위해서는
orthonormal 표현법에 대한 자코비안
카메라 포즈에 대한 자코비안
6.5. Code implementations
- Structure PLP SLAM 코드: g2o/se3/pose_opt_edge_line3d_orthonormal.h#L62
- Structure PLP SLAM 코드2: g2o/se3/pose_opt_edge_line3d_orthonormal.h#L81
'Fundamental' 카테고리의 다른 글
3D 강체 변환(Rigid Body Transformation) 개념 정리 (6) | 2023.01.24 |
---|---|
[SLAM] 에러와 자코비안 정리(Errors and Jacobians) 정리 Part 2 (3) | 2023.01.13 |
Quaternion kinematics for the error-state Kalman filter 내용 정리 Part 3 (8) | 2022.10.10 |
[SLAM] 파티클 필터(Particle Filter) 개념 정리 (1) | 2022.10.07 |
Quaternion kinematics for the error-state Kalman filter 내용 정리 Part 2 (5) | 2022.08.31 |