일단 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를 포함하게 된다.
A. Propagation
- 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)의 이용을 가능하게 한다.
B. On-Manifold Update
- 다음의 비선형 측정 함수(nonlinear measurement function)를 고려한다.
- 이 때, 우리는 측정 노이즈를 다음과 같이 갖는다.
- 일반적인 EKF 갱신(update)을 위해 현재 상태 추정(current state estimate)에서 위의 방정식을 선형화한다.
- 본 논문에서 사용하는 indirect EKF[26]에서는, 식 (10)을 아래 수식과 같은 현재 zero-mean error state를 이용해 선형화 한다.
- 이 때, measurement Jacobian H_k는 아래와 같이 계산된다.
- 이러한 선형화된 측정 모델(linearized measurement model)을 사용하여, 갱신된 상태(updated states)가 on-manifold를 유지하도록 하면서 아래와 같이 standard EKF update를 수행할 수 있다.
3. OPENVINS RESEARCH PLATFORM
C. Landmark 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)
반응형