[ 영상처리/번역 ] OpenCV 를 이용한 Monocular Visual Odometry

 

 본 포스팅은 Avi Sigh's blog 의 블로그 포스팅을 번역한 글입니다.

앞서의 포스팅과 마찬가지로 번역이 이상할 수 있으니 어느정도 걸러서 봐 주시는 편이 좋을 것 같습니다ㅠㅠ 틀린 부분은 지적해주시면 감사히 수정토록 하겠습니다.

 

 

 

OpenCV 를 이용한

 

Monocular Visual Odometry

 

 지난 달, 저는 Streo Visual Odometry 와 이에 대해 실제 MATLAB 에서 수행한 내용을 포스팅 (역자: 제가 번역한 페이지 링크)했었습니다. 이번 포스팅은 Monocular Visual Odometry 를 중점으로, 그리고 어떻게 이를 OpenCV/C++ 을 통해 구현할 수 있는지에 대한 내용으로 작성될 것입니다. 제가 구현한 내용들은 깃헙을 통해 자유롭게 이용하실 수 있습니다. 또한 이 포스팅은 기존 제가 이전에 Streo 방식을 통해 구현한 내용보다 더 이해하기 쉽고, 더 빠른 5fps 로 수행됩니다.

 

 만일 여러분이 Visual Odometry 가 처음이라면, 저는 첫번째로 저의 이전 포스팅에서 일부 내용(수학적인 내용이 시작되기 전 내용의 전부)을 보시는 걸 권해드립니다. 거기에 Visual Odometry 가 무엇인지, 왜 우리가 이를 필요로 하는지, 그리고 monocular 방식과 streo 방식의 접근법에 대한 비교까지 작성해 두었습니다.

 

 이미 visual odometry 에 대해 알고 계시다고요? 좋네요. 그럼 바로 가보죠.

 

 

Demo

 

 제가 실제 구현을 하기에 앞서, 알고리즘이 어떻게 수행되는지 한 번 보시죠!

 

 

멋지죠, 그렇지 않나요? 그럼 이제 OpenCV 로 이를 구현해 보도록 합시다.

 

 

해당 문제의 공식화(Formulation of the probrem)

 

Input

 우리는 어떤 카메라로부터 입력받은 gray scale 이미지 stream을 가지고 있습니다. 그리고 시간 $t$ 와 $t+1$ 에 캡쳐된 각각의 프레임을 $I^t, I^{t+1}$ 이라고 부르도록 하겠습니다. 우리는 캘리브레이션(Calibration)을 통해 얻은 모든 내부 파라미터(intrinsic parameter)에 대한 사전지식을 가지고 있고, 이는 OpenCV 로도 또한 수행될 수 있습니다.

 

Output

 모든 이미지 쌍에 대하여, 우리는 두 프레임 간에 이동하는 물체를 표현하는 회전 행렬(rotation matrix) $R$ 과 병진 벡터(translation vector) $t$ 를 구해야 할 필요가 있습니다. 벡터 $t$ 는 우리가 수행하는 monocular 방식에서 scale factor 까지만 계산될 수 있습니다.

 

 

알고리즘 개요(Algorithm Outline)

 

1, 캡쳐된 이미지: $I^t, I^{t+1}$

2. 위 이미지들의 왜곡 보정

3. $I^t$ 에서 피쳐(Feature)를 찾고, $I^{t+1}$ 까지 피쳐를 트래킹 하기 위해서 FAST 알고리즘 사용. 특정 기준값(Threshold) 이하로 피쳐의 수가 줄어들었을 경우, 새로 탐색을 시작

4. Essential Matrix 를 계산하기 위해, RANSAC 알고리즘을 이용한 Nister's 5-point 알고리즘 사용

5. 이전 단계에서 계산된 Essential Matrix 로부터 $R$ 과 $t$ 를 추정

6. (속도계 같은) 외부 소스로부터 scale 정보를 얻고, 병진 벡터와 회전 행렬을 결합(concatenate)

 

여러분께서 위에 언급한 단계들을 이해하셨을 수도, 그렇지 않을 수도 있지만, 너무 걱정하지 마세요. 위 내용의 중요한 포인트들은 아래쪽에서 훨씬 더 자세하게 설명 드릴 것입니다.

 

 

왜곡 보정(Undistortion)

 

 왜곡은 실제 세계에서의 직선이 이미지 상에서 굽어 보일 때 발생합니다. 이 단계는 렌즈의 왜곡을 보정하는 단계입니다. 왜곡 보정은 캘리브레이션 동안에 얻어진 왜곡 파라미터(distortion parameters)들을 이용하여 수행됩니다. 제가 사용하고 있는 KITTI 데이터 셋의 경우 이미 왜곡 보정이 이루어진 이미지 이기 때문에, 여기에 코드를 작성하지는 않았습니다. 그렇지만 왜곡보정을 하는 것은 OpenCV 를 이용하면 비교적 쉬운 일입니다.

 

 

피쳐 탐색(Feature Detection)

 

 저는 제가 구현했던 streo 방식과 동일하게, FAST corner detector를 이용하여 접근했습니다. 여러분이 정말로 이 과정을 이해하려면 원본 논문과 소스 코드를 보셔야 하지만, 일단 저는 짧게나마 detector 가 어떻게 동작하는지 설명할 것입니다. 코너가 맞는지 아닌지 판단하고자 하는 어떤 점 $P$ 가 있다고 가정해 보겠습니다. 그리고 이제 우리는 아래쪽 그림에서 볼 수 있듯이 이 점을 중심으로 16px 의 둘레를 가지는 원을 그립니다. 다음으로 이러한 원 둘레 위에 있는 모든 픽셀들에 대해서, 우리는 어떤 요소 $I$ 에 대해서 원래의 픽셀($P$) 을 초과하는 강도를 지닌 연속된 픽셀들의 집합이 있는지 확인하고, 최소한 동일한 요소 $I$ 에 대하여 강도가 덜한 픽셀들의 또다른 연속적 집합이 있는지 확인합니다. 만약 존재한다면, 우리는 이 지점을 코너라고 표시합니다. 코너가 아닌 대부분의 것들을 제외하기 위해서는 경험적 방법(heuristic) 이 사용되는데, 처음에 1, 9, 5, 13 번째 픽셀에 대해서 시험해보고, 코너가 되는 포인트의 $I$ 에 대해, 최소 $I$ 보다 더 높은 강도를 가지는 픽셀이 셋 이상 있는지, 혹은 더 낮은 강도를 가지는 픽셀이 셋 이상 있는지 판단하는 식입니다. 이러한 특수한 접근 방식은 SIFT 와 같이 대중적으로 사용되는 다른 point detector 들에 비해 계산 효율성이 높기 때문에 선택되었습니다.

 

 

 

FAST 피쳐 탐색 원본 논문에 실린 이미지

 

 OpenCV 를 사용하여, 피쳐 탐색을 하는 것은 매우 간단합니다. 아래쪽에 이를 구현한 코드가 있습니다.

 

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

 

 위 코드에서의 파라미터들은 KITTI 데이터 셋에 대하여 매우 잘 설정되어 있기 때문에, 해당 이미지 하나에 대하여 4000 개까지의 피쳐를 찾아냅니다. 아마도 여러분은 여러분의 데이터에 대해 최상의 퍼포먼스를 얻기 위해서 이 파라미터들을 조정하시길 원할 것입니다. 탐색된 피쳐 포인트들의 데이터 타입이 위 코드를 통해 KeyPoints 에서  Point2f 의 vector 로 변환되기 때문에, 아래쪽에 설명된 피쳐 트래킹 단계로 넘어갈 수 있다는 점을 기억하세요.

 

 

피쳐 트래킹(Feature Tracking)

 

 이전 단계에서 탐지된 FAST 코너들은 다음 단계에 KLT 트래커를 사용할 때 이용됩니다. KLT 트래커는 기본적으로 모든 코너들을 추적하며, 다음 이미지의 코너를 찾기 위해서 이러한 로컬 정보를 이용합니다. 여러분은 KLT 링크를 통해서 더 많은 정보를 얻을 수 있습니다. $I^t$ 에서 탐지된 코너는 $I^{t+1}$ 에서 추적됩니다. $I^T$ 에서 탐지된 피쳐들의 집합을 $F^t$ 라고 하겠습니다. 아래쪽은 KLT 트래커를 이용하여 OpenCV 에서 피쳐를 추적하는 함수입니다:

 

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++;
     	}
     }
}

 

피쳐 재탐색(Feature Re-Detection)

 

 KLT 트래킹을 하는 동안에, 결국에는 일부 포인트를 놓치게 됩니다(차량의 시야에서 포인트들이 벗어나 버릴 때처럼). 그래서 우리는 전체 피쳐의 수가 어떤 기준점 이하로 떨어질 때에 피쳐 재탐색을 수행합니다(제 구현의 경우에는 2000 이 기준점입니다).

 

 

Essential Matrix 추정(Essential Matrix Estimation)

 

 일단 점 대응(point-correspondences)에 대한 내용을 가지고 있고, 또한 essential matrix 의 계산을 위한 몇몇 기술들 역시 가지고 있습니다. essential matrix는 다음과 같이 정의됩니다: $y_1^T E y_2 = 0$ 여기서, $y_1, y_2$ 는 동차 정규화 이미지 좌표(homogenous normalised image coordinates)입니다. 8개의 점 대응을 필요로 하는 간단한 알고리즘이 존재하기는 하지만, 더 나은 결과를 내는 것으로 보이는 더 최근의 접근 방식은 5개의 점을 사용하는 알고리즘(5 point algorithm)입니다(Divid Nister An efficient solution to the five-point relative pose problem(2004)). Essential Matrix 는 단지 5개의 자유도를 가지고 있기 때문에, 이 방식은 많은 수의 비선형 방적식들을 풀 수 있고, 최소의 가능한 점들을 요구합니다.

 

RANSAC

 

 만약 우리의 모든 점 대응이 완벽하다면, 우리는 움직임을 정확하게 추측하는 데에 이어지는 두 프레임 사이의 피쳐 대응 5개만 있으면 됩니다. 그러나, 피쳐 트래킹 알고리즘은 완펵하지 않고, 그렇기 때문에 우리는 몇몇 잘못된 대응을 얻개 됩니다. 모델 추측을 할 때, 이런 outlier 를 제어하는 일반적인 기술이 RANSAC 입니다. RANSAC 은 반복적인 알고리즘입니다. 매 반복에서, 알고리즘을 통해 대응 세트 중에 5개의 점을 임의로 뽑고, Essential Matrix 를 추정합니다. 그리고, 이 Essential Matrix 를 사용할 때 다른 점들이 inlier 인지 확인합니다. 이 알고리즘은 정해진 반복을 수행한 후 종료되며, 포인트의 수가 최대가 되는 경우의 Essential Matrix 를 사용합니다.

 

 위의 내용을 OpenCV 를 통해서 하는 것은 매우 쉬운 일이며, 여러분 모두 아래쪽 라인을 작성해야 할 것입니다.

E = findEssentialMat(points2, points1, focal, pp, RANSAC, 0.999, 1.0, mask);

 

 

Essential Matrix 로부터 R, t 를 계산

 

 앞서 언급드렸던 Essential Matrix(consistent) 의 정의 외에 또다른 정의는 다음과 같습니다: $E=R[t]_x$ 여기서, $R$ 은 회전 행렬이며, $[t]_x$ 는 $t$ 와의 외적을 행렬로 표현한 것입니다. 이제 Essential Matrix 에서 SVD 를 취하고, 회전 행렬에서의 제약조건을 활용하면, 우리는 다음과 같은 수식을 얻을 수 있습니다:

$$E=U\sum V^T$$

$$[t]_x=VW\sum V^T$$

$$R=UW^{-1}V^T$$

 아래쪽이 OpenCV 로 이를 구현한 한 줄 입니다.

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

 

궤적(Trajectory) 구하기

 

 카메라의 위치가 $R_{pos}, t_{pos}$ 로 표현된다고 하겠습니다. 그러면 우리는 아래의 방정식을 통해서 궤적을 추적할 수 있습니다:

$$R_{pos}=RR_{pos}$$

$$t_{pos}=t_{pos}+tR_{pos}$$

 결합(concatenating) 전에 다른 소스로부터 병진 벡터 $t$ 의 scale 정보를 얻어야 한다는 것을 기억하세요. 제가 구현한 내용에서는 이 정보를 KITTI 데이터 셋에서 제공되는 Ground Truth 로부터 얻습니다.

 

 

경험적 추론(Heuristics)

 

 대부분의 컴퓨터 비전 알고리즘들은 일부 경험적 추론 없이는 완전하지 않으며, Visual Odometry 역시도 예외는 아닙니다. 우리가 사용할 경험적 내용들은 아래쪽에 설명되어 있습니다:

 

도미넌트 모션(Dominant Motion)이 정면이다

 

 Visual Odometry 알고리즘은 모든 부분에서, 수행 환경 안에서, 대부분의 점들이 강체(rigid) 라는 가정 하에 작성됩니다. 그러나 우리가 타고 있는 차량이 계속 서 있고, 버스는 (예를 들어 교차로를) 지나고 있다고 하면, 물리적으로 불가능한 길로 차량이 움직이는 것처럼 보이게 될 수도 있습니다. 결론부터 말하자면, 우리가 정면 방향으로 변환한 것이 더 우세하다는 것을 알게 되면, 이러한 움직임을 간단히 무시하게 됩니다.

 

 

결론

 

 결국 KITTI 데이터 셋을 이용할 경우 알고리즘의 퍼포먼스가 얼마나 좋을까요? 한번 보세요.

 

 

 

2000 프레임 동안에 계산된 궤적(Trajectory) vs Ground Truth

 

 

다음은?

 

 제가 구현한 내용의 한계는 상대적인 scale 을 계산할 수가 없다는 점입니다. 저는 어떤 방법으로 구현을 시도해 보았습니다만, 저는 "scale drift" 라고 알려진 문제(작은 에러들이 쌓이는 것)에 직면했고, Odometry 추정 결과는 그다지 좋지 못했습니다. 저는 제가 조만간 더 완벽한 상대적 scale 을 계산하는 pipeline 을 구현하고, 이에 관해서 포스팅을 작성했으면 좋겠습니다.