40 lines
972 B
C++
40 lines
972 B
C++
/**
|
|
* @file ConstraintOptimizer.cpp
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#include <ConstraintOptimizer.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
|
|
/* ************************************************************************* */
|
|
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));
|
|
}
|
|
|