Visual SLAM

Linear Algebra

SLAM에서 접할 수 있는 선형대수 문제에는 크게 비동차 시스템(Non-honomegenous), 동차 시스템(Homogeneous)이 존재한다.

Non-homogeneous system(Ax = b)

모르는 미지수를 $x$로 표시하고 어떤 현상이나 문제에 대해 관찰이나 측정으로부터 얻은 우리가 알고 있는 정보 행렬 $A$와 벡터 $b$로 표시한다. 행렬 A의 모양에 따라 3가지 경우로 나뉠 수 있다.

1. squared(full rank, $x=A^{-1}b$)
   행렬 $A$가 정방행렬이며 Full rank(선형독립인 행벡터의 갯수가 행렬의 행의 갯수와 동일) 가장 기본적인 경우를 의미하고 유일해가 존재하는 경우이다. 간단하게 행렬 $A$의 역행렬을 구하여 $x$를 구할 수 있다.

2. overdetermined ($x^*=argmin||Ax-b||$)
   행렬 A의 행의 크기가 열의 크기보다 큰 경우를 의미하며, 실제 세계에서 자주 발생하는 일반적인 경우이다. 모든 단서를 만족시키는 정확한 solution이 존재하지 않으며 $Ax-b=0$의 근사해를 찾는 최소 자승 문제(least-square)로 변경할 수 있다.

3. underdetermined
   행렬 A의 행의 크기가 열의 크기보다 작은 경우를 의미, 미지수의 개수가 단서의 개수보다 많은 경우이며 무한히 많은 해가 존재하거나 이 시스템을 만족시키는 해가 존재하지 않을 수 있다.

Homogeneous system(Ax = 0)

영벡터가 아니면서 위의 수식을 만족시키를 벡터 $x$를 찾는 것이 목적이고 그러한 조건을 만족시키는 벡터 $x$를 행렬 $A$의 null space라 한다. 동차 시스템의 해를 찾는 방법에는 SVD(Singular value Decompostion)이 있다. 이는 Eigenvalue와 Eigenvector를 이용한다.

Eigen Value, Eigen Vector

고윳값과 고유벡터는 정방행렬이면서 실수 값을 가지는 대칭행렬 $A=A^T$에 대하여 위의 수식을 만족시키는 벡터 $v$와 스칼라 $\lambda$의 조합을 의미한다. 이때 고윳값($\lambda$)이 0인 것에 해당하는 고유벡터($v$)를 찾으면 $Av=0v=0$을 만족한다.그것이 바로 동차 시스템 $Av=0$의 해가 되고 행렬 $A$에 대한 null space가 된다.

Singular value, Singular vector

일반적인 선형시스템에서 행렬 $A$가 정방행렬인 경우는 거의 드물고 대부분 비정방(Non-squared)행렬이다. 고윳값과 고유벡터의 성질을 유지하면서 비정방 행렬의 경우로 확장된 개념이 바로 특이값과 특이벡터이다.

비정방 행렬 $A$는 왼쪽 특이벡터들이 행벡터로 구성된 직교행렬 $U$와 특이값들이 대각성분으로 구성된 대각행렬 $D$, 오른쪽 특이벡터들이 열벡터로 구성된 직교행렬 $V$로 분해된다.

대각 행렬 $D$의 특이값들은 내림차순으로 정렬되어 행렬의 왼쪽 위부터 오른쪽 아래까지 배치되는데, 가장 작은 특이값에 해당하는 오른쪽 특이벡터를 찾으면 그것이 바로 선형시스템 $Ax=0$의 비자명해(Non-trivial solution)이 된다. 여기서 비자명해는 $||Ax||$를 최소화 시키는 단위벡터 $x$를 의미한다.

Skew-symmetric matrix(A.transpose() = -A)

반대칭 행렬은 어떤 행렬 $A$에 대하여 transpose 했을 때 자기 자신에 음수를 곱한 행렬이 나오는 행렬을 의미한다. 이러한 반대칭 행렬의 특성을 이용하는 예제가 대표적으로 두 벡터 사이의 외적(Cross product)을 반대칭 행렬과 벡터 사이의 곱셈으로 표현할 수 있다.

Non-linear optimization theorem

SLAM 시스템에서 자주 접하는 선형 시스템의 형태인 Over-determined시스템을 푸는 방법이다. 근사해를 찾기 위한 대표적인 방법으로 경사 하강법(Gradient descent method), 가우스-뉴턴법(Gauss-Newton method), 마지막으로 이 둘의 조합인 르벤버그-마쿼트법(Levenberg-Marquardt method)가 존재한다.

Least suqare problem

Over-determined 시스템은 단서의 개수가 미지수의 개수보다 많을 때 만들어지는 선형 시스템이고 모든 단서를 만족시키는 엄밀해가 존재하지 않는 경우이다. 따라서 모든 단서에 대해 어느 정도 만족시키는 근사해를 찾는다. 예시로 다음과 같은 수식을 선형시스템으로 표현하면 다음과 같다.

$ax_{i}^2+bx_i+c=y_i$

이는 $Ax=b$형태의 Over-determined 선형 시스템이다!

이러한 시스템의 엄밀해를 찾는 것은 다시 말하면 모든 포인트들을 지나가는 직선의 방정식을 찾는 것과 같고 그런 해는 대수적으로 불가능한 해이다. 따라서 이 시스템을 최소 제곱 문제로 변형하여 근사해를 찾는 문제로 만들기 위해서 각 포인트들의 실제 관측값과 예측 값 사이의 차이를 나타내는 기본 오차를 정의한다.

$e_i(x)=A_{i}x+b_i$ , loss function이 평균이 0이고 공분산이 $\Sigma$인 gaussian distribution을 따른다고 가정

최종적으로 정의되는 제곱 오차함수는 다음과 같다.

$E_i(x)=e_i^T(x)\Sigma^{-1}e_i(x)$

여기서 $\Sigma^{-1}=\Omega$이며 Information matrix라고 칭한다. 즉 공분산이 크면 데이터의 분포가 넓게 퍼져있으므로 information은 작고, 공분산이 작으면 데이터의 분포가 작으므로 information크다. 결국, 최적화 시에 $i$번째 포인트의 영향도를 가중치로 나타낸 것을 의미한다.

따라서 global loss function은 다음과 같다.

$F(x)=\Sigma_i{(e_i^T(x)\Sigma^{-1}e_i(x))}$

근사해를 찾기 위한 최적화 수식은 다음과 같다.

$x^*=argmin_{x}{F(x)}$

$x^*=argmin_{x}\Sigma_i{(e_i^T(x)\Sigma^{-1}e_i(x))}$

이러한 최소 제곱 문제는 한번의 연산을 통해 직접적으로 해를 구할 수 없고 iterative하게 해의 값을 조금씩 업데이트 해가며 수렴하는 지점을 찾아야 한다.

Gradient descent method

경사하강 방법은 전역 오차 함수 $F(x+\Delta x)$의 결과가 최소가 되도록 만드는 $\Delta x$를 찾기 위해 벡터 $x$에서의 1차 미분인 Gradient를 이용하는 방법이다.

위의 수식은 다음과 같이 정리할 수 있다.

$\nabla E_{i}(x)=2J_{i}^T(x) \Omega_i e_i(x)$

이 기울기 값을 이용해 구하려는 최적해인 $\Delta x$를 정의하면 다음과 같다.

$\Delta x=\Sigma_i{\lambda\nabla_{i}(x)=\Sigma_i(2\lambda J_{i}^T(x) \Omega_i e_i(x))}$

결국 스칼라 값인 step size라고 칭하는 $\lambda$는 일반적인 머신러닝에서 학습률(learning rate)와 동일한 의미를 지닌다. $x^*\leftarrow x + \lambda\frac{\partial F}{\partial x}=x+\Delta x$

Gauss-Newton method

가우스-뉴턴 방법은 전역 오차 함수 $F(x+\Delta x)$의 결과가 최소가 되도록 만드는 $\Delta x$를 찾기 위해 2차 미분인 곡률(Curvature)의 근삿값을 사용한다. 기본 오차 함수 $e_i(x+\Delta x)$를 taylor expansion을 이용하여 수식유도를 하면 다음과 같다.

$e_i(x+\Delta x)\approx e_i(x)+J_i(x)\Delta x$

$J_i(x)$오차 함수의 벡터 $x$에 대한 미분을 의미하는 자코비안 행렬이다.

이렇게 유도된 전역 오차 함수 $F(x+\Delta x)$에 대해 $\Delta x$로 미분해서 0이 되는 값을 찾으면 전역 오차 함수의 결과가 최소가 되도록 만드는 최적해를 찾을 수 있다.

$F(x+\Delta x)\approx c+2b^T\Delta x+\Delta x^TH\Delta x$

$\frac{\partial F(x+\Delta x)}{\partial\Delta x}\approx 2b+2H\Delta x$

$0 = 2b + 2H\Delta x$

$H\Delta x=-b$

$\Delta x = -H^{-1}b$

가우스-뉴턴 방법은 곡률을 이용해 해를 찾기 때문에 걸리는 시간이 더 적다는 장점이 있지만 역행렬을 구할 때 수치적으로 불안정하고 해가 발산할 위험이 있다는 단점이 존재한다.

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>
#include <vector>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv){
 double ar = 1.0, br = 2.0, cr = 1.0; // real value(실제 해)
 double ae = 2.0, be = -1.0, ce = -5.0 // init value(최적화를 통해 업데이트되는 변수)
 int N = 100; // 데이터의 총 갯수

 double w_sigma = 1.0;
 double inv_sigma = 1.0 / w_sigma;
 cv::RNG rng;

 vector<double> x_data, y_data;

 for(int i = 0; i < N; i++){
  double x = i / 100.0;
  x_data.push_back(x); // 모델의 입력값
  y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma)); // 모델의 출력값 계산 및 가우시안 노이즈 추가
 }
 
 int iterations = 100;
 double cost = 0, last_cost = 0;

 // least squre
 chrono::stead_clock::time_point t1 = chrono::steady_clock::now();
 for(int iter = 0; iter < iterations; iter++){
  Matrix3d H = Matrix3d::Zero(); // 행렬 H(영행렬)
  Vector3d b = Vector3d::Zero(); // 벡터 b(영벡터)
  cost = 0;
  
  for(int i = 0; i < N; i++){
   double xi = x_data[i], yi = y_data[i];
   double error = yi - exp(ae * xi * xi + be * xi + ce);
   Vector3d J;
   
   // Jacobian calculation
   J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // ae에 대한 편미분
   J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // be에 대한 편미분
   J[2] = -exp(ae * xi * xi + be * xi + ce); // ce에 대한 편미분

   H += inv_simga * inv_sigma * J * J.transpose();
   b += -inv_simga * inv_sigma * error * J;
   
   cost += error * error;
  }
  
  Vector3d dx = H.ldlt.solve(b); // cholesky decomposition
  
  if (iter > 0 && cost >= lastCost){
   cout << "cost:" << cost << ">= last cost:" << lastCost << ", break." << endl;
   break;
  }

  ae += dx[0];
  be += dx[1];
  ce += dx[2];
  
  lastCost = cost;
 }
 chorono::steady_cllock::time_point t2 = chrono::stead_clock::now();
 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1)
 cout << "estimated abc :" << ae << be << ce << endl
}

Projective Geomtry(사영기하학)

vanishing point(소실점)같은 현상이 나타나는 이유는 원근법에 의한 것이다. 즉, 현실세계에서는 평행하지만 사영된 공간에서는 교차하는 현상이 발생한다. 이로 인해 3d 공간을 다룰 때 필요한 유클리드 기하학이 이미지를 다룰때는 적용이 될 수 없다. 사영을 통해 소실되는 값은 다음과 같다.

1. Depth(거리)
2. Orthogonality(직교성) - 3d 공간에서는 직교하는 물체가 이미지 상에서 직교하지 않음
3. Parallelism(평행성) - 3d 공간에서는 평행하던 물체가 이미지 상에서 평행하지 않음
4. Scale(비율) - 3d 공간에서는 동일한 크기를 가짐에도 가까이 있는 물체는 크게 나타나고 멀리 있는 물체는 작게 나타남

유클리드 변환과 가장 큰 차이점은 유클리드 변환은 변환 전과 후의 물체의 길이, 직교성, 평행성, 비율 등을 유지해야 하는 제약 조건이 존재하지만 사영 변환(projective transformation)은 이를 모두 유지하지 않아도 된다는 것이다. 즉, 3d 공간에서는 무한의 거리를 2d 이미지에서는 유한하게 표현할 수 있다는 것을 의미한다. 이때 사영 기하학에서 사용하는 좌표계를 동차 좌표계(Homogeneous coordinates)라고 한다.

Example

두개의 벡터 {1, 2, 3}, {2, 4, 6}이 존재할 때 유클리드 기하학에서는 두 벡터는 다른 값을 가지고 있기 때문에 다른 좌표를 가리키는 것을 의미한다. 하지만 사영기하학의 동차 좌표계에서는 {2, 4, 6}의 최대 공약수인 2로 나누었을 때 {1, 2, 3}이 되고 두개의 벡터는 같은 좌표를 가리킨다고 볼 수 있다. 즉, 위에서 정의했던 비율에 대한 제약조건을 유지하지 않아도 된다는 것을 의미하는 대표적인 예제이다. 결론적으로 벡터 전체를 비율값으로 나눈다면 변하지 않는 물체의 진짜 크기로 돌아간다는 점을 의미한다.

위의 그림에서 $u, v$는 2차원 축 방향을 나타내며, $w$는 비율 값의 축이다. 비율 값이 1로 되어있는 공간이 2d 유클리드 공간이다. 이를 통해 유클리드 공간은 사영 공간의 부분집합이라는 점을 알 수 있다. 특히, 2D 유클리드 공간에서 점으로 표현되는 공간은 사영 공간에서는 {0, 0, 0}에서 시작하여 {x, y, 1}을 지나 {wx, wy, w}로 연장되는 직선이 된다. 따라서 사영 기하학의 정의는 투영을 통한 N+1 차원에서 N차원으로의 차원 축소를 위한 기하학임을 나타낸다.

행렬 또한 동차 좌표계로 표현될 수 있다. SO(3) 회전 매트릭스는 유클리드 공간에서 데카르트 좌표계로 표현되는 3x3 형태의 행렬이다. 이 행렬을 사영 공간에서 표현하기 위해서는 행렬의 차원을 하나 증가시키고 우측 최하단 항에 1을 위치시킨다. SE(3) 변환 행렬은 유클리드 공간에서 6 DOF 강체운동을 표현하는 4x4형태의 행렬이다. SO(3), SE(3) 행렬 모두 우측 최하단 값이 1로 되어있는 경우를 볼 수 있는데, 이는 유클리드 공간에서의 변환을 수행하고 있다는 것을 의미한다.

Local feature

특징점은 Object detection, Tracking, Matching과 같은 CV의 영역에서 사용되는 기술이다. 특징점은 크게 2개로 이루어져 있다.

1. Keypoint(키포인트) - 코너, 엣지, 영역(blob)과 같이 이미지 속 두드러지는 성질을 가진 영역
2. Descriptor(기술자) - 이미지 속 키포인트가 위치한 영역 주변이 어떻게 생겼고 어떠한 시각적 요소를 가지고 있는지 표현하는 벡터

이를 통해 효율적으로 서로 다른 임지 속에서 동일한 물체를 의미하는 부분을 찾아 이미지 매칭을 수행하고, 이를 통해 두 이미지 간의 이동이나 3D 공간이 어떻게 구성되어있는지 추론할 수 있다.

Parameters of Camera

카메라의 파라미터는 world를 구성하고 있는 3차원 좌표가 어떻게 카메라의 2차원 픽셀로 변환되는지 수식적으로 이해하기 위해 필요한 값이다.

$x$는[3X1] 카메라의 이미지 평면에 사영된 2차원 픽셀 좌표를 동차좌표계로 나타낸값이다.
$P$[3X4]는 카메라 파라미터로 구성된 행렬로서 카메라 내부 파라미터와 외부 파라미터의 조합으로 구성되어 있다.
결국 카메라 캘리브레이션 과정이란 $P$를 구하는 과정을 의미하는 것이다.

Parameters	설명
Extrinsic	world 좌표계에서 camera 좌표계로 변환하는데 필요한 파라미터(from $S_O$ to $S_k$)
Intrinsic	camera 좌표계에서 image 좌표계를 거쳐 sensor 좌표계로 변환하는데 필요한 파라미터, 카메라 distortion 파라미터 포함(from $S_k$ to $S_s$)

Extrinsic Parameters

카메라의 외부 파라미터는 world 좌표계를 기준으로 상대적인 카메라의 자세를 나타내는 값이며 3자유도의 카메라 이동 파라미터와 3 자유도의 카메라 회전 파라미터를 가진다. 즉 유클리드공간에서의 변환 행렬을 구하는 것을 의미한다.

위의 그림과 같이 world 좌표계 상에 점 $P$가 존재할 때 점 $P$의 좌표를 $X_p=[X_p, Y_p, Z_p]^T$라 정의하고 카메라 $O$의 중심좌표를 $X_o=[X_o, Y_o, Z_o]^T$라 가정했을때 camera 좌표계를 기준으로 점 $P$의 좌표를 표현하면 다음과 같다.

${X_p}^k=R(X_p-X_o)$

이는 점 $P$의 world 좌표에서 camera 중심좌표 $O$를 빼고 카메라의 회전 행렬 $R$을 곱하면 camera 좌표계에서 바라본 점 $P$의 좌표는 ${X_p}^k$로 나타낸다.

즉 $H$는 일반적인 유클리드 공간에서 SE(3) 변환 행렬을 의미하는 것이다.

Intrinsic Parameters

카메라의 내부 파라미터는 3차원 점 $P$가 카메라 좌표계 $S_k$에서 영상 좌표계 $S_c$를 거쳐 센서 좌표계 $S_s$까지 도달하는데 필요한 변환 파라미터를 나타낸다. 각 단계별 순서는 다음과 같다.

1. 원근 사영 변환
2. 영상 좌표에서 실제 센서 좌표 변환
3. 랜즈 왜곡 변환

원근 사영 변환(Perspective projection transformation)

원근 사영 변환은 핀홀 카메라 모델에서 왜곡이 없는 렌즈를 가정하여 3차원 점 $P$가 2차원 이미지 픽셀 $\bar{P}$로 변환되는 과정을 의미한다.

위의 그림에서 $c$는 이미지 평면에서 렌즈 중심(optical center)까지의 거리인 초점 거리를 의미한다. 두 직각 삼각형의 비례식을 통해 이미지 상에서의 $x, y$좌표를 구하는 절차는 다음과 같다.

영상 좌표에서 실제 센서 좌표 변환

영상 좌표에서 실제 센서 좌표 변환은 두 좌표계의 중심이 일치하지 않고 물체에 대한 상이 맺히는 실제 센서의 가로세로 축의 비율이 동일하지 않은 차이를 affine 변환을 활용하여 보정하는 과정을 의미한다.

위의 그림과 같이 영상 좌표계 $S_c$의 중심은 2차원 영상 평면에 중앙에 위치해 있지만 실제 센서 좌표계 $S_s$의 중심은 2차원 영상 평면의 좌상단에 위치해 있다. 이러한 차이를 affine 변환 행렬 ${H_c}^s$로 나타내어 행렬의 우상단에는 두 좌표계의 원점 사이의 거리 차이 $[x_H, y_H]^T$를 넣고 대각 성분에 가로세로 축의 비율 차이 $m$을 넣어 표현할 수 있다.

렌즈 왜곡 변환

앞에서 살펴본 좌표계 사이의 변환 과정은 렌즈의 왜곡 계수를 고려하고 있지 않다. 따라서 센서 좌표계에서 $S_s$에서 렌즈의 왜곡을 고려한 센서 좌표계 $S_a$를 추가적으로 고려하여 화소의 위치를 정확하게 계산할 수 있다.

이러한 왜곡 모델을 가질 경우 센서 좌표계 $S_s$의 점 $X^s=[x^s, y^s]$에 대해 $\delta X=[\delta x, \delta y]$을 더하는 형태로 구성이 된다.
$\delta x$는 센서 좌표계에서 좌표 값과 왜곡 파라미터를 입력으로 가지는 비선형 함수로서 왜곡 모델에 따라 다르게 정의된다. 배럴 왜곡 모델의 $\delta X$는 다음과 같다.

$\delta{x}=q_1 r^2 + q_2 r^4$

$\delta{y}=q_1 r^2 + q_2 r^4$

결론적으로 최종적인 camera calibration matrix $P$는 다음과 과정을 통해 구해진다는 것이다.