카메라와 Range 센서 기반 Optical Flow로 위치 추정하는 방법

1. optical flow 정의 

wiki pedia는 Optical flow를 패턴이라고 정의한다. t1에서 찍은 사진과 t2에서 찍은 사진이 있다고 하자. t1에서 밝기를 갖는 임의의 patch가 t2에서 어디로 옮겨 갔는지에 대한 패턴이다. 개인적으로 "패턴"이란 단어는 모호하다. 이미지 위에 정의 된 2D vector field 라고 정의 하는 것이 타당하다. 하나의 벡터는 motion vector라고 불리며, 해당 모션 벡터들이 모여 Field(장)을 이룬다. vector field는 시계열 이미지의 공간적, 시간적 locality에 의해 계산 된다. 

 

https://viso.ai/deep-learning/optical-flow/

2. 구현 예시

다양한 구현 방법이 있다. 우선 어떤 Feature를 기반으로 하느냐에 따라 다르다. 위와 같이 이미지 전체에서 몇개의 grid를 나눠서 진행하는 경우도 있고, Robust한 Feature (ORB, SIFT, HoG ...)를 뽑아서 tracking 할 수도 있다. 다양한 크기의 Feature를 뽑기 위해선, image pyramid를 사용할 수 있고, t1에서의 patch가 t2에서 어디로 이동했는지 Sliding window로 하나씩 비교할 수도 있다. 

https://gaussian37.github.io/vision-concept-optical_flow/ 에 자세한 구현이 나와있다. 특히, LK optical flow를 구하는 과정이 인상 깊다. Hariss corner를 구하는 것과 같이, 임의의 kernal(window) 를 기준으로 한다. t1에서 t2로 가는 동안 kernel의 모든 픽셀이 같은 motion vector를 공유한다는 가정을 하면, 쉽게 해당 지점의 motion vector를 구할 수 있다.

 

2.1 구현 프로젝트 (mav ROS)

pixhawk라는 오픈소스 드론 hw이다. 여기서 보내는 msg가 OpticalFlow를 쓴다. mavlink optical flow 의 구현은 두가지 방식으로 이뤄진다. https://github.com/PX4/PX4-OpticalFlow를 보면 Opencv기반 구현과 SIMD 기반 구현이 있다. 실제 드론에선 SIMD 기반으로 구현한다.

 

3. pose estimation 원리 

대부분의 고전적 vision 알고리즘은 둘 중 하나의 용도로 쓰인다. pose estimation, 3d reconstruction. 그 중 pose estimation에 관해 서술한다. [mavlink optical flow]는 드론에서 아래를 보고 있는 카메라와 카메라로부터 지면의 거리를 아는 센서가 있을 때, x y평면에서 얼만큼 움직였는지에 대한 알고리즘이 있다. 이미지의 OpticalFlow를 안다면 Drone이 x,y평면에서 어떻게 이동했는지 추정할 수 있다.

원래는 Image Plane이 Optcial Center위에 존재해야 한다. 그러나 닮음이기 때문에 저렇게 그려도 된다.

Δx = h · Δu / f_x        Δy = h · Δv / f_y
v_x = h · (Δu/Δt) / f_x  v_y = h · (Δv/Δt) / f_y

삼각형 닮음을 이용해, 위와 같이 x,y변화를 추정할 수 있다.

 

3.1 tangent 근사화 

tan(θ) ≈ θ 소각도 근사로 비선형 투영 관계가 선형화된다. θ < 15°에서 오차 2.3% 이내로 사용 안전. FOV가 넓은 광각 렌즈(60° 이상)는 exact tan() 또는 렌즈 왜곡 보정이 필요하다.

 

3.2 gyro 센서로 egeo 기울임 보정 

드론 회전이 "가짜 optical flow"를 만들므로, integrated_xgyro/ygyro로 rotational flow를 제거한 뒤 translational flow만 속도 계산에 사용한다. EKF2는 IMU(Predict, ~200Hz) + Optical Flow + Sonar를 융합해 위치·속도·자세·자이로 바이어스를 동시에 최적 추정한다.