[Paper] OpenVINS: A Research Platform for Visual-Inertial Estimation

Information/Paper

페트론 2020. 11. 30. 23:41

일단 ON-MANIFOLD MODULAR EKF 부분만 확인합니다.

2. ON-MANIFOLD MODULAR EKF

본 논문의 visual-inertial system의 상태 벡터(state vector) 구성은 현재 inertial navigation state, historical IMU pose clones c의 세트, environmental landmarks m의 세트, camera의 외부(extrinsic), 내부(intrinsic) 파라미터 w의 세트가 된다.

이 때, IkG_-q는 시간 k에서 global frame {G}에서 IMU local frame {I_k}까지의 회전 R(IkG_-q)=IkG_R의 파라미터가 되며, b_w, b_a는 gyroscope, accelerometer biases가 되고, G_v_Ik, G_p_Ik는 각각 global frame에서 IMU의 속도와 위치가 된다.
inertial state X_I는 단위 쿼터니언(unit quaternions) H와 벡터 공간(vector space) R_12의 곱(i.e. M=H X R_12)에 의해 정의되는 manifold가 되며, 총 15개의 자유도(degrees of freedom, DOF)를 갖는다.
벡터 변수(vector variables)에서, "boxplus"와 "boxminus" 연산자는 주어진 manifold 벡터로부터 각 요소의 단순 덧셈, 뺄셈 연산과 같다[27].
quaternions의 경우, quaternion boxpluse 연산을 다음과 같이 정의한다.

비록 본 논문에서 회전(orientations)을 left quaternion error를 사용해서 정의하였지만, 실제에서는 이에 그치지 않으며, 어떤 on-manifold 표현도 사용 가능하다는 것에 유의한다(e.g.,[28]).
enviromental landmarks X_M은 오로지 단순성을 위해 global 3D positions을 포함하게 되며, 실제에서는 다른 표현들도 사용될 수 있다. (e.g. inverse MSCKF[25], full inverse depth[29], and anchored 3D position[30]).
calibration vector X_W는 focal length, camera center, distortion parameters로 구성되어 있는 camera intrinsics ζ와 IMU에서 각 카메라로의 공간변환(spatial transformation, relative pose)를 의미하는 camera-IMU extrinsics로 구성되어 있다.
본 논문에서는 camera clocks의 동기화(synchronize)를 고려하기 때문에, calibration vector에 IMU와 camera clock간의 single time offset C_t_I를 포함하게 된다.

Inertial state X_I는 timestep k-1으로부터 k까지의 상태(state)를 propagating 하는 일반적인 비선형(nonlinear) IMU kinematics를 기반으로 하며, 선형 가속도(linear accelerations) I_a_m과 회전 속도(angular velocities) I_w_m의 IMU measurements 입력을 사용하여 propagate 된다[31].

이 때, n은 random walk bias noise와 함께 IMU measurements의 zero-mean white Gaussian noise를 포함하게 된다.
이러한 상태 추정(state estimate)는 현재 추정된 정보에 의해 평가받게 된다.

이 때, ˆ· 는 추정된 값(estimated value)이며, 하부기호(subscript) k|k-1은 k-1까지의 측정(measurements)이 주어졌을 때 k에서의 예측된 추정(predicted estimate)이 된다.
상태 공분산 행렬(state covariance matrix)은 일반적으로 현재 추정(current estimate)에서의 비선형 모델(nonlinear model)을 선형화(linearizing)함으로써 propagated 된다.

이 때, Φ_k−1 와 Q_k-1은 각각 system Jacobian과 discrete noise covariance matricse가 된다[25].
clones X_C와 environmental features X_M, calibration X_W 상태(states)들은 시간과 관련이 없다. 그러므로 연관된 state Jacobian은 zero propagation noise이며, 연산량 감소를 위한 희소성(sparsity)의 이용을 가능하게 한다.

일반적인 EKF 갱신(update)을 위해 현재 상태 추정(current state estimate)에서 위의 방정식을 선형화한다.
본 논문에서 사용하는 indirect EKF[26]에서는, 식 (10)을 아래 수식과 같은 현재 zero-mean error state를 이용해 선형화 한다.

이러한 선형화된 측정 모델(linearized measurement model)을 사용하여, 갱신된 상태(updated states)가 on-manifold를 유지하도록 하면서 아래와 같이 standard EKF update를 수행할 수 있다.

본 논문에서는 3D position과 inverse depth와 같은 다른 feature parameterizations을 모두 아우르기 위해 일련의 중첩함수(series of nested functions)로써의 landmark measurement model을 만들게 된다.
visual feature가 stochastic clones의 sliding window에서 추적(tracked)된다고 가정하면, visual-bearing measurements(예를 들면 픽셀 좌표)를 다음과 같은 일련의 중첩함수(series of nested functions)로 쓸 수 있다.

이 때, z_m,k는 raw uv pixel coordinate이고, n_m,k는 raw pixel noise로써 통상적으로 zero-mean white Gaussian으로 가정한다. z_n,k는 normalized undistorted uv measurement이고, Ck_p_f는 현재 카메라 프레임의 landmark position, G_p_f는 Global frame에서의 landmark position인데 표현방식에 따라 state elements의 함수(function)이 될 수도 있다. {CkG_R,G_p_Ck}는 global frame에서의 현재 카메라 포즈(위치와 회전)를 나타낸다.
measurement functions h_d, h_p, h_t는 각각 intrinsic distortion, projection, transformation functions과 관련되고, 관련된 measurement Jacobians은 simple chain rule에 의해 계산될 수 있다.
어떠한 카메라 모델이든 지원할 수 있도록 raw uv picels의 erorrs를 계산하여 camera intrinsics ζ의 calibration과 함수 h_d가 바뀔수 있도록 만든 것에 유의한다.(e.g., radial-tangential and equidistant)