Visual Odometry 요약

원본 : http://avisingh599.github.io/vision/monocular-vo/

 

<사전 지식>

- Camera Calibration

- Feature Extraction 

- RANSAC

- Essential Matrix

 

Intro

어릴적 눈을 감고 걸으면서 원(Circle) 혹은 한 변이 1 ~ 2m의 정사각형(Square)를 발자취로 만들어 본 경험이 있다. 뒤돌아 보면 예상과는 다르게 원 모양이 삐뚤하거나 정사각형이 아닌 직사각형이 되어 있기도 하고, 시작점과 도착점이 만나지 않는 경우가 생긴다. 발자취로 예상한 나의 Odometry가 정확하지 않은 탓이다.

Visual Odometry도 마찬가지다. 이미지 프레임간(t, t+1) R, t 행렬을 매번 구해 Odometry를 하여도 Ground Truth와는 조금 다른 오차가 생긴다.

---

배경지식

흔히 차량에 부착된 오도미터(Odometry)는 바퀴 회전수를 측정하지만, 로봇에서 사용되는 오도미터는 지나온 거리(distance) 뿐만 아니라, 전체적인 로봇의 궤적(trajectory)을 포함한다. 따라서, 모든 시간 t에는 다음이 기록된다.

$ [ x^t y^t z^t \alpha^t \beta^t \gamma^t ] $

- $ x^t y^t z^t $ 는 Caetesian 좌표계

- $ \alpha^t \beta^t \gamma^t $ 는 오일러 각도를 뜻한다. 

 

정의

Visual odometry는 즉, 시간 t 동안 6-DOF 궤적을 Video stream에 따라 구축하는 것이다.

- 단안 카메라를 이용하면 Monocular Visual Odometry

- 스테레오 카메라를 이용하면 Stereo Visual Odometry

 

블로그 저자의 말에 의하면 Monocular는 Scale Factor 단위로 움직임을 표현할 수 있고, Stereo 카메라를 사용하면 1M, 2M 등 물리적인 수치로 움직임을 나타낼 수 있다고 한다.
물체와 Stereo 카메라 간의 거리가 멀면, 거리를 측정하기 힘들어 Monocular 카메라와 다름이 없다. 

 

입력

- $I_l^t$, $I_r^t$, ($I_l^{t+1}$, $I_r^{t+1}$) 즉, 타임스탬프 간격 이미지

- 카메라의 Intrinsic(K, 5DOF) , Extrinsic(R[I | t], 6DOF) Parameters 를 알고 있다고 가정한다.

출력

- 매 TimeStamp 마다 R, T를 구한다. 

 

알고리즘 

Monocular Camera (2D-to-2D)

1. Capture images: It, It+1,
2. Undistort the above images.
3. Use FAST algorithm to detect features in It, and track those features to I(t+1).
   A new detection is triggered if the number of features drop below a certain threshold.
4. Use Nister’s 5-point alogirthm with RANSAC to compute the essential matrix.
5. Estimate R,t from the essential matrix that was computed in the previous step.
6. Take scale information from some external source (like a speedometer), and concatenate the translation vectors, and rotation matrices.

Stereo Camera (3D-to-3D)

1. Capture images: Itl, Itr, It+1l, It+1r
2. Undistort, Rectify the above images.
3. Compute the disparity map Dt from Il^t, Ir^t and the map Dt+1 from Il^(t+1), Ir^(t+1).
4. Use FAST algorithm to detect features in Il^t, Il^(t+1) and match them.
5. Use the disparity maps Dt, Dt+1 to calculate the 3D posistions of the features detected in the previous steps.
   Two point Clouds Wt, Wt+1 will be obtained
6. Select a subset of points from the above point cloud such that all the matches are mutually compatible.
7. Estimate R,t from the inliers that were detected in the previous step.

 

<Keyword>

- FAST Algorithm

- Nister's 5-Point Algorithm with RANSAC

- Essential Matrix 

 

Summary)

Undistor -> Feature Detection -> Feature Tracking(& Feature Re-Detection)

-> Essential Matrix Estimation -> RANSAC -> Computing 'R', 't'

 

1. Feature Detection

- 입력 : Mat 형식 이미지

- 출력 : vector<Point2f>

<C++>

void featureDetection(Mat img_1, vector<Point2f>& points1)	{ 
  vector<KeyPoint> keypoints_1;
  int fast_threshold = 20;
  bool nonmaxSuppression = true;
  FAST(img_1, keypoints_1, fast_threshold, nonmaxSuppression);
  KeyPoint::convert(keypoints_1, points1, vector<int>());
}

<Python>

# Initiate FAST object with default values
fast = cv.FastFeatureDetector_create()
# find and draw the keypoints
kp = fast.detect(img,None)
img2 = cv.drawKeypoints(img, kp, None, color=(255,0,0))

 

2. KLT Tracker

- 매번 tracking할 corner를 확인한다.

- $I^t$에서 취득한 코너가 $I^{t+1}$에서 Tracking 된다.

<C++>

void featureTracking(Mat img_1, Mat img_2, 
                     vector<Point2f>& points1, vector<Point2f>& points2,
                     vector<uchar>& status)	
{ 
  //this function automatically gets rid of points for which tracking fails
  vector<float> err;					
  Size winSize = Size(21,21);																								
  TermCriteria termcrit = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01);

  calcOpticalFlowPyrLK(img_1, img_2, points1, points2, 
                       status, err, winSize, 3, termcrit, 0, 0.001);

  //getting rid of points for which the KLT tracking failed or those who have gone outside the frame
  int indexCorrection = 0;
  for( int i=0; i<status.size(); i++)
  {  
        Point2f pt = points2.at(i- indexCorrection);
     	if ((status.at(i) == 0)||(pt.x<0)||(pt.y<0))	
        {
              if((pt.x<0)||(pt.y<0))	
              {
                status.at(i) = 0;
              }
              points1.erase (points1.begin() + i - indexCorrection);
              points2.erase (points2.begin() + i - indexCorrection);
              indexCorrection++;
        }
  }
}

<Python

# calculate optical flow
p1, st, err = cv.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)
# Select good points
if p1 is not None:
  good_new = p1[st==1]
  good_old = p0[st==1]

3. Feature Re-Detection

- Tracking을 하면서 Camara View를 벗어나면, 몇몇 점(Points)들을 잃게 된다.

- 따라서, 점이 특정 Threshold(e.g. 2000)이하로 떨어지면, 다시 특징점 추출을 시도 한다.

 

4. Essential Matrix Estimation with RANSAC (핵심!!)

- Essential Matrix(5 DOF)는 다음과 같이 정의 된다. $ y_1^{T}Ey_2 = 0 $

- y1, y2는 동차 정규화된(Homogenous normalised) 이미지 좌표계이다.

- 8개의 대응점을 필요로 한다. 

- Feature Traking 알고리즘은 완벽하지 않기 때문에, 에러 수정이 필요하다.

  1) 대응점에서 임의로 5개의 점을 추출(Sample) 한다.

  2) Essential Matrix를 추정해서 다른 점들이 해당 Essential Matrix에서 Inliers 인지를 본다.

Matrix는 Monocular 이미지 일 경우 Scale Factor를 알지 못한다(단점).

 

 OpenCV 함수(RANSAC을 이용한 Essential Matrix 추정 함수)

E, mask = findEssentialMat(points2, points1, focal, pp, RANSAC, 0.999, 1.0, mask);
//points2 : current
//points1 : past

 

5. Computing R, t from the Essential Matrix

- Essential Matrix에 SVD를 취한다. 

- $ E = U\Sigma V^{T} $
- $ [t]_{x} = VW\Sigma V^{T} $
- $ R = UW^{-1}V^{T} $

 

OpenCV 함수

- Essential Matrix로 부터 Cheirality Check를 이용하여 Camera Rotation과 Translation을 반환한다.

_, R, t, mask = recoverPose(E, points2, points1, cameraMatrix, focal, pp, mask);
// 추출된 points1, points2가 같은 intrinsic matrix를 가진다고 가정한다.

_, R, t, mask = recoverPose(E, points2, points1, R, t, focal, pp, mask);

 

위의 두 과정을 종합하면 다음과 같이 나타낼 수 있다

// Example. Estimation of fundamental matrix using the RANSAC algorithm
int point_count = 100;
vector<Point2f> points1(point_count);
vector<Point2f> points2(point_count);

// initialize the points here ...
for( int i = 0; i < point_count; i++ )
{
    points1[i] = ...;
    points2[i] = ...;
}

// cametra matrix with both focal lengths = 1, and principal point = (0, 0)
Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
Mat E, R, t, mask;

E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, mask);
recoverPose(E, points1, points2, cameraMatrix, R, t, mask);

 

 

전체 VO 알고리즘은 취득한 특징점들이 고정(Rigid)라고 가정한다. 그러나, 현실은 그렇지 못한 경우가 많다. 

 

블로그 저자 말에 의하면, Translation이 Forward 방향 외로 우세한 경우(Dynamic Backgound 영향일 수도 있으므로) 해당 동작을 무시하는 것이 낫다고 한다.

결과 예시

---

그외 참고 자료

[1] http://www.cs.toronto.edu/~urtasun/courses/CSC2541/03_odometry.pdf

[2] [Youtube] Visual Odometry

[3] [Youtube] Feature-based, Direct, and Deep Learning Methods of Visual Odometry