34 lines
792 B
C++
34 lines
792 B
C++
/**
|
|
* @file ConstraintOptimizer.h
|
|
* @brief Utilities and classes required for solving Constrained Problems
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <Matrix.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Basic function that uses LDL factorization to solve a
|
|
* KKT system (state and lagrange multipliers) of the form:
|
|
* Gd=b, where
|
|
* G = [B A] d = [ x ] b = - [g]
|
|
* [A' 0] [lam] [h]
|
|
* B = Hessian of Lagragian function
|
|
* A = Gradient of constraints
|
|
* x = state
|
|
* lam = vector of lagrange mulipliers
|
|
* g = gradient of f(x) evaluated a point
|
|
* h = current value of c(x)
|
|
*
|
|
* @return pair of state and lambas
|
|
*/
|
|
std::pair<Vector, Vector> solveCQP(const Matrix& B, const Matrix& A,
|
|
const Vector& g, const Vector& h);
|
|
|
|
} // \namespace gtsam
|
|
|
|
|