DynaVINS++: Robust Visual-Inertial State Estimator in Dynamic Environments by Adaptive Truncated Least Squares and Stable State Recovery

TL;DR

A robust VINS framework called DynaVINS++ is proposed, which employs an adaptive truncated least square method that adaptively adjusts the truncation range using both feature association and IMU preintegration to effectively minimize the effect of the dynamic objects while reducing the computational cost.

Abstract

Despite extensive research in robust visual-inertial navigation systems~(VINS) in dynamic environments, many approaches remain vulnerable to objects that suddenly start moving, which are referred to as \textit{abruptly dynamic objects}. In addition, most approaches have considered the effect of dynamic objects only at the feature association level. In this study, we observed that the state estimation diverges when errors from false correspondences owing to moving objects incorrectly propagate into the IMU bias terms. To overcome these problems, we propose a robust VINS framework called \mbox{\textit{DynaVINS++}}, which employs a) adaptive truncated least square method that adaptively adjusts the truncation range using both feature association and IMU preintegration to effectively minimize the effect of the dynamic objects while reducing the computational cost, and b)~stable state recovery with bias consistency check to correct misestimated IMU bias and to prevent the divergence caused by abruptly dynamic objects. As verified in both public and real-world datasets, our approach shows promising performance in dynamic environments, including scenes with abruptly dynamic objects.

Figures (7)

• Figure 1: Scenes with an abruptly dynamic object in the real world. (a) The bus and its features were initially static. (b) The bus moved suddenly during observation. (c) The estimated trajectory for each method in this case. (d) The baseline method assumes that the features from the abruptly dynamic object are static, leading to divergence in the opposite direction of the bus's movement. In contrast, the proposed method effectively reduces the effect of the abruptly dynamic object and uses the static features as inliers, resulting in an accurate trajectory estimate.
• Figure 2: Pipeline of the proposed robust VIO framework. (a) The truncation range for the surrogate cost, a form of truncated least squares, is defined by feature information. Initial range is shown by the blue vertical lines. Concurrently, the bundle adjustment (BA) process is optimized using Black-Rangarajan (B-R) duality. (b) The stability of the optimized result is checked using bias. (c) If the optimized states are determined to be unstable, all states are recovered to the previous stable values, and the truncation range of the surrogate cost is narrowed as shown by the orange vertical lines. Otherwise, the process moves to the next sliding window, and the states are stored.
• Figure 3: (a) Surrogate cost and the corresponding derivative (gradients) with respect to the previous weight $\bar{w}$ in DynaVINS song2022dynavins. It is shown that the gradient cannot be zero for all ranges. (b) Surrogate cost and the gradients with respect to the maximum residual of static features, $\hat{r}^2_{\text{max}}$, in the current frame in our proposed method. The gradient is zero when the residual exceeds the predefined maximum threshold, 10 in this figure, or $2\hat{r}_{\text{max}}^2$.
• Figure 4: Visual description of the residual for the current frame when an abruptly dynamic object exists ($\mathbf{SE}(3)$ is projected onto the 2D for better visualization). The current state can fall into incorrect optimum if the influence of the visual residual becomes greater than the IMU residual. By increasing the non-convexity of the surrogate cost (from the blue dotted line to the green dashed line), the optimizer can ignore the visual residual and make the state converge into the actual optimum.
• Figure 5: (a) Absolute trajectory error and (b) the average BA time with respect to the maximum number of features in parking_lothigh sequence.