RSS

[review] 1-Point RANSAC for EKF Filtering. Application to Real-Time Structure from Motion and Visual Odometry

05 Jun

발표 : Journal of Field Robotics, 2010

제목 : 1-Point RANSAC for EKF Filtering. Application to Real-Time Structure from Motion and Visual Odometry

저자 : Javier Civera, Oscar G. Grasa, Andrew J. Davison and J. M. M. Montiel

소속 : Universidad de Zaragoza, Spain. (A. Davison은 imperial college).

페이퍼 : http://webdiis.unizar.es/~jcivera/papers/civera_etal_jfr10.pdf

동영상 : http://webdiis.unizar.es/~jcivera/videos/jfr10_monocular_plus_odometry.avi

소스코드 : http://webdiis.unizar.es/~jcivera/code/EKF_monoSLAM_1pRANSAC_1.01.zip
https://svn.openslam.org/data/svn/ekfmonoslam/

이 논문의 핵심은, 아니 정확히 말해서 다른 논문과의 차이점은 1-point RANSAC에 있다.  1-point RANSAC을 쓰면 수행시간이 짧아지고, (그 짧아진 수행시간에 random hypothesis를 더 만들어 낼 수 있어서인지) 정확도도 향상된다고 한다.  근데, 정확히 1-point RANSAC의 실체가 무엇인지 애매하게 써 놓고 있다.  SLAM을 업으로 살아온 애들은 이해할 수 있을 지 몰라도, 나처럼 초심자가 그 애매한 말을 머리속에서 컴퓨터 알고리듬으로 구체화하긴 힘들다.

그 애매한 말이 뭔고하니,

As the first key di erence, the starting point is a data set and its underlying model, but also a prior probability distribution over the model parameters. RANSAC random hypotheses are then generated based on this prior information and data points, di erently from standard RANSAC hypothesis solely based on data points.

여기서 말하는 “hypotheses are then generated based on this prior information and data points” 가 문제다.  이게 말이 좋지, 뭘 어떻게 prior info랑 data를 가지고 model instance를 만들라는 거냐?

RANSAC을 설명할 때, 단골 example인 line fitting을 가지고 쉽게 설명한다고 했는데, 역시 그 핵심부분, 즉 하나의 data point와 prior model parameter distribution을 가지고 하나의 line hypothesis를 만들어 내는지는 설명안하고 있다.  ㅅㅂㄴㅁ.

Clipboard01

위 그림은 일반적인 RANSAC을 이용한 line fitting이다.

Clipboard02

위 그림은 1-point RANSAC을 이용한 line fitting으로 왼쪽 처음 그림에 갈색 line이랑 점선으로 된 standard deviation 부분이 prior information을 나타내주고 있다.  이 prior line이랑 점 하나를 이용해서 (원래는 두 점으로 하는 건데) line hypothesis를 만들어 낸다고 한다.

이 부분에 해당하는 pseudo code는 아래 순서도의 line 13에 해당한다.

Clipboard03

논문에서도 역시 line 13에 해당하는 자세한 설명이 없다.  그래서 할 수 없이 소스코드를 보기로 했다.  EKF_monoSLAM_1pRANSAC_1.01.zip 에 들어 있는 matlab code를 보니,  ransac_hypothesis.m 파일에

% select random match
[zi,position,num_IC_matches] = select_random_match(features_info);

% 1-match EKF state update
x_k_km1 = get_x_k_km1(filter);
p_k_km1 = get_p_k_km1(filter);
hi = features_info(position).h’;
Hi = sparse(features_info(position).H);
S = full(Hi*p_k_km1*Hi’ + features_info(position).R);
K = p_k_km1*Hi’*inv(S);
xi = x_k_km1 + K*( zi – hi );

라는 부분이 있다.

코드에서 상황을 보면,

  • camera pose(여기서는 camera-centered라고 해서 world coordinate pose란다. 암튼) : 7차원(position 3차원, quaternion 4차원)
  • camera pose 속도 : 6차원 (position 3차원, orientation 3차원)
  • map point 1개당 : 6차원 (inverse depth representation으로 6차원, 나중에 계산해 줄 때는 Euclidean coordinate로 3차원으로 바꿈)

따라서 map point가 4개이면, 37(= 7 + 6 + 4 * 6) 개의 차원을 갖는 state vector가 형성 된다.

select_random_match()에서 random하게 index하나(여기서는 position)를 고르고, 그 index에 해당하는 measurement 2D point zi(2 by 1)를 구한다.

get_x_k_km1()에서 구한 predicted state vector, x_k_km1 는 37 by 1 vector이고, get_p_k_km1()에서 구한 predicted covariance matrix p_k_km1 은 37 by 37 matrix이다.  hi 는 2 by 1 짜리 predicted measurement point이고, Hi 는 random index에 해당하는 measurement의 Jacobian matrix로 사이즈는 2 by 37이다.  또한 R은 각 점의 measurement에 있어서의 Gaussian noise의 covariance matrix로 2 by 2이다.

따라서, 해당 index, 즉 position에 해당하는 점의 predicted covariance S는 matrix 계산을 통해 [(2 by 37) * (37 by 37) * (37 by 2) + (2 by 2)], 2 by 2 짜리 matrix가 된다.

Gain인 K는 [(37 by 37)  * (37 by 2) * (2 by 2)] matrix 곱을 통해 37 by 2 짜리 matrix가 된다.

최종적으로 prior information x_k_km1과 p_k_km1 그리고 1-point data zi를 가지고 만든 hypothesis 모델 xi는 [(37 by 1) + (37 by 2) * (2 by 1)] 을 통해 x_k_km1 와 같은 사이즈의 37 by 1 짜리 vector가 된다.

음…  옛날에 극좌표계에서 line fitting을 통해서 Kalman filter로 차선 추적하던게 생각난다.  그 때도 RANSAC을 썼었는데, 이 1-point RANSAC을 썼으면 어땠을까 하는 생각이 든다.  그럼 어떻게 될까?  모델 파라미터는 theta와 rho가 될 테고, state vector는 (theta, rho, theta_velocity, rho_velocity)의 4 by 1 vector가 될 것이다.

x_k_km1 는 역시 4 by 1 vector가 될 것이고, p_k_km1 은 4 by 4 matrix가 될 것이다.  이 둘을 가지고 (예를 들어, 95% 신뢰구간에서) line fitting에 쓸 점들을 추리고, 그 점들 중에서 random하게 zi (2D point, 2 by 1 vector)를 선택하면 hi는 2 by 1, Hi 는 2 by 4, S는 2 by 2, 그리고 K는 4 by 2 matrix가 된다.  마지막으로 xi는 4 by 1 짜리 vector로 앞의 두 element인 theta_i, rho_i 를 가지고 직선을 만들면 x_k_km1과 p_k_km1과 하나의 점 zi를 가지고 line hypothesis를 만드는 셈이 된다.  생각해 보니까 상태 x에 point 좌표 정보가 없으니, x랑 measurement h를 연결하는 함수를 만들기가 까다로워 보이긴하다.

Advertisements
 

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

 
%d bloggers like this: