ov_core: Delayed Feature Initialization

We describe a method of delayed initialization of a 3D point feature as in Visual-Inertial Odometry on Resource-Constrained Systems [Li2014THESIS]. It is important to note that this is just an example of how to leverage delayed initialization which is very pertinent to visual-inertial state estimation. In practice, as long as sufficient measurements are available to constrain the new state (e.g. ENU-to-VIO transform, sensor calibration, object pose, etc..), the below delayed initialization procedure can be applied directly.

Specifically, given a set of measurements involving the state $\mathbf{x}$ and a new feature $\mathbf{f}$ , we want to optimally and efficiently initialize the feature.

$\begin{align*} \mathbf{z}_i = \mathbf{h}_i\left(\mathbf{x}, \mathbf{f}\right) + \mathbf{n}_i \end{align*}$

In general, we collect more than the minimum number of measurements at different times needed for initialization (i.e. delayed). For example, although in principle we need two monocular images to initialize a 3D point feature, we often collect more than two images in order to obtain better initialization. To process all collected measurements, we stack them and perform linearization around some linearization points (estimates) denoted by $\hat{\mathbf x}$ and $\hat{\mathbf f}$ :

$\begin{align*} \mathbf{z} &= \begin{bmatrix} \mathbf{z}_1 \\ \mathbf{z}_2 \\ \vdots \\ \mathbf{z}_m \end{bmatrix} = \mathbf{h}\left(\mathbf{x}, \mathbf{f}\right)+ \mathbf{n}\\ \Rightarrow~~ \mathbf{r} &= \mathbf{z}-\mathbf{h}(\hat{\mathbf{x}}, \hat{f}) = \mathbf{H}_x \tilde{\mathbf{x}} +\mathbf{H}_f \tilde{\mathbf{f}} + \mathbf{n} \end{align*}$

To efficiently compute the resulting augmented covariance matrix, we perform Givens rotations to zero-out rows in $\mathbf{H}_f$ with indices larger than the dimension of $\tilde{\mathbf{f}}$ , and apply the same Givens rotations to $\mathbf{H}_x$ and $\mathbf{r}$ . As a result of this operation, we have the following linear system:

$\begin{align*} \begin{bmatrix} \mathbf{r}_1 \\ \mathbf{r}_2 \end{bmatrix} = \begin{bmatrix} \mathbf{H}_{x1} \\ \mathbf{H}_{x2} \end{bmatrix} \tilde{\mathbf{x}} +\begin{bmatrix} \mathbf{H}_{f1} \\ \mathbf{0} \end{bmatrix} \tilde{\mathbf{f}} + \begin{bmatrix} \mathbf{n}_1 \\ \mathbf{n}_2 \end{bmatrix} \end{align*}$

Note that the bottom system essentially is corresponding to the nullspace projection as in the MSCKF update and $\mathbf{H}_{f1}$ is generally invertible. Note also that we assume the measurement noise is isotropic; otherwise, we should first perform whitening to make it isotropic, which would save significant computations. So, if the original measurement noise covariance $\mathbf{R} = \sigma^2\mathbf{I}_m$ and the dimension of $\tilde{\mathbf{f}}$ is n, then the inferred measurement noise covariance will be $\mathbf{R}_1 = \sigma^2\mathbf{I}_n$ and $\mathbf{R}_2 = \sigma^2\mathbf{I}_{m-n}$ .

Now we can directly solve for the error of the new feature based on the first subsystem:

$\begin{align*} \tilde{\mathbf{f}} &= \mathbf{H}_{f1}^{-1}(\mathbf{r}_1-\mathbf{n}_1-\mathbf{H}_x\tilde{\mathbf{x}}) \\ \Rightarrow~ \mathbb{E}[\tilde{\mathbf{f}}] &= \mathbf{H}_{f1}^{-1}(\mathbf{r}_1) \end{align*}$

where we assumed noise and state error are zero mean. We can update $\hat{\mathbf f}$ with this correction by $\hat{\mathbf f}+\mathbb{E}[\tilde{\mathbf{f}}]$ . Note that this is equivalent to a Gauss Newton step for solving the corresponding maximum likelihood estimation (MLE) formed by fixing the estimate of $\mathbf{x}$ and optimizing over the value of $\hat{\mathbf{f}}$ , and should therefore be zero if we used such an optimization to come up with our initial estimate for the new variable.

We now can compute the covariance of the new feature as follows:

$\begin{align*} \mathbf{P}_{ff} &= \mathbb{E}\Big[(\tilde{\mathbf{f}}-\mathbb{E}[\tilde{\mathbf{f}}]) (\tilde{\mathbf{f}}-\mathbb{E}[\tilde{\mathbf{f}}])^{\top}\Big] \\ &= \mathbb{E}\Big[(\mathbf{H}_{f1}^{-1}(-\mathbf{n}_1-\mathbf{H}_{x1}\tilde{\mathbf{x}})) (\mathbf{H}_{f1}^{-1}(-\mathbf{n}_1-\mathbf{H}_{x1}\tilde{\mathbf{x}}))^{\top}\Big] \\ &= \mathbf{H}_{f1}^{-1}(\mathbf{H}_{x1}\mathbf{P}_{xx}\mathbf{H}_{x1}^{\top} + \mathbf{R}_1)\mathbf{H}_{f1}^{-\top} \end{align*}$

and the cross correlation can be computed as:

$\begin{align*} \mathbf{P}_{xf} &= \mathbb{E}\Big[(\tilde{\mathbf{x}}) (\tilde{\mathbf{f}}-\mathbb{E}[\tilde{\mathbf{f}}])^{\top}\Big] \\ &= \mathbb{E}\Big[(\tilde{\mathbf{x}}) (\mathbf{H}_{f1}^{-1}(-\mathbf{n}_1-\mathbf{H}_{x1}\tilde{\mathbf{x}}))^{\top}\Big] \\ &= -\mathbf{P}_{xx}\mathbf{H}_{x1}^{\top}\mathbf{H}_{f1}^{-\top} \end{align*}$

These entries can then be placed in the correct location for the covariance. For example when initializing a new feature to the end of the state, the augmented covariance would be:

$\begin{align*} \mathbf{P}_{aug} = \begin{bmatrix} \mathbf{P}_{xx} & \mathbf{P}_{xf} \\ \mathbf{P}_{xf}^{\top} & \mathbf{P}_{ff} \end{bmatrix} \end{align*}$

Note that this process does not update the estimate for $\mathbf{x}$ . However, after initialization, we can then use the second system, $\mathbf{r}_2$ , $\mathbf{H}_{x2}$ , and $\mathbf{n}_2$ to update our new state through a standard EKF update (see Linear Measurement Update section). We note that if there are exactly enough measurements to initialize the state, then no update occurs. This make sense, as the system is not yet overconstrained, thus only the to-be-initialized state and its uncertainty can be found (it does not yet benefit the navigation state estimates). Some additional notes on performing delayed initialization can be found in this technical report.