InCOpt: Incremental constrained optimization using the Bayes tree

Mohamad Qadri, Paloma Sodhi, Joshua G Mangelson, Frank Dellaert, Michael Kaess

Abstract:

Abstract— In this work, we investigate the problem of incrementally solving constrained non-linear optimization problems
formulated as factor graphs. Prior incremental solvers were
either restricted to the unconstrained case or required periodic
batch relinearizations of the objective and constraints which
are expensive and detract from the online nature of the algorithm. We present InCOpt, an Augmented Lagrangian-based
incremental constrained optimizer that views matrix operations
as message passing over the Bayes tree. We first show how
the linear system, resulting from linearizing the constrained
objective, can be represented as a Bayes tree. We then propose
an algorithm that views forward and back substitutions, which
naturally arise from solving the Lagrangian, as upward and
downward passes on the tree. Using this formulation, InCOpt can exploit properties such as fluid/online relinearization
leading to increased accuracy without a sacrifice in runtime.
We evaluate our solver on different applications (navigation
and manipulation) and provide an extensive evaluation against
existing constrained and unconstrained solvers.