InCOpt: Incremental Constrained Optimization using the Bayes Tree

Abstract

We consider the problem of learning error covariance matrices for robotic state estimation. The convergence of a state estimator to the correct belief over the robot state is dependent on the proper tuning of noise models. During inference, these models are used to weigh different blocks of the Jacobian and error vector resulting from linearization and hence, additionally affect the stability and convergence of the non-linear system. We propose a gradient-based method to estimate well-conditioned covariance matrices by formulating the learning process as a constrained bilevel optimization problem over factor graphs. We evaluate our method against baselines across a range of simulated and real-world tasks and demonstrate that our technique converges to model estimates that lead to better solutions as evidenced by the improved tracking accuracy on unseen test trajectories.

Handling Constrained Optimization in Factor Graphs for Autonomous Navigation

“…They address nonlinear unconstrained optimization problems using ILS [15]. Extending factor graphs to constrained optimization is a relevant topic: it allows both new ways of addressing old problems, such as distributed or robust SLAM [2], [7], [10], and new applications of the tool, such as Optimal Control [21], [23], [27].…”

“…They explain how to represent constrained optimization over a Bayes Tree. More recently, the newer version of the solver was presented by Qadri et al [21] where online relinearization is used for efficiency. In the computer vision literature, the AL method was adopted by Eriksson et al [14] to address the rotation synchronization problem.…”

“…Inspired by [21], [23], this work revisits AL on factor graphs and provides an implementation scheme of our primal-dual procedure. We test its generality with application to three problems from different areas and of varying complexity.…”

“…Instead of Eq. ( 9), in [21] they use as AL function The dual update follows the same rule of Eq. ( 12)…”

“…Factor graphs are widely used to both model and solve unconstrained nonlinear optimization problems, relying on Iterative Least-Squares (ILS) solvers, such as those developed in the field of SLAM [15]. In this paper, we present the AL-extension of [15] to constrained optimization, leveraging on recent results from the work of Sodhi et al [23], Qadri et al [21] and ours [4].…”