50 lines
1.3 KiB
C++
50 lines
1.3 KiB
C++
/**
|
|
* @file ConstraintOptimizer.cpp
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <ConstraintOptimizer.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
void gtsam::BFGSEstimator::update(const Vector& dfx, const boost::optional<Vector&> step) {
|
|
if (step) {
|
|
Vector Bis = B_ * *step,
|
|
y = dfx - prev_dfx_;
|
|
B_ = B_ + outer_prod(y, y) / inner_prod(y, *step)
|
|
- outer_prod(Bis, Bis) / inner_prod(*step, Bis);
|
|
}
|
|
prev_dfx_ = dfx;
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
pair<Vector, Vector> gtsam::solveCQP(const Matrix& B, const Matrix& A,
|
|
const Vector& g, const Vector& h) {
|
|
// find the dimensions
|
|
size_t n = B.size1(), p = A.size2();
|
|
|
|
// verify matrices
|
|
if (n != B.size2())
|
|
throw invalid_argument("solveCQP: B matrix is not square!");
|
|
if (A.size1() != n)
|
|
throw invalid_argument("solveCQP: A matrix needs m = B.size1()");
|
|
|
|
// construct G matrix
|
|
Matrix G = zeros(n+p, n+p);
|
|
insertSub(G, B, 0, 0);
|
|
insertSub(G, A, 0, n);
|
|
insertSub(G, trans(A), n, 0);
|
|
|
|
Vector rhs = zero(n+p);
|
|
subInsert(rhs, -1.0*g, 0);
|
|
subInsert(rhs, -1.0*h, n);
|
|
|
|
// solve the system with the LDL solver
|
|
Vector fullResult = solve_ldl(G, rhs);
|
|
|
|
return make_pair(sub(fullResult, 0, n), sub(fullResult, n, n+p));
|
|
}
|
|
|