80 lines
2.0 KiB
C++
80 lines
2.0 KiB
C++
/**
|
|
* @file ConstraintOptimizer.h
|
|
* @brief Utilities and classes required for solving Constrained Problems
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <boost/optional.hpp>
|
|
#include <Matrix.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* BFGS Hessian estimator
|
|
* Maintains an estimate for a hessian matrix using
|
|
* only derivatives and the step
|
|
*/
|
|
class BFGSEstimator {
|
|
protected:
|
|
size_t n_; // dimension of square matrix
|
|
Matrix B_; // current estimate
|
|
Vector prev_dfx_; // previous gradient value
|
|
public:
|
|
|
|
/**
|
|
* Creates an estimator of a particular dimension
|
|
*/
|
|
BFGSEstimator(size_t n) : n_(n), B_(eye(n,n)) {}
|
|
|
|
~BFGSEstimator() {}
|
|
|
|
/**
|
|
* Direct vector interface - useful for small problems
|
|
*
|
|
* Update will set the previous gradient and update the
|
|
* estimate for the Hessian. When specified without
|
|
* the step, this is the initialization phase, and will not
|
|
* change the Hessian estimate
|
|
* @param df is the gradient of the function
|
|
* @param step is the step vector applied at the last iteration
|
|
*/
|
|
void update(const Vector& df, const boost::optional<Vector&> step = boost::none);
|
|
|
|
// access functions
|
|
const Matrix& getB() const { return B_; }
|
|
size_t dim() const { return n_; }
|
|
|
|
};
|
|
|
|
|
|
/**
|
|
* 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);
|
|
|
|
/**
|
|
* Simple line search function using an externally specified
|
|
* penalty function
|
|
*/
|
|
Vector linesearch(const Vector& x, const Vector& delta,
|
|
double (*penalty)(const Vector&), size_t maxIt = 10);
|
|
|
|
} // \namespace gtsam
|
|
|
|
|