71 lines
1.9 KiB
C++
71 lines
1.9 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));
|
||
|
|
}
|
||
|
|
|
||
|
|
/* ************************************************************************* */
|
||
|
|
Vector gtsam::linesearch(const Vector& x0, const Vector& delta,
|
||
|
|
double (*penalty)(const Vector&), size_t maxIt) {
|
||
|
|
|
||
|
|
// calculate the initial error
|
||
|
|
double init_error = penalty(x0);
|
||
|
|
Vector step = delta;
|
||
|
|
for (size_t i=0; i<maxIt; ++i) {
|
||
|
|
Vector x = x0 + step;
|
||
|
|
double cur_error = penalty(x);
|
||
|
|
if (cur_error < init_error) // if we have improved, return the step
|
||
|
|
return step;
|
||
|
|
else { // otherwise, make a smaller step
|
||
|
|
step = 0.5 * step;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO: should do something clever here
|
||
|
|
return step;
|
||
|
|
}
|
||
|
|
|