InCOpt: Incremental Constrained Optimization using the Bayes Tree

In this work, we investigate the problem of incre-mentally 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, In-COpt 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.

Authors

• Mohamad Qadri
• Paloma Sodhi
• Joshua G. Mangelson
• Frank Dellaert
• Michael Kaess

Keywords

• Linear systems
• Runtime
• Navigation
• Message passing
• Matrices
• Optimization
• Intelligent robots
• Constrained Optimization
• Nonlinear Problem
• Factor Graph
• Root Mean Square Error
• Time Step
• Matrix Factorization
• Inequality Constraints
• Nonlinear Least Squares
• Equality Constraints
• Optimization Step
• Penalty Term
• Subtree
• Constraint Violation
• Conditional Density
• Hard Constraints
• Update Time
• Simultaneous Localization And Mapping
• Soft Constraints
• Probabilistic Inference
• Sequential Quadratic Programming
• Nonlinear Factors
• QR Decomposition
• Updated Matrix
• Ground Truth Trajectory
• Bayesian Model
• Constraint Satisfaction
• Noisy Measurements