AsynEIO: Asynchronous Monocular Event-Inertial Odometry Using Gaussian Process Regression

Define the transformation of the rigid body frame $b$ with respect to (w.r.t) the fixed inertial frame $i$ as a matrix $\bm{T}_{ib}$, where the pose $\bm{T}_{ib}$ forms the special Euclidean group, denoted $SE(3)$. Suppose the WNOJ prior to the kinematics of $\bm{T}_{ib}$ and apply the right multiply update of $SE(3)$. We have the differential equation as follows:

where

is the generalized velocity, $\bm{s}$ is the generalized acceleration, and $\bm{Q}_{c}$ is the power spectral density matrix of the GP. Given the detailed rotation and translation components as

where $(\bm{\omega}^{bi}_{b})^{\land}$ maps the angular velocity vector to a skew-symmetric matrix. Then, we have $\dot{\bm{r}}_{i}^{bi}=\bm{C}_{ib}\bm{\nu}_{b}^{bi}$. Comparing the subscripts, $\dot{\bm{r}}_{i}^{bi}$ can be defined as $\bm{\nu}_{i}^{bi}$, which represents the linear velocity of body frame w.r.t the inertial frame observed and expressed in the inertial frame. In fact, the generalized linear velocity $\bm{\nu}_{b}^{bi}$ indicates the same linear velocity observed in the inertial frame but expressed in the body frame. Notice that $\bm{\nu}_{b}^{bi}\neq\dot{\bm{r}}_{b}^{bi}$, where $\dot{\bm{r}}_{b}^{bi}$ represents the linear velocity of body frame w.r.t the inertial frame observed and expressed in the body frame. Thereby, the generalized acceleration is given by

where $\dot{\bm{C}}_{bi}=\bm{C}_{bi}(\bm{\omega}_{i}^{ib})^{\land}$. We know that $(\bm{C}\bm{\omega})^{\land}\equiv\bm{C}(\bm{\omega})^{\land}\bm{C}^{\top}$ and $\bm{\omega}_{b}^{bi}=-\bm{C}_{bi}\bm{\omega}_{i}^{ib}$. Substituting them into (4), we can show

where $\ddot{\bm{r}}_{i}^{bi}$ represents the linear acceleration of body frame w.r.t the inertial frame observed and expressed in the inertial frame, and $\bm{\omega}_{b}^{bi}$ is the angular velocity of body frame w.r.t the inertial frame observed in the body frame. The kinematic state can be defined as

where the sub- and superscripts have been dropped and (III-A)-(5) describe the stochastic differential equations (SDEs) of kinematics. Since $\bm{x}(t)\in SE(3)\times\mathbb{R}^{12}$ forms a manifold, these nonlinear SDEs are difficult to directly deploy in continuous-time estimation.

To linearize it, the local variable $\bm{\xi}$ in the tangent space of $\bm{T}_{k}$ can be given by

where $\bm{T}_{k}\doteq\bm{T}(t_{k})$, and $\exp(\bullet)$ is the exponential map from $\mathfrak{se}(3)$ to $SE(3)$. The time derivative of (7) indicates the relationship between local variable and kinematic state as

where $\bm{\mathcal{J}}(\bm{\xi}_{k}(t))$ is the right Jacobian of $\bm{\xi}_{k}(t)$. Notice that $\bm{\mathcal{J}}\approx\bm{1}^{6\times 6}$ when $\bm{\xi}_{k}(t)$ is small enough. Further take the time derivative of (8), we can show the relationship between $\bm{\varpi}(t)$ and $\ddot{\bm{\xi}}_{k}(t)$:

Apply the first-order approximation and substitute (8), we have

where $(\bullet)^{\curlywedge}$ is the adjoint matrix operator. For our definition, we have

where $\bm{\phi}\in\mathbb{R}^{3}$ is the rotation component and $\bm{\rho}\in\mathbb{R}^{3}$ is the translation component. Now, we can define the local linear kinematic state w.r.t $\bm{T}_{k}$ as

Similarly, we can assume the motion prior to be

On this basis, the linear time-invariant SDEs are

which has a closed-form solution for the state transition function $\bm{\Phi}(t,t_{k})=\exp(\bm{A}(t-t_{k}))$. Meanwhile, the transition equation of the covariance matrix can be given by

Under the WNOJ assumption, the GP prior residual between timestamp $t_{k}$ and $t_{k+1}$ can be defined as

in which $\Delta t_{k}=t_{k+1}-k_{k}$, $\bm{\xi}_{k,k+1}=\log(\bm{T}_{k}^{-1}\bm{T}_{k+1})^{\vee}$, $\bm{\mathcal{J}}_{k,k+1}=\bm{\mathcal{J}}(\bm{\xi}_{k,k+1})$, and $\bm{\varpi}_{k}=\bm{\varpi}(t_{k})$. Notice that $\log(\bullet)$ and $(\bullet)^{\vee}$ are the inverse maps of $\exp(\bullet)$ and $(\bullet)^{\land}$, respectively. We can compute the covariance matrix for prior residual from (15). Furthermore, the detailed Jacobians of prior residuals are listed in Appendix-C.

Generally, the measurement of the gyroscope is the noisy angular velocity $\tilde{\bm{\omega}}_{b}^{bi}=\bm{\omega}_{b}^{bi}+\mathbf{b}_{g}+\bm{\varsigma}_{g}$, and the measurement of the accelerometer $\tilde{\bm{a}}$ is the noisy value

where $\bm{\varsigma}_{g}\sim\mathcal{N}(\bm{0},\bm{Q}_{g})$ and $\bm{\varsigma}_{a}\sim\mathcal{N}(\bm{0},\bm{Q}_{a})$ are the Gaussian measurement noises, $\bm{g}_{i}$ is the gravitational acceleration, $\mathbf{b}_{g}$ and $\mathbf{b}_{a}$ are biases that obey the Brownian movement.

where $Q_{a},Q_{g},Q_{bg},Q_{ba}$ are covariance matrices.

Before describing the GPIF, we need first know the kinematic state at an arbitrary timestamp. To query the trajectory at timestamp $\tau$ between $t_{k}$ and $t_{k+1}$, we firstly interpolate the local variable $\bm{\gamma}_{k}(\tau)$ with

Then, in conjunction with (7)-(12), the kinematic state $\bm{x}(\tau)$ in (6) can be resolved.

With the GP-based continuous-time trajectory, we construct the GPIF directly from the angular velocity and linear acceleration states, which avoids the preintegration. Considering (2), (5) and (17), the residual of GPIF can be given by

where the sub- and superscripts have been dropped. Neglecting the effects of bias and gravity, we can see that the raw acceleration measurement $\tilde{\bm{a}}$ has a direct relationship with the kinematic state $\dot{\bm{\nu}}$ and an extra term $\bm{\omega}^{\land}\bm{\nu}$. Notably, when the angular velocity or linear velocity is very small, or when the axis of rotation is approximately aligned with the direction of the linear velocity, this extra term can be neglected. In other cases, this extra term should be taken into account. Up to this point, we have established a detailed residual model between the raw IMU measurements and the kinematic states.

The covariance matrix of the direct residual is exactly the measurement covariance $diag(\bm{Q}_{g},\bm{Q}_{a})$. The jacobians of GPIF residuals can be derived from the chain rule as follows:

where the derivations of $\frac{\partial\bm{x}(\tau)}{\partial\bm{x}(t_{k})}$ can be found in the Appendix-D. Here, the nonzero components of $\frac{\partial\bm{e}(\tau)}{\partial\bm{x}(\tau)}$ are listed as follows:

where $\bm{b}_{a}(\tau)$ and $\bm{b}_{g}(\tau)$ are calculated with linear interpolation. In addition, a bias prior factor is attached between each pair of sampling timestamps $t_{k}$ and $t_{k+1}$, as follows:

Concretely, the GPP models the “local accelerations” $\bm{a}_{k}(t)\in\mathbb{R}^{3}$ and the “local angular velocities” $\dot{\bm{\phi}}_{k}(t)\in\mathbb{R}^{3}$ as six independent GPs. Therefore, we have

where $\dot{\bm{\phi}}_{k}(t)\in\mathbb{R}^{3}$ is the time derivative of $\bm{\phi}_{k}(t)$, and $k_{\phi}(t,t_{k}),k_{a}(t,t_{k})$ are kernel functions of GPs, and $\bm{I}_{3}\in\mathbb{R}^{3\times 3}$ is an identity matrix. The local variables are defined as

in which $\bm{C}(t)$ is the rotation matrix of the body frame with respect to the inertial frame at the timestamp $t$, and $\bm{a}(t)$ is so called the proper acceleration of the body frame. From the definition of (17), we know that $\bm{a}(t)=\bm{C}_{bi}(\ddot{\bm{r}}_{i}^{bi}-\bm{g}_{i})$. As shown in (25) and (26), the local accelerations and angular velocities are expressed in a local body frame at the timestamp $t_{k}$. On this basis, these variables can be integrated linearly.

To permit efficient analytical preintegration, the GPP introduces a series of the latent states as the induced pseudo-observations and conducts an online optimization problem to infer them from inertial measurements. With the linear operator [45] defined on corresponding latent states, it can effectively execute the preintegration for given timestamps.

Suppose the latent states $\hat{\bm{\rho}}_{j}$ and $\hat{\bm{\alpha}}_{j}$ be noisy observed vectors of local variables at the timestamp $t_{j}$ as in (25), we have

where $\bm{\eta}$ follows the zero-mean Gaussian distribution with a diagonal covariance matrix. By conditioning upon these latent states, the posterior values of local variables in (25) can be expressed as

where $\bm{t}=[t_{1},t_{2},\cdots,t_{N}]$ is timestamp vector of desired latent states, $\bm{K}(\bm{t},\bm{t})=k(\bm{t},\bm{t})$ is $N\times N$, $\bm{k}(\bm{t},\tau)=k(\bm{t},\tau)$ is $N\times 1$, $\tilde{\bm{\rho}}=[\tilde{\bm{\rho}}_{1},\tilde{\bm{\rho}}_{2},\cdots,\tilde{\bm{\rho}}_{N}]$ is $3\times N$, and $\tilde{\bm{\alpha}}=[\tilde{\bm{\alpha}}_{1},\tilde{\bm{\alpha}}_{2},\cdots,\tilde{\bm{\alpha}}_{N}]$ is $3\times N$. According to [38], the latent states can be recovered by means of constructing several optimization problems where the latent states $\tilde{\bm{\rho}}$ and $\tilde{\bm{\alpha}}$ in (28) are optimized to predict a bunch of true IMU measurements as accurate as possible. After that, the preintegrated velocity $\Delta\tilde{\bm{\nu}}_{k}(\tau)$, position $\Delta\tilde{\bm{r}}_{k}(\tau)$ and rotation vector increment $\Delta\tilde{\bm{\phi}}_{k}(\tau)$ can be written as:

where $\tau>t_{k}$ is the queried timestamp. From (28), we know that the related variables of integration operations in (29) are only $\bm{k}_{\phi}(\bm{t},\tau)$ and $\bm{k}_{a}(\bm{t},\tau)$. Indeed, the integration on latent states can be calculated analytically when $\bm{k}_{\phi}(\bm{t},\tau)$ and $\bm{k}_{a}(\bm{t},\tau)$ are given. The efficient analytical preintegration offers an ability for inferring on asynchronous measurements and continuous-time trajectories. The preintegration for the rotation matrix can be calculated with (26) as $\Delta\tilde{\bm{C}}_{k}(\tau)=\exp(\Delta\tilde{\bm{\phi}}_{k}(\tau)^{\land})$. Also, it is convenient to recover the kinematic state $\bm{x}(\tau)$ by means of composing the previous state $\bm{x}(t_{k})$ with (29). With these composed states, the asynchronous visual projection can be done on arbitrary timestamps.

According to the inertial preintegration method, the residual terms for GPP are built with the integration increments in (29) as follows:

where we have $\bm{r}_{k}=\bm{r}_{i}^{bi}(t_{k})$, and $\bm{\nu}_{k}=\bm{\nu}_{b}^{bi}(t_{k})$, and $\Delta\tilde{\bm{\nu}}_{k,k+1}=\Delta\tilde{\bm{\nu}}_{k}(t_{k+1})$, and $\bm{C}_{k}=\bm{C}_{ib}(t_{k})$, and $\bm{C}_{k,k+1}=\bm{C}_{k}^{\top}\bm{C}_{k+1}$, and so on. All notations and frames of (30) follow the definitions in Sec. III.

To realize high-temporal resolution, we design an asynchronous event-driven frontend where sparse features are detected and tracked in an event-by-event manner, as depicted in Fig. 3. Although separate event-driven feature detecting and tracking methods have been proposed in previous literature, a whole event-driven frontend needs further design on strategies of screening and maintenance. Concretely, we leverage a registration table to record the unique identifier of each active event feature. The pixel location of each event feature can be infer from its index in the registration table. Then, the unique identifier is mapped to the corresponding feature trajectory and its independent tracker. When a event measurement is obtained, we first search in the registration table to determine if its neighbor region already exists activate event features. If there is at least one active event feature, we utilize the identifier-tracker map to find the corresponding tracker and feed the event measurement to it. The movement of active feature will be reported by the tracker and collected into the feature trajectory. Otherwise, we update the SAE and determine if a FA-Harris [47] corner feature can be detected at the pixel location of this event measurement.

Since the registration table has the same width and height as the resolution of event cameras, it is high-efficient to access and modify elements in the registration table. The search of existing features only pays attention to a small patch within the registration table, and thus has a low time complexity. The small patch also reject the feature clustering and realize sparse features. Similarly, the detection only occurs at the location of the current event measurement and is thus efficient.

As the tracking of active features is triggered by subsequent events, a special case is that an active feature have not be triggered for a long time. For example, a corner feature is detected in noisy events by mistake, and thus it should be discarded promptly. To realize it, we assume the active feature should be triggered frequently within a maximum time threshold. We periodically delete the active features that have not been updated for more than the maximum time threshold. In addition, we also set a minimum time threshold to avoid the feature trajectory collecting a mass of redundant measurements. With patch search and time threshold mechanisms, the result frontend can obtain sparse and asynchronous feature tracking.

The feature trajectory, uniquely indexed by $i$, can be defined as

which represents an ordered spatial-temporal association set. The $\bm{m}_{j}^{i}$ indicates the $j$-th event feature measurement of the landmark $\bm{l}_{i}$ at timestamp $t_{i,j}$, and it contains

where $t_{i,j}$ is the measurement timestamp and $\tilde{\bm{q}}_{i,j}=[x_{i,j},y_{i,j}]^{\top}$ is the observed pixel position.

When a feature trajectory has collected enough measurements for triangulation, the induced projection will be utilized to triangulate and embed these measurements into the underlying factor graph. Given the triangulated landmark $\bm{l}_{i}=[x_{i},y_{i},z_{i}]^{\top}$ and the event feature measurement $\bm{m}_{j}^{i}$ as defined in (32), the projection model can be given by

where $\bm{K}$ indicates the intrinsic matrix, $d_{i}$ is the depth of this landmark in current camera frame, $\bm{T}_{bc}$ is the extrinsic parameter of the event camera w.r.t the IMU, which can be solved by calibration. The kinematic state $\bm{T}(t_{i,j})$ can be queried using (7) and (IV-B).

Different from the interpolated projection factor, the induced projection factor has reshaped the continuous-time trajectory using current accessible inertial measurements. Intuitively, it is more credible to build visual projection with this inertial induced factor. This concept is similar to the induce GP in the latent state preintegration method, while we leverage the proposed direct inertial factor to accomplish it. In contrast, the discrete preintegration scheme produces a unique pseudo-observation until the next sampling state. As a result, the asynchronous visual projection factors between inter-sampling states can only be interpolated under the motion prior assumption, which may sometimes lead to overly approximate results. Generally, both the direct factor and latent state preintegration can enhance the asynchronous projection of our event-driven VIO.

The inverse depth parametrization [48] is adopted to enhance the numerical stability and realize undelayed initialization. Different from the filter-based method in [48], we leverage this parametrization to optimize the GP-based factor graph and derive the analytical Jacobians. Furthermore, we define the directional vector in local frames rather than global frames, which is compatible with the optimization method. The (33) can be simplified as

We use the inverse depth $\rho$, the directional vector $\bm{\kappa}$ in the unit sphere of the first camera pose $\bm{T}_{1}$ observing this feature trajectory $\bm{F}_{i}$ to represent the landmark $\bm{l}_{i}$. As displayed in Fig. 5, the latter camera pose $\bm{T}_{2}$ should observe the ray $\bm{h}_{2}$ and have a reprojected pixel location $\bm{q}_{2}$. When the poses and landmark are estimated precisely, this reprojected pixel location should coincide with the visual tracking result $\tilde{\bm{q}}_{2}$ at the same camera pose. In fact, the direction of $\bm{h}_{2}$ can uniquely determine the $\bm{q}_{2}$. From the geometric constraints in Fig. 5, the projected directional vector can be given by

where

Therefore, the projection function can be written as

where

It means the infinity landmark or the pure rotation when the inverse depth $\rho$ is $0$ for all time. In both situations, the pose and inverse depth can be optimized without ambiguity. Therefore, this parametrization permits immediate initialization of feature trajectories. The analytical Jacobians can be derived using the chain rule, as detailed in Appendix-E. Although inverse depth has a better numerical performance in optimization, it introduces more state variables (6-DoF first observed pose) in each projection factor. When the first observed pose occurs in inter-samples of the GP trajectory, it will increase further due to the GP interpolation (Two adjacent trajectory points with 36 dimensions). To alleviate this problem, we interpolate on the feature trajectory to find the closest sampling timestamp on the motion trajectory. The interpolated pixel position is then used to compute $\bm{\kappa}$. This process initializes each feature trajectory for inverse depth optimization and reduces 30 dimensions for each projection factor. Benefited from high-time resolution of visual frontend, the interpolation on feature trajectories normally has negligible deviations.

The primary advancement of GPIF is that it avoids the integral operations on raw measurements. For the micro-electro-mechanical systems (MEMS) IMU, these integral operations would introduce collective integral errors and deteriorate the estimation system after a fewer second. Instead, we establish the detailed relationship between the differential SE(3) states of the kinematic trajectory and the physical measurements obtained from the IMU. We find that the residual, Jacobians and covariance matrix for GPIF are elegant and suitable to be embedded into the GP-based factor graph. The overall cost for our event-driven VIO with GPIF is given by

where $\bm{R}$ is the measurement covariance matrix of the event camera. We can see that there is only measurement covariance required by (VI-D) if the direct inertial factor is applied. Compared with preintegration schemes, we reduce the computational consumption paid for the covariance propagation. Also, this propagation is unreliable when the inertial measurements are partly lost or even asynchronous.

We design an initialization scheme (as shown in Fig. 7) to warm up our event-inertial system. The asynchronous continuous-time initialization is first converted to a synchronous discrete-time problem using a sampling strategy. A standard visual-inertial initialization method [49] is adopted to achieve initial estimating of pose, linear velocity and bias. With these known states, the remaining unknown states on the continuous-time GP trajectory are solved by a warm up optimization as

where $\bm{\omega}_{0}$ and $\dot{\bm{\varpi}}_{0}$ are the rotational velocity and generalized acceleration of the first trajectory point, $\lambda_{1}$ and $\lambda_{2}$ are two weight coefficients. This procedure adds some zero priors and limits the drift of GP trajectory in null space. Finally, a joint optimization as (VI-D) is built to add all asynchronous measurements and refine overall state variables.

where $\bm{C}=\exp(\bm{\phi}^{\land})$, $\bm{J}=\bm{J}(\bm{\phi})$ is the right Jacobian matrix.