Visual-Inertial Odometry with Online Calibration of Velocity-Control Based Kinematic Motion Models

Various recent approaches combine IMU and wheel odometry. Brossard et al. [11] suggest an EKF based approach which uses deep learning to predict the noise properties in an EKF framework which fuses IMU measurements to predict motion. In [12], deep kernel Gaussian Process models are learned for the motion and observation models which are used to fuse IMU and wheel odometry measurements in an EKF framework. In RINS-W [13], Brossard et al. propose to estimate the motion from IMU and odometry measurements using an RNN which detects different motion profiles in an EKF framework. These approaches, however, do not use the complementary strengths of visual measurements and do not provide a forward model.

Cameras provide complementary information to inertial measurement units for motion estimation. For general motions, 3-DoF linear acceleration and rotational velocity measurements make roll and pitch orientation observable relative to the gravity direction. However, double integration of the linear acceleration requires accurate estimation of biases (offsets) in the measurements and makes linear position estimation prone to drift. Similarly, the yaw orientation around the gravity direction is not observable and prone to drift due to noisy and biased gyroscope measurements. Visual measurements provide a reference for pose estimation to a local 3D map of the environment which is concurrently build with the pose estimates in VIO approaches. By this, all DoFs become observable, while the IMU provides high frame-rate measurements which improve the accuracy between images.

The Multi-State-Constrained Kalman Filter (MSCKF [1]) for VIO has been recently extended to incorporate wheel odometry and overcome observability issues of monocular visual-inertial odometry on wheeled robots in VINS on wheels [5]. The authors analyze observability of monocular visual-inertial navigation systems on a mobile robot platform and show that for specific motions, scale and 3-DoF rotation become unobservable. They also show that adding wheel encoder measurements makes scale observable. The approach does not calibrate the motion model parameters online like our method. In our setting, scale is already observable through the fixed calibrated baseline of our stereo camera.

Ma et al. [6] adopt the VINS on wheels approach and extend it with an Ackerman drive model. Jung et al. [14] add GPS measurements directly to VINS on wheels to make position observable. Yang and Huang [7] analyze observability for VINS on wheels with line and plane observations. Another approach concurrently estimates the wheel slippage with VIO [15]. More closely related to our method is the approach by Lee et al. [16] which investigates online calibration of the wheel odometry parameters and analyzes observability of the calibration parameters for different constraint motion scenarios. In contrast, we employ a non-linear optimization based approach for visual-inertial odometry and incorporate an inverse motion model for constraints.

Some approaches integrate wheel odometry into non-linear optimization based approaches. Liu et al. [17] develop online calibration of the extrinsics between camera, IMU and odometer. Liu et at. [18] propose a novel initialization approach which corrects the initial state estimates after the first turning motion to handle unobservability of the calibration parameters for straight motions. Chen et al. [19] calibrate visual-inertial-wheel odometry offline. The approach in [20] integrates smooth motion manifold constraints. Zheng and Liu [21] incorporate a planar motion constraint for the mobile base but allow small deviations from this motion in 6 DoF for the camera-IMU system. Also, different to ours, these approaches use wheel encoders to measure odometry. We propose a new approach which incorporates an inverse motion model into optimization-based visual-inertial odometry and calibrates the model online including extrinsics and time synchronization.

Optimal control approaches typically rely on action-conditional dynamics models which use them as forward models to plan towards goals. One recent example is the approach of Williams et al. [22] which learns a dynamics model offline from GPS. In [23] a sensor-based localization method using LiDAR, optical speed sensors and INS is used to provide feedback for learning deviations from an analytic dynamics model.

In our approach, we tightly integrate a kinematic motion model in a visual-inertial odometry which allows for calibrating the model online.

We extend the non-linear optimization-based visual-inertial odometry approach in [3]. The approach uses a KLT-based keypoint tracking frontend to track the camera motion from frame to frame. Keyframes are extracted along the camera trajectory and the keypoint tracks generate landmarks in the keyframes with corresponding point measurements. Optimization variables are the camera poses for the frames and keyframes, and the camera velocity and biases in the frames. Only a set of recent keyframes and frames is optimized (3 frames and 7 keyframes in our experiments). Older frames and keyframes are marginalized from the optimization window and their information serves as a prior. More formally, for each frame at time $t$, we estimate the camera pose $\mathbf{T}_{t}\in SE(3)$, the camera velocity $\mathbf{v}_{t}$, and the bias parameters $\mathbf{b}_{t}$ of the IMU. For the keyframes, their camera poses $\mathbf{T}$ are optimized. The tracked keypoints become landmarks which are hosted in the keyframe in which they have been observed for the first time. Landmark $l$ is parameterized by 2D coordinates $(u_{l},v_{l})$ which minimally represent the bearing vector to the 3D point, and the inverse distance $d_{l}$. The KLT tracking provides measurements of the landmarks in subsequent frames and keyframes. Residuals $\mathbf{r}_{tl}=\mathbf{z}_{tl}-\pi\left(\mathbf{T}_{t},\mathbf{T}_{h(l)},u_{l},v_{l},d_{l}\right)$ are determined which measure the difference between the measured 2D positions $\mathbf{z}_{tl}$ in image at time $t$ towards the reprojection of landmark $l$ hosted in frame $h(l)$ into the image using function $\pi$.

In addition to visual residuals, an IMU provides linear acceleration and rotational velocity measurements from which further residuals on the pose and velocity estimates are formed. To this end, the IMU measurements are preintegrated [24] to obtain pseudo-measurements $\boldsymbol{\Delta}\mathbf{T}$, $\boldsymbol{\Delta}\mathbf{v}$ between successive frames at times $t_{i}$, $t_{j}$ with associated uncertainty $\mathbf{\Sigma}_{ij}$. The inertial residuals are

where $\mathbf{g}$ is the gravity direction and $\Delta t=t_{j}-t_{i}$.

The visual-inertial odometry corresponds to the non-linear optimization problem

where $\mathcal{K}$ and $\mathcal{F}$ are the set of keyframes and frames, respectively, $\mathcal{O}(t)$ is the set of landmarks observed in frame $t$, and $\mathcal{C}$ is the set of indices of subsequent frame pairs. To optimize the error function $E$ efficiently, old keyframes and frames outside an optimization window are marginalized and only the variables inside the window are optimized.

For further details on the visual-inertial odometry method, we refer the reader to [3].

We use a velocity-control based motion model [25] and assume that the robot can be controlled by a control command $\mathbf{u}=(v,\omega)^{\top}$ through a linear velocity $v\in\mathbb{R}$ in forward direction and a rotational velocity $\omega\in\mathbb{R}$. The motion model propagates the robot pose $\mathbf{P}_{t}\in SE(2)$ on the ground plane with the control command, $\mathbf{P}_{t^{\prime}}=\mathbf{P}_{t}\exp\left(\Delta t\,\widehat{\boldsymbol{\xi}}\right)$, where $\boldsymbol{\xi}=(v,0,\omega)$ is the twist vector, $\widehat{\boldsymbol{\xi}}=\left(\begin{array}[]{ccc}0&-\omega&v\\ \omega&0&0\\ 0&0&0\end{array}\right)$ maps a 3D vector to $se(2)$, and

is the exponential map of $SE(2)$ with time difference $\Delta t$ between the successive time steps. The exponential map finds the relative $SE(2)$ motion for constant velocities $\boldsymbol{\xi}$ over time duration $\Delta t$. The logarithm map $\widehat{\boldsymbol{\xi}}=\log(\mathbf{P})$ of $SE(2)$ is the inverse of the exponential map and maps relative poses $\mathbf{P}\in SE(2)$ to Lie algebra elements in $\widehat{\boldsymbol{\xi}}\in se(2)$. The operator $\widehat{\cdot}$ maps 3D twist coordinate vectors to twists in $se(2)$.

We incorporate the velocity-control motion model into the visual-inertial odometry framework in order to calibrate the parameters of the model and improve the robustness of the VIO. There are basically two choices to form residuals with a motion model: relative pose constraint (forward) or velocity constraint (inverse). Mathematically the forward and inverse model residuals are equivalent to each other. Both kinds of residuals achieve similar results, while the Jacobian matrix of the forward model is computationally more expensive (see appendix). For our motion constraint, we treat the velocity controls as measurement and assume the measurement noise comes from the velocity controls. In this case, using the inverse model residuals is simpler while using the forward model residuals requires error propagation with linear approximation of the exponential mapping.

Velocity commands are executed by the robot in the robot base frame whose pose relative to the world frame is denoted by $\mathbf{T}_{b}^{w}\in SE(3)$ (transforming coordinates from base $b$ to world frame $w$). In the base frame, the $x$-axis points in forward driving direction, while the $z$-axis points upwards and is the axis of rotational robot motion. The VIO provides pose estimates of the body frame of the IMU-camera sensor in the world frame, i.e. $\mathbf{T}_{i}^{w}\in SE(3)$. The sensor is placed rigidly on the robot at a relative pose $\mathbf{T}_{i}^{b}\in SE(3)$ to the robot base frame. To quantify the relative motion $\mathbf{T}_{b,t^{\prime}}^{b,t}$ of the robot base frame from times $t$ to $t^{\prime}$ of subsequent image frames, we can hence determine $\mathbf{T}_{b,t^{\prime}}^{b,t}=\mathbf{T}_{i,t}^{b,t}\left(\mathbf{T}_{i,t}^{w}\right)^{-1}{\mathbf{T}_{i,t^{\prime}}^{w}}\left(\mathbf{T}_{i,t^{\prime}}^{b,t^{\prime}}\right)^{-1}$. The rotation $\Delta\theta$ around the $z$-axis of the base frame is calculated from the relative rotation $\mathbf{R}_{b,t^{\prime}}^{b,t}$ in $\mathbf{T}_{b,t^{\prime}}^{b,t}$ as the $z$-component of $\log\left(\mathbf{R}_{b,t^{\prime}}^{b,t}\right)$. The translational motion $(\Delta x,\Delta y)^{\top}$ in the $x$-$y$-plane is determined from the corresponding entries of $\mathbf{T}_{b,t^{\prime}}^{b,t}$. The estimated twist is

where $\Delta t=t^{\prime}-t$. We add residuals of the form $\mathbf{r}_{\boldsymbol{\xi}}=\boldsymbol{\zeta}-\boldsymbol{\bar{\xi}}$ which implicitly measure the difference between the state estimates and the motion model prediction.

In practice, the real action of the robots differs from the received control commands due to effects such as time offsets and properties of low-level controllers.

Typically, control inputs and image frames are not synchronized but run asynchronously and often also at different rates. In our experiments, the control rate is 15 Hz and is lower than the 30 Hz image frame rate which is also used to update the VIO estimate. Moreover in the real world, a delay exists between the control command sent by the controller and the control command executed by the robot.

The robot physical hardware acceleration limits and internal controllers also prevent the robot from directly executing the control command even if the delay is known. To mitigate this difference and build a meaningful residual, we estimate an effective control $\boldsymbol{\bar{\xi}}_{t}$ at arbitrary time $t$, e.g. at the time of an image frame, from a window of most recent commands. We average a window of recent control commands with weights determined by a RBF kernel (see Fig. 1) for the translational and rotational parts separately:

Here $\mathcal{W}_{t}$ is a window of $N$ control commands indexed by their times $\tau$ at or before time $t$ and $d\tau:=t-\tau$. For an image frame at time $t$, the window typically spans the $N$ control commands that have occurred before the frame. We optimize for $\mu$ and $\sigma$ and scale factor $s$ of both linear and angular parts as global parameters together with the VIO states. The RBF parameters are summarized in the state variables $\mathbf{p}_{\mathit{rbf},t}$ at time $t$. In the experiments we demonstrate that the optimized RBF kernel can be used for motion prediction.

The robot body can vibrate during operation, the extrinsic pose $\mathbf{T}^{b}_{i}$ is thus modeled as a time-variant state that is affected by white noise. For $k$ factors within the optimization window this kinematics-based error can be summarized as

where $\mathbf{\Sigma}_{\boldsymbol{\xi},k}$ is the diagonal weight matrix for the velocity residuals, $\mathbf{r}_{\mathit{extr},k}$ is the difference between two adjacent extrinsic pose estimates and $\mathbf{\Sigma}_{\mathit{extr},k}$ is the diagonal weight matrix that reflects the white noise.

The VIO system takes the image frame at time $t^{\prime}$ and the raw controls up to the time $t$ of the previous frame as input for the optimization which also calibrates the RBF parameters for the motion model. The controls can be generated by manual control or an automatic high-level controller such as model predictive control for path tracking. In our experiments we use manual control commands as input and calibrate the RBF parameters online. A high-level controller would potentially require extrapolation of the last state estimate at the image rate to the current control time using the previous controls and the motion model.

We exploit prior knowledge that our robot moves on flat ground in indoor environments and add a stochastic plane constraint [5] for the robot pose. The plane can be parameterized as a 2 degree-of-freedom quaternion $\mathbf{q}^{g}_{w}$ and a scalar $d^{g}_{w}$ which represents the distance between the ground plane to the world frame origin. The residual is

with $\mathbf{e}_{3}=\begin{pmatrix}0&0&1\end{pmatrix}^{\top}$. The plane motion error term becomes $E_{\mathit{plane}}=\sum_{l}\mathbf{r}_{plane,l}^{\top}\mathbf{\Sigma}_{plane,l}^{-1}\mathbf{r}_{plane,k}$ with covariance matrix $\mathbf{\Sigma}_{plane}$. The stochasticity of the constraint allows for handling vibrations of the robot.

We integrate the above introduced calibration parameters of the constraints as additional variables into the visual-inertial odometry. The state of each frame in our optimization framework comprises the sensor pose $\mathbf{T}_{i}^{w}$, linear velocity $\mathbf{v}_{t}$, acceleration and gyroscope biases $\mathbf{b}_{acc}$, $\mathbf{b}_{gyro}$, landmark parameters $(u_{l},v_{l},d_{l})$ of hosted keypoints, base frame to sensor frame extrinsics $\mathbf{T}_{i}^{b}$, and the global variables including the RBF parameters $\mu$, $\sigma$, $s$ and the plane parameters $\mathbf{q}^{g}_{w}$ and $d^{g}_{w}$. The optimized error function can be summarized as $E=E_{\mathit{VIO}}+E_{\mathit{kin}}+E_{\mathit{plane}}$. During optimization the extrinsic poses will be marginalized like linear velocity and IMU biases while the global variables are kept and their linearization point is fixed once the first connected state is marginalized. A discussion of the observability of our model is provided in appendix.

Table I summarizes the RPE and ATE evaluation results. By integrating the kinematics motion constraint and the plane constraint (kin-vio) both ATE and RPE are reduced over pure VIO (vio). Fig. 1 illustrates and compares the results of data sequence big_01 estimated with purely VIO and our kinematics-constraint VIO. As can be seen, with the proposed method the deviation is decreased, especially in those parts of the trajectory with larger rotational motion.

In an ablation study, we compare the estimation accuracy of using RBF kernel weighting (kin-vio rbf) with other different weighting methods including RBF kernel with fixed initial parameters (kin-vio rbf w/o opt), non-weighted averaging of the command window (kin-vio avg), and using the last command that comes before the first frame of each frame pair (kin-vio raw). In addition, we also evaluate the estimate with only the plane motion constraint in addition to the VIO constraints. The error values are averaged over all data sequences and summarized in Tab. II. in which our combined method (kin-vio) consistently outperforms the others. The motion model can improve the tracking accuracy while the parameters are calibrated together with VIO. This can be attributed to the regularization by the parametric motion model whose parameters are adapted for a range of positive and negative velocity commands.

We also evaluate the performance of our method for forward motion prediction. Results for online prediction on data sequence small-01, mid-01 and large-01 are shown in Fig. 4. For this evaluation, we compute the prediction from each frame in the trajectory with different horizon lengths starting from the current estimate of the parameters. The command window is shifted along the future trajectory with the prediction step. We also compare the prediction accuracy with three alternative calibration methods. The first method uses the RBF model with constant initial parameters (rbf w/o opt). The second method uses the unweighted average value of the commands in the command window (avg). The third method calculates the prediction with the latest command (raw). Our proposed approach with calibrated RBF kernel parameters is denoted as ”rbf”.

It can be observed that the RBF kernel with optimized parameters has the smallest prediction error especially for longer prediction horizons. During the experiment we noticed that the improvement of the prediction accuracy is relatively smaller on the longer data sequences. This is because the rotations introduce larger errors and in the longer data sequences the robot mostly performs translational motion along the corridors with constant speed. Fig. 5 illustrates a prediction result on data sequence small-02 from beginning to the end of the trajectory with the final optimized RBF parameters. Our approach shows the smallest deviation compared with other methods.

We compare the run-time of our approach with the base VIO on our dataset using one Intel Xeon Silver 4112 [email protected] with 8 cores. On average the computation time increases by 34.14% from 14.79 ms to 19.84 ms for processing one frame, the maximum time stays similar with 60.05 ms at rare peaks, the minimum time is 3.90 ms. The approach can still process faster than real-time.

A potential limitation of our approach can be estimation bias in the VIO in settings such as texture-less scenes or biased camera intrinsics. In our work, we assume that the VIO result is sufficiently accurate with negligible systematic offsets so it can be used to calibrate the effective control. In future work, we could additionally integrate other types of sensors like GPS. Outlier measurements can be handled with robust norms in the VIO. One could also only activate the motion model when state variables like IMU bias are converged and indicate an accurate VIO.

Integrating wheel encoder information can also improve VIO accuracy, as it measures the actual rotation of the wheels at high frequency. To convert the wheel encoder information to body velocity or relative pose, one needs to consider the type of the vehicle. Our approach is simpler to integrate for robots whose motion can be modeled with our model (such as differential or Ackerman drives [25]). Moreover, our calibrated model could be used for downstream tasks such as model-predictive control based path tracking. In future work, we are also interested in combining wheel encoder measurements with our method.

Given the relative pose between two frames $\mathbf{T}_{b,t^{\prime}}^{b,t}$, the corresponding $SE(2)$ pose $\mathbf{P}_{b,t^{\prime}}^{b,t}$ in the horizontal plane consisting of rotational part $\mathbf{Q}_{b,t^{\prime}}^{b,t}$ and translational part $\mathbf{p}_{b,t^{\prime}}^{b,t}$, the effective control input $\boldsymbol{\xi}=(v,0,w)^{\top}$ with linear and angular velocity, and $\bar{\mathbf{P}}^{b,t}_{b,t^{\prime}}=exp(\Delta t\boldsymbol{\xi})$, the forward model can be written as

with derivatives $\frac{\partial\mathbf{r}_{\mathit{forw}}}{\partial\mathbf{P}_{b,t^{\prime}}^{b,t}}=\frac{\partial f(.)}{\partial{\mathbf{P}_{b,t^{\prime}}^{b,t}}}$ and $\frac{\partial\mathbf{r}_{\mathit{forw}}}{\partial\mathbf{p}_{\mathit{rbf}}}=\frac{\partial f(.)}{\partial\bar{\mathbf{P}}^{b,t}_{b,t^{\prime}}}\frac{\partial\bar{\mathbf{P}}^{b,t}_{b,t^{\prime}}}{\partial\boldsymbol{\xi}}\frac{\partial\boldsymbol{\xi}}{\partial\mathbf{p}_{\mathit{rbf}}}$, while the inverse model is

with derivatives $\frac{\partial\mathbf{r}_{\mathit{inv}}}{\partial\mathbf{P}_{b,t^{\prime}}^{b,t}}=\frac{\partial log_{\mathit{se2}}(.)}{\partial\mathbf{P}_{b,t^{\prime}}^{b,t}}$ and $\frac{\partial\mathbf{r}_{\mathit{inv}}}{\partial\mathbf{p}_{\mathit{rbf}}}=-\frac{\partial\boldsymbol{\xi}}{\partial\mathbf{p}_{\mathit{rbf}}}\Delta t$.

As in [5], the observability of the state variables can be analyzed based on the underlying state-space model irrespective of the implementation of the estimator. We discuss the observability properties of the state variables such as pose, plane parameters and RBF parameters for our model. A detailed analysis can be found in [27]. We follow the derivation in [5]. In general, the observability properties of the pose variables are the same for forward and inverse model. While in the forward model the derivative wrt. $\mathbf{p}_{b,t^{\prime}}^{b,t}$ is an identity matrix, the derivative in the inverse model is $J_{\log_{\mathit{se2}}}$. Because $J_{\log_{\mathit{se2}}}$ is an invertible matrix by definition, when we compute the observability matrix by multiplying this Jacobian matrix and the transition matrix, the rank of the observability matrix remains the same based on Sylvester’s inequality. Note that we use a stereo camera and hence scale becomes directly observable. We follow the proof scheme in [5] and show that the global orientation becomes observable by using the plane constraint and a set of priors in the initial frames. As in [5], the state transition is given by the IMU propagation model. The constraint from the velocity-based kinematic motion model is treated as an observation. The linearized transition matrix $\mathbf{\Phi}_{k,1}$ from time-step 1 to $k$ for the IMU propagation is derived in [28]. By including the transition of the RBF parameters as a constant propagation model, the transition matrix of our model becomes: $\bar{\boldsymbol{\Phi}}_{k,1}=\begin{bmatrix}\mathbf{\Phi}_{k,1}&\mathbf{0}\\ \mathbf{0}&\mathbf{I}\end{bmatrix}$ with the augmented state vector including plane parameters $\mathbf{q}^{g}_{w}$, $d^{g}_{w}$ between ground $g$ and world frame $w$, extrinsic parameters $\mathbf{q}^{b}_{i}$, $\mathbf{t}^{b}_{i}$ between robot base and IMU frame and RBF parameters $\mathbf{p}_{\mathit{rbf}}$, $\bar{\mathbf{x}}=\begin{bmatrix}\mathbf{x}^{\top}&\mathbf{q}^{g}_{w}&d^{g}_{w}&\mathbf{q}^{b}_{i}&\mathbf{t}^{b}_{i}&\mathbf{p}_{\mathit{rbf}}\end{bmatrix}^{\top}$.

As shown in [5], the plane distance $d^{g}_{w}$ is observable. Following the derivation in [5], the global orientation of the plane can be shown to be unobservable. Since our robot starts from still state, we use the initial accelerator measurement to initialize the plane angle. By adding this information as a Gaussian initial prior on the plane angle $\mathbf{q}^{g}_{w}$, the global orientation becomes observable. As shown in [16] the extrinsic parameters are also unobservable under specific motions. We used the extrinsics derived from the CAD model as an initial Gaussian prior to counteract this problem. We prove the observability of the RBF parameters ($s_{lin},\mu_{lin},\sigma_{lin}$) for the translational velocity. The parameters for the angular velocity have the same observability. We denote $\exp\left(-\frac{\|d\tau_{i}-\mu_{lin}\|^{2}}{2\sigma_{lin}^{2}}\right)$ as $\exp(.)$, where $d\tau_{i}$ is the time difference between control command at $t_{i}$ and image frame at $t$. The Jacobian matrix is $\mathbf{H}_{rbf}=\begin{bmatrix}\dots&-\frac{\bar{v}}{s}&A&B\end{bmatrix}$,

The nullspace for the RBF parameters $\mathbf{N}_{\mathit{rbf}}$ is $\mathbf{N}_{rbf}=\begin{bmatrix}\mathbf{0}&\dots&\mathbf{0}&\mathbf{I}_{3\times 3}\end{bmatrix}^{\top}$. The corresponding $3\times 3$ bottom right block of the observability matrix for our inverse motion model is $\mathbf{M}_{\mathit{rbf}}=\begin{bmatrix}-\frac{\bar{v}}{s}&0&0\\ 0&A&0\\ 0&0&B\end{bmatrix}$. The product of the motion model observability matrix and the nullspace $\mathbf{N}_{\mathit{rbf}}$ is equal to $\mathbf{M}_{\mathit{rbf}}$ as the other terms cancel out by multiplication with the $\mathbf{0}$ components in $\mathbf{N}_{\mathit{rbf}}$. The RBF parameters are unobservable if the velocities $v_{i}$ in the window are constant, which can happen especially at the beginning of the datasets when the robot stands still. We placed a weak Gaussian prior on the initial values of the RBF parameter estimates. Due to the marginalization prior, the RBF parameters will remain observable even if they become temporarily unobservable in the window.