git subrepo commit third_party_modules/gtsam_module/gtsam
subrepo: subdir: "third_party_modules/gtsam_module/gtsam" merged: "abb912d" upstream: origin: "ssh://gerrit.skyd.io:29418/gtsam_upstream" branch: "pull_on_6_14" commit: "1ae7204" git-subrepo: version: "0.3.0" origin: "https://github.com/ingydotnet/git-subrepo" commit: "9a0f034"release/4.3a0
Abe
commit
9152b656cf
|
|
@ -1,4 +1,5 @@
|
|||
/build*
|
||||
.idea
|
||||
*.pyc
|
||||
*.DS_Store
|
||||
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
|
||||
|
|
|
|||
|
|
@ -68,6 +68,12 @@ protected:
|
|||
testGroup##testName##Instance; \
|
||||
void testGroup##testName##Test::run (TestResult& result_)
|
||||
|
||||
/**
|
||||
* Declare friend in a class to test its private methods
|
||||
*/
|
||||
#define FRIEND_TEST(testGroup, testName) \
|
||||
friend class testGroup##testName##Test;
|
||||
|
||||
/**
|
||||
* For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this
|
||||
* will not wrap execution with a try/catch block
|
||||
|
|
|
|||
|
|
@ -18,8 +18,6 @@
|
|||
//
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
#ifndef TESTRESULT_H
|
||||
#define TESTRESULT_H
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,21 @@
|
|||
NAME QP example
|
||||
ROWS
|
||||
N obj
|
||||
G r1
|
||||
L r2
|
||||
COLUMNS
|
||||
c1 r1 2.0 r2 -1.0
|
||||
c1 obj 1.5
|
||||
c2 r1 1.0 r2 2.0
|
||||
c2 obj -2.0
|
||||
RHS
|
||||
rhs1 obj -4.0
|
||||
rhs1 r1 2.0 r2 6.0
|
||||
RANGES
|
||||
BOUNDS
|
||||
UP BOUNDS c1 20.0
|
||||
QUADOBJ
|
||||
c1 c1 8.0
|
||||
c1 c2 2.0
|
||||
c2 c2 10.0
|
||||
ENDATA
|
||||
|
|
@ -354,3 +354,5 @@ namespace gtsam {
|
|||
}; // FactorGraph
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
|
|
@ -142,7 +142,7 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
|
|||
|
||||
// GTSAM_CONCEPT_MANIFOLD_TYPE(V);
|
||||
|
||||
int iteration = 0;
|
||||
size_t iteration = 0;
|
||||
|
||||
// check if we're already close enough
|
||||
double currentError = system.error(initial);
|
||||
|
|
|
|||
|
|
@ -131,7 +131,8 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Creates a shared_ptr clone of the factor with different keys using
|
||||
* Creates a shared_ptr clone of the
|
||||
* factor with different keys using
|
||||
* a map from old->new keys
|
||||
*/
|
||||
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ public:
|
|||
SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
|
||||
};
|
||||
|
||||
int maxIterations; ///< The maximum iterations to stop iterating (default 100)
|
||||
size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
|
||||
double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
|
||||
double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
|
||||
double errorTol; ///< The maximum total error to stop iterating (default 0.0)
|
||||
|
|
@ -54,7 +54,7 @@ public:
|
|||
}
|
||||
virtual void print(const std::string& str = "") const;
|
||||
|
||||
int getMaxIterations() const {
|
||||
size_t getMaxIterations() const {
|
||||
return maxIterations;
|
||||
}
|
||||
double getRelativeErrorTol() const {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,290 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ActiveSetSolver-inl.h
|
||||
* @brief Implmentation of ActiveSetSolver.
|
||||
* @author Ivan Dario Jimenez
|
||||
* @author Duy Nguyen Ta
|
||||
* @date 2/11/16
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
|
||||
|
||||
/******************************************************************************/
|
||||
// Convenient macros to reduce syntactic noise. undef later.
|
||||
#define Template template <class PROBLEM, class POLICY, class INITSOLVER>
|
||||
#define This ActiveSetSolver<PROBLEM, POLICY, INITSOLVER>
|
||||
|
||||
/******************************************************************************/
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints
|
||||
* If some inactive inequality constraints complain about the full step (alpha = 1),
|
||||
* we have to adjust alpha to stay within the inequality constraints' feasible regions.
|
||||
*
|
||||
* For each inactive inequality j:
|
||||
* - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints
|
||||
* - We want: aj'*(xk + alpha*p) - bj <= 0
|
||||
* - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
|
||||
* it's good!
|
||||
* - We only care when aj'*p > 0. In this case, we need to choose alpha so that
|
||||
* aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p)
|
||||
* We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
|
||||
*
|
||||
* We want the minimum of all those alphas among all inactive inequality.
|
||||
*/
|
||||
Template boost::tuple<double, int> This::computeStepSize(
|
||||
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||
const VectorValues& p, const double& maxAlpha) const {
|
||||
double minAlpha = maxAlpha;
|
||||
int closestFactorIx = -1;
|
||||
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
|
||||
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
|
||||
double b = factor->getb()[0];
|
||||
// only check inactive factors
|
||||
if (!factor->active()) {
|
||||
// Compute a'*p
|
||||
double aTp = factor->dotProductRow(p);
|
||||
|
||||
// Check if a'*p >0. Don't care if it's not.
|
||||
if (aTp <= 0)
|
||||
continue;
|
||||
|
||||
// Compute a'*xk
|
||||
double aTx = factor->dotProductRow(xk);
|
||||
|
||||
// alpha = (b - a'*xk) / (a'*p)
|
||||
double alpha = (b - aTx) / aTp;
|
||||
// We want the minimum of all those max alphas
|
||||
if (alpha < minAlpha) {
|
||||
closestFactorIx = factorIx;
|
||||
minAlpha = alpha;
|
||||
}
|
||||
}
|
||||
}
|
||||
return boost::make_tuple(minAlpha, closestFactorIx);
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
/*
|
||||
* The goal of this function is to find currently active inequality constraints
|
||||
* that violate the condition to be active. The one that violates the condition
|
||||
* the most will be removed from the active set. See Nocedal06book, pg 469-471
|
||||
*
|
||||
* Find the BAD active inequality that pulls x strongest to the wrong direction
|
||||
* of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0)
|
||||
*
|
||||
* For active inequality constraints (those that are enforced as equality constraints
|
||||
* in the current working set), we want lambda < 0.
|
||||
* This is because:
|
||||
* - From the Lagrangian L = f - lambda*c, we know that the constraint force
|
||||
* is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay
|
||||
* on the constraint surface, the constraint force has to balance out with
|
||||
* other unconstrained forces that are pulling x towards the unconstrained
|
||||
* minimum point. The other unconstrained forces are pulling x toward (-\grad f),
|
||||
* hence the constraint force has to be exactly \grad f, so that the total
|
||||
* force is 0.
|
||||
* - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0),
|
||||
* while we are solving for - (<=0) constraint.
|
||||
* - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction
|
||||
* i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied.
|
||||
* That means we want lambda < 0.
|
||||
* - This is because when the constrained force pulls x towards the infeasible region (+),
|
||||
* the unconstrained force is pulling x towards the opposite direction into
|
||||
* the feasible region (again because the total force has to be 0 to make x stay still)
|
||||
* So we can drop this constraint to have a lower error but feasible solution.
|
||||
*
|
||||
* In short, active inequality constraints with lambda > 0 are BAD, because they
|
||||
* violate the condition to be active.
|
||||
*
|
||||
* And we want to remove the worst one with the largest lambda from the active set.
|
||||
*
|
||||
*/
|
||||
Template int This::identifyLeavingConstraint(
|
||||
const InequalityFactorGraph& workingSet,
|
||||
const VectorValues& lambdas) const {
|
||||
int worstFactorIx = -1;
|
||||
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
|
||||
// inactive or a good inequality constraint, so we don't care!
|
||||
double maxLambda = 0.0;
|
||||
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
|
||||
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
|
||||
if (factor->active()) {
|
||||
double lambda = lambdas.at(factor->dualKey())[0];
|
||||
if (lambda > maxLambda) {
|
||||
worstFactorIx = factorIx;
|
||||
maxLambda = lambda;
|
||||
}
|
||||
}
|
||||
}
|
||||
return worstFactorIx;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Template JacobianFactor::shared_ptr This::createDualFactor(
|
||||
Key key, const InequalityFactorGraph& workingSet,
|
||||
const VectorValues& delta) const {
|
||||
// Transpose the A matrix of constrained factors to have the jacobian of the
|
||||
// dual key
|
||||
TermsContainer Aterms = collectDualJacobians<LinearEquality>(
|
||||
key, problem_.equalities, equalityVariableIndex_);
|
||||
TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
|
||||
key, workingSet, inequalityVariableIndex_);
|
||||
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
|
||||
AtermsInequalities.end());
|
||||
|
||||
// Collect the gradients of unconstrained cost factors to the b vector
|
||||
if (Aterms.size() > 0) {
|
||||
Vector b = problem_.costGradient(key, delta);
|
||||
// to compute the least-square approximation of dual variables
|
||||
return boost::make_shared<JacobianFactor>(Aterms, b);
|
||||
} else {
|
||||
return boost::make_shared<JacobianFactor>();
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
/* This function will create a dual graph that solves for the
|
||||
* lagrange multipliers for the current working set.
|
||||
* You can use lagrange multipliers as a necessary condition for optimality.
|
||||
* The factor graph that is being solved is f' = -lambda * g'
|
||||
* where f is the optimized function and g is the function resulting from
|
||||
* aggregating the working set.
|
||||
* The lambdas give you information about the feasibility of a constraint.
|
||||
* if lambda < 0 the constraint is Ok
|
||||
* if lambda = 0 you are on the constraint
|
||||
* if lambda > 0 you are violating the constraint.
|
||||
*/
|
||||
Template GaussianFactorGraph::shared_ptr This::buildDualGraph(
|
||||
const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
|
||||
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
|
||||
for (Key key : constrainedKeys_) {
|
||||
// Each constrained key becomes a factor in the dual graph
|
||||
JacobianFactor::shared_ptr dualFactor =
|
||||
createDualFactor(key, workingSet, delta);
|
||||
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
|
||||
}
|
||||
return dualGraph;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Template GaussianFactorGraph
|
||||
This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
|
||||
const VectorValues& xk) const {
|
||||
GaussianFactorGraph workingGraph;
|
||||
workingGraph.push_back(POLICY::buildCostFunction(problem_, xk));
|
||||
workingGraph.push_back(problem_.equalities);
|
||||
for (const LinearInequality::shared_ptr& factor : workingSet)
|
||||
if (factor->active()) workingGraph.push_back(factor);
|
||||
return workingGraph;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Template typename This::State This::iterate(
|
||||
const typename This::State& state) const {
|
||||
// Algorithm 16.3 from Nocedal06book.
|
||||
// Solve with the current working set eqn 16.39, but instead of solving for p
|
||||
// solve for x
|
||||
GaussianFactorGraph workingGraph =
|
||||
buildWorkingGraph(state.workingSet, state.values);
|
||||
VectorValues newValues = workingGraph.optimize();
|
||||
// If we CAN'T move further
|
||||
// if p_k = 0 is the original condition, modified by Duy to say that the state
|
||||
// update is zero.
|
||||
if (newValues.equals(state.values, 1e-7)) {
|
||||
// Compute lambda from the dual graph
|
||||
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
|
||||
newValues);
|
||||
VectorValues duals = dualGraph->optimize();
|
||||
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
|
||||
// If all inequality constraints are satisfied: We have the solution!!
|
||||
if (leavingFactor < 0) {
|
||||
return State(newValues, duals, state.workingSet, true,
|
||||
state.iterations + 1);
|
||||
} else {
|
||||
// Inactivate the leaving constraint
|
||||
InequalityFactorGraph newWorkingSet = state.workingSet;
|
||||
newWorkingSet.at(leavingFactor)->inactivate();
|
||||
return State(newValues, duals, newWorkingSet, false,
|
||||
state.iterations + 1);
|
||||
}
|
||||
} else {
|
||||
// If we CAN make some progress, i.e. p_k != 0
|
||||
// Adapt stepsize if some inactive constraints complain about this move
|
||||
double alpha;
|
||||
int factorIx;
|
||||
VectorValues p = newValues - state.values;
|
||||
boost::tie(alpha, factorIx) = // using 16.41
|
||||
computeStepSize(state.workingSet, state.values, p, POLICY::maxAlpha);
|
||||
// also add to the working set the one that complains the most
|
||||
InequalityFactorGraph newWorkingSet = state.workingSet;
|
||||
if (factorIx >= 0)
|
||||
newWorkingSet.at(factorIx)->activate();
|
||||
// step!
|
||||
newValues = state.values + alpha * p;
|
||||
return State(newValues, state.duals, newWorkingSet, false,
|
||||
state.iterations + 1);
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Template InequalityFactorGraph This::identifyActiveConstraints(
|
||||
const InequalityFactorGraph& inequalities,
|
||||
const VectorValues& initialValues, const VectorValues& duals,
|
||||
bool useWarmStart) const {
|
||||
InequalityFactorGraph workingSet;
|
||||
for (const LinearInequality::shared_ptr& factor : inequalities) {
|
||||
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
|
||||
if (useWarmStart && duals.size() > 0) {
|
||||
if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
|
||||
else workingFactor->inactivate();
|
||||
} else {
|
||||
double error = workingFactor->error(initialValues);
|
||||
// Safety guard. This should not happen unless users provide a bad init
|
||||
if (error > 0) throw InfeasibleInitialValues();
|
||||
if (fabs(error) < 1e-7)
|
||||
workingFactor->activate();
|
||||
else
|
||||
workingFactor->inactivate();
|
||||
}
|
||||
workingSet.push_back(workingFactor);
|
||||
}
|
||||
return workingSet;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Template std::pair<VectorValues, VectorValues> This::optimize(
|
||||
const VectorValues& initialValues, const VectorValues& duals,
|
||||
bool useWarmStart) const {
|
||||
// Initialize workingSet from the feasible initialValues
|
||||
InequalityFactorGraph workingSet = identifyActiveConstraints(
|
||||
problem_.inequalities, initialValues, duals, useWarmStart);
|
||||
State state(initialValues, duals, workingSet, false, 0);
|
||||
|
||||
/// main loop of the solver
|
||||
while (!state.converged) state = iterate(state);
|
||||
|
||||
return std::make_pair(state.values, state.duals);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Template std::pair<VectorValues, VectorValues> This::optimize() const {
|
||||
INITSOLVER initSolver(problem_);
|
||||
VectorValues initValues = initSolver.solve();
|
||||
return optimize(initValues);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#undef Template
|
||||
#undef This
|
||||
|
|
@ -0,0 +1,204 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file ActiveSetSolver.h
|
||||
* @brief Active set method for solving LP, QP problems
|
||||
* @author Ivan Dario Jimenez
|
||||
* @author Duy Nguyen Ta
|
||||
* @date 1/25/16
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This class implements the active set algorithm for solving convex
|
||||
* Programming problems.
|
||||
*
|
||||
* @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or
|
||||
* QP (quadratic program).
|
||||
* @tparam POLICY specific detail policy tailored for the particular program
|
||||
* @tparam INITSOLVER Solver for an initial feasible solution of this problem.
|
||||
*/
|
||||
template <class PROBLEM, class POLICY, class INITSOLVER>
|
||||
class ActiveSetSolver {
|
||||
public:
|
||||
/// This struct contains the state information for a single iteration
|
||||
struct State {
|
||||
VectorValues values; //!< current best values at each step
|
||||
VectorValues duals; //!< current values of dual variables at each step
|
||||
InequalityFactorGraph workingSet; /*!< keep track of current active/inactive
|
||||
inequality constraints */
|
||||
bool converged; //!< True if the algorithm has converged to a solution
|
||||
size_t iterations; /*!< Number of iterations. Incremented at the end of
|
||||
each iteration. */
|
||||
|
||||
/// Default constructor
|
||||
State()
|
||||
: values(), duals(), workingSet(), converged(false), iterations(0) {}
|
||||
|
||||
/// Constructor with initial values
|
||||
State(const VectorValues& initialValues, const VectorValues& initialDuals,
|
||||
const InequalityFactorGraph& initialWorkingSet, bool _converged,
|
||||
size_t _iterations)
|
||||
: values(initialValues),
|
||||
duals(initialDuals),
|
||||
workingSet(initialWorkingSet),
|
||||
converged(_converged),
|
||||
iterations(_iterations) {}
|
||||
};
|
||||
|
||||
protected:
|
||||
const PROBLEM& problem_; //!< the particular [convex] problem to solve
|
||||
VariableIndex equalityVariableIndex_,
|
||||
inequalityVariableIndex_; /*!< index to corresponding factors to build
|
||||
dual graphs */
|
||||
KeySet constrainedKeys_; /*!< all constrained keys, will become factors in
|
||||
dual graphs */
|
||||
|
||||
/// Vector of key matrix pairs. Matrices are usually the A term for a factor.
|
||||
typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
|
||||
|
||||
public:
|
||||
/// Constructor
|
||||
ActiveSetSolver(const PROBLEM& problem) : problem_(problem) {
|
||||
equalityVariableIndex_ = VariableIndex(problem_.equalities);
|
||||
inequalityVariableIndex_ = VariableIndex(problem_.inequalities);
|
||||
constrainedKeys_ = problem_.equalities.keys();
|
||||
constrainedKeys_.merge(problem_.inequalities.keys());
|
||||
}
|
||||
|
||||
/**
|
||||
* Optimize with provided initial values
|
||||
* For this version, it is the responsibility of the caller to provide
|
||||
* a feasible initial value, otherwise, an exception will be thrown.
|
||||
* @return a pair of <primal, dual> solutions
|
||||
*/
|
||||
std::pair<VectorValues, VectorValues> optimize(
|
||||
const VectorValues& initialValues,
|
||||
const VectorValues& duals = VectorValues(),
|
||||
bool useWarmStart = false) const;
|
||||
|
||||
/**
|
||||
* For this version the caller will not have to provide an initial value
|
||||
* @return a pair of <primal, dual> solutions
|
||||
*/
|
||||
std::pair<VectorValues, VectorValues> optimize() const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Compute minimum step size alpha to move from the current point @p xk to the
|
||||
* next feasible point along a direction @p p: x' = xk + alpha*p,
|
||||
* where alpha \in [0,maxAlpha].
|
||||
*
|
||||
* For QP, maxAlpha = 1. For LP: maxAlpha = Inf.
|
||||
*
|
||||
* @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex
|
||||
* is the closest inactive inequality constraint that blocks xk to move
|
||||
* further and that has the minimum alpha, or (-1, maxAlpha) if there is no
|
||||
* such inactive blocking constraint.
|
||||
*
|
||||
* If there is a blocking constraint, the closest one will be added to the
|
||||
* working set and become active in the next iteration.
|
||||
*/
|
||||
boost::tuple<double, int> computeStepSize(
|
||||
const InequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||
const VectorValues& p, const double& maxAlpha) const;
|
||||
|
||||
/**
|
||||
* Finds the active constraints in the given factor graph and returns the
|
||||
* Dual Jacobians used to build a dual factor graph.
|
||||
*/
|
||||
template<typename FACTOR>
|
||||
TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
|
||||
const VariableIndex& variableIndex) const {
|
||||
/*
|
||||
* Iterates through each factor in the factor graph and checks
|
||||
* whether it's active. If the factor is active it reutrns the A
|
||||
* term of the factor.
|
||||
*/
|
||||
TermsContainer Aterms;
|
||||
if (variableIndex.find(key) != variableIndex.end()) {
|
||||
for (size_t factorIx : variableIndex[key]) {
|
||||
typename FACTOR::shared_ptr factor = graph.at(factorIx);
|
||||
if (!factor->active())
|
||||
continue;
|
||||
Matrix Ai = factor->getA(factor->find(key)).transpose();
|
||||
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
|
||||
}
|
||||
}
|
||||
return Aterms;
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a dual factor from the current workingSet and the key of the
|
||||
* the variable used to created the dual factor.
|
||||
*/
|
||||
JacobianFactor::shared_ptr createDualFactor(
|
||||
Key key, const InequalityFactorGraph& workingSet,
|
||||
const VectorValues& delta) const;
|
||||
|
||||
public: /// Just for testing...
|
||||
|
||||
/// Builds a dual graph from the current working set.
|
||||
GaussianFactorGraph::shared_ptr buildDualGraph(
|
||||
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
|
||||
|
||||
/**
|
||||
* Build a working graph of cost, equality and active inequality constraints
|
||||
* to solve at each iteration.
|
||||
* @param workingSet the collection of all cost and constrained factors
|
||||
* @param xk current solution, used to build a special quadratic cost in LP
|
||||
* @return a new better solution
|
||||
*/
|
||||
GaussianFactorGraph buildWorkingGraph(
|
||||
const InequalityFactorGraph& workingSet,
|
||||
const VectorValues& xk = VectorValues()) const;
|
||||
|
||||
/// Iterate 1 step, return a new state with a new workingSet and values
|
||||
State iterate(const State& state) const;
|
||||
|
||||
/// Identify active constraints based on initial values.
|
||||
InequalityFactorGraph identifyActiveConstraints(
|
||||
const InequalityFactorGraph& inequalities,
|
||||
const VectorValues& initialValues,
|
||||
const VectorValues& duals = VectorValues(),
|
||||
bool useWarmStart = false) const;
|
||||
|
||||
/// Identifies active constraints that shouldn't be active anymore.
|
||||
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
|
||||
const VectorValues& lambdas) const;
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Find the max key in a problem.
|
||||
* Useful to determine unique keys for additional slack variables
|
||||
*/
|
||||
template <class PROBLEM>
|
||||
Key maxKey(const PROBLEM& problem) {
|
||||
auto keys = problem.cost.keys();
|
||||
Key maxKey = *std::max_element(keys.begin(), keys.end());
|
||||
if (!problem.equalities.empty())
|
||||
maxKey = std::max(maxKey, *problem.equalities.keys().rbegin());
|
||||
if (!problem.inequalities.empty())
|
||||
maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin());
|
||||
return maxKey;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam_unstable/linear/ActiveSetSolver-inl.h>
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file EqualityFactorGraph.h
|
||||
* @brief Factor graph of all LinearEquality factors
|
||||
* @date Dec 8, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam_unstable/linear/LinearEquality.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Collection of all Linear Equality constraints Ax=b of
|
||||
* a Programming problem as a Factor Graph
|
||||
*/
|
||||
class EqualityFactorGraph: public FactorGraph<LinearEquality> {
|
||||
public:
|
||||
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
|
||||
|
||||
/// Compute error of a guess.
|
||||
double error(const VectorValues& x) const {
|
||||
double total_error = 0.;
|
||||
for (const sharedFactor& factor : *this) {
|
||||
if (factor)
|
||||
total_error += factor->error(x);
|
||||
}
|
||||
return total_error;
|
||||
}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template<> struct traits<EqualityFactorGraph> : public Testable<
|
||||
EqualityFactorGraph> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file InequalityFactorGraph.h
|
||||
* @brief Factor graph of all LinearInequality factors
|
||||
* @date Dec 8, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/linear/LinearInequality.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Collection of all Linear Inequality constraints Ax-b <= 0 of
|
||||
* a Programming problem as a Factor Graph
|
||||
*/
|
||||
class InequalityFactorGraph: public FactorGraph<LinearInequality> {
|
||||
private:
|
||||
typedef FactorGraph<LinearInequality> Base;
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<InequalityFactorGraph> shared_ptr;
|
||||
|
||||
/** print */
|
||||
void print(const std::string& str, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
Base::print(str, keyFormatter);
|
||||
}
|
||||
|
||||
/** equals */
|
||||
bool equals(const InequalityFactorGraph& other, double tol = 1e-9) const {
|
||||
return Base::equals(other, tol);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute error of a guess.
|
||||
* Infinity error if it violates an inequality; zero otherwise. */
|
||||
double error(const VectorValues& x) const {
|
||||
for (const sharedFactor& factor : *this) {
|
||||
if (factor)
|
||||
if (factor->error(x) > 1e-7)
|
||||
return std::numeric_limits<double>::infinity();
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template<>
|
||||
struct traits<InequalityFactorGraph> : public Testable<InequalityFactorGraph> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,45 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file InfeasibleInitialValues.h
|
||||
* @brief Exception thrown when given Infeasible Initial Values.
|
||||
* @date jan 24, 2015
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace gtsam {
|
||||
/* ************************************************************************* */
|
||||
/** An exception indicating that the provided initial value is infeasible
|
||||
* Also used to inzdicatethat the noise model dimension passed into a
|
||||
* JacobianFactor has a different dimensionality than the factor. */
|
||||
class InfeasibleInitialValues: public ThreadsafeException<
|
||||
InfeasibleInitialValues> {
|
||||
public:
|
||||
InfeasibleInitialValues() {
|
||||
}
|
||||
|
||||
virtual ~InfeasibleInitialValues() throw () {
|
||||
}
|
||||
|
||||
virtual const char *what() const throw () {
|
||||
if (description_.empty())
|
||||
description_ =
|
||||
"An infeasible initial value was provided for the solver.\n";
|
||||
return description_.c_str();
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::string description_;
|
||||
};
|
||||
}
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file InfeasibleOrUnboundedProblem.h
|
||||
* @brief Throw when the problem is either infeasible or unbounded
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 1/24/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class InfeasibleOrUnboundedProblem: public ThreadsafeException<
|
||||
InfeasibleOrUnboundedProblem> {
|
||||
public:
|
||||
InfeasibleOrUnboundedProblem() {
|
||||
}
|
||||
virtual ~InfeasibleOrUnboundedProblem() throw () {
|
||||
}
|
||||
|
||||
virtual const char* what() const throw () {
|
||||
if (description_.empty())
|
||||
description_ = "The problem is either infeasible or unbounded.\n";
|
||||
return description_.c_str();
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::string description_;
|
||||
};
|
||||
}
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LP.h
|
||||
* @brief Struct used to hold a Linear Programming Problem
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 1/24/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/linear/LinearCost.h>
|
||||
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/// Mapping between variable's key and its corresponding dimensionality
|
||||
using KeyDimMap = std::map<Key, size_t>;
|
||||
/*
|
||||
* Iterates through every factor in a linear graph and generates a
|
||||
* mapping between every factor key and it's corresponding dimensionality.
|
||||
*/
|
||||
template <class LinearGraph>
|
||||
KeyDimMap collectKeyDim(const LinearGraph& linearGraph) {
|
||||
KeyDimMap keyDimMap;
|
||||
for (const typename LinearGraph::sharedFactor& factor : linearGraph) {
|
||||
if (!factor) continue;
|
||||
for (Key key : factor->keys())
|
||||
keyDimMap[key] = factor->getDim(factor->find(key));
|
||||
}
|
||||
return keyDimMap;
|
||||
}
|
||||
|
||||
/**
|
||||
* Data structure of a Linear Program
|
||||
*/
|
||||
struct LP {
|
||||
using shared_ptr = boost::shared_ptr<LP>;
|
||||
|
||||
LinearCost cost; //!< Linear cost factor
|
||||
EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0
|
||||
InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0
|
||||
private:
|
||||
mutable KeyDimMap cachedConstrainedKeyDimMap_; //!< cached key-dim map of all variables in the constraints
|
||||
|
||||
public:
|
||||
/// check feasibility
|
||||
bool isFeasible(const VectorValues& x) const {
|
||||
return (equalities.error(x) == 0 && inequalities.error(x) == 0);
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const string& s = "") const {
|
||||
std::cout << s << std::endl;
|
||||
cost.print("Linear cost: ");
|
||||
equalities.print("Linear equality factors: ");
|
||||
inequalities.print("Linear inequality factors: ");
|
||||
}
|
||||
|
||||
/// equals
|
||||
bool equals(const LP& other, double tol = 1e-9) const {
|
||||
return cost.equals(other.cost) && equalities.equals(other.equalities)
|
||||
&& inequalities.equals(other.inequalities);
|
||||
}
|
||||
|
||||
const KeyDimMap& constrainedKeyDimMap() const {
|
||||
if (!cachedConstrainedKeyDimMap_.empty())
|
||||
return cachedConstrainedKeyDimMap_;
|
||||
// Collect key-dim map of all variables in the constraints
|
||||
cachedConstrainedKeyDimMap_ = collectKeyDim(equalities);
|
||||
KeyDimMap keysDim2 = collectKeyDim(inequalities);
|
||||
cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end());
|
||||
return cachedConstrainedKeyDimMap_;
|
||||
}
|
||||
|
||||
Vector costGradient(Key key, const VectorValues& delta) const {
|
||||
Vector g = Vector::Zero(delta.at(key).size());
|
||||
Factor::const_iterator it = cost.find(key);
|
||||
if (it != cost.end()) g = cost.getA(it).transpose();
|
||||
return g;
|
||||
}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LP> : public Testable<LP> {
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LPInitSolver.h
|
||||
* @brief This finds a feasible solution for an LP problem
|
||||
* @author Duy Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 6/16/16
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/LPInitSolver.h>
|
||||
#include <gtsam_unstable/linear/LPSolver.h>
|
||||
#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/******************************************************************************/
|
||||
VectorValues LPInitSolver::solve() const {
|
||||
// Build the graph to solve for the initial value of the initial problem
|
||||
GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph();
|
||||
VectorValues x0 = initOfInitGraph->optimize();
|
||||
double y0 = compute_y0(x0);
|
||||
Key yKey = maxKey(lp_) + 1; // the unique key for y0
|
||||
VectorValues xy0(x0);
|
||||
xy0.insert(yKey, Vector::Constant(1, y0));
|
||||
|
||||
// Formulate and solve the initial LP
|
||||
LP::shared_ptr initLP = buildInitialLP(yKey);
|
||||
|
||||
// solve the initialLP
|
||||
LPSolver lpSolveInit(*initLP);
|
||||
VectorValues xyInit = lpSolveInit.optimize(xy0).first;
|
||||
double yOpt = xyInit.at(yKey)[0];
|
||||
xyInit.erase(yKey);
|
||||
if (yOpt > 0)
|
||||
throw InfeasibleOrUnboundedProblem();
|
||||
else
|
||||
return xyInit;
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
LP::shared_ptr LPInitSolver::buildInitialLP(Key yKey) const {
|
||||
LP::shared_ptr initLP(new LP());
|
||||
initLP->cost = LinearCost(yKey, I_1x1); // min y
|
||||
initLP->equalities = lp_.equalities; // st. Ax = b
|
||||
initLP->inequalities =
|
||||
addSlackVariableToInequalities(yKey,
|
||||
lp_.inequalities); // Cx-y <= d
|
||||
return initLP;
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const {
|
||||
// first add equality constraints Ax = b
|
||||
GaussianFactorGraph::shared_ptr initGraph(
|
||||
new GaussianFactorGraph(lp_.equalities));
|
||||
|
||||
// create factor ||x||^2 and add to the graph
|
||||
const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
|
||||
for (Key key : constrainedKeyDim | boost::adaptors::map_keys) {
|
||||
size_t dim = constrainedKeyDim.at(key);
|
||||
initGraph->push_back(
|
||||
JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
|
||||
}
|
||||
return initGraph;
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
double LPInitSolver::compute_y0(const VectorValues& x0) const {
|
||||
double y0 = -std::numeric_limits<double>::infinity();
|
||||
for (const auto& factor : lp_.inequalities) {
|
||||
double error = factor->error(x0);
|
||||
if (error > y0) y0 = error;
|
||||
}
|
||||
return y0;
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
std::vector<std::pair<Key, Matrix> > LPInitSolver::collectTerms(
|
||||
const LinearInequality& factor) const {
|
||||
std::vector<std::pair<Key, Matrix> > terms;
|
||||
for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
|
||||
terms.push_back(make_pair(*it, factor.getA(it)));
|
||||
}
|
||||
return terms;
|
||||
}
|
||||
|
||||
/******************************************************************************/
|
||||
InequalityFactorGraph LPInitSolver::addSlackVariableToInequalities(
|
||||
Key yKey, const InequalityFactorGraph& inequalities) const {
|
||||
InequalityFactorGraph slackInequalities;
|
||||
for (const auto& factor : lp_.inequalities) {
|
||||
std::vector<std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
|
||||
terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y
|
||||
double d = factor->getb()[0];
|
||||
slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey()));
|
||||
}
|
||||
return slackInequalities;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LPInitSolver.h
|
||||
* @brief This LPInitSolver implements the strategy in Matlab.
|
||||
* @author Duy Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 1/24/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/linear/LP.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <CppUnitLite/Test.h>
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* This LPInitSolver implements the strategy in Matlab:
|
||||
* http://www.mathworks.com/help/optim/ug/linear-programming-algorithms.html#brozyzb-9
|
||||
* Solve for x and y:
|
||||
* min y
|
||||
* st Ax = b
|
||||
* Cx - y <= d
|
||||
* where y \in R, x \in R^n, and Ax = b and Cx <= d is the constraints of the original problem.
|
||||
*
|
||||
* If the solution for this problem {x*,y*} has y* <= 0, we'll have x* a feasible initial point
|
||||
* of the original problem
|
||||
* otherwise, if y* > 0 or the problem has no solution, the original problem is infeasible.
|
||||
*
|
||||
* The initial value of this initial problem can be found by solving
|
||||
* min ||x||^2
|
||||
* s.t. Ax = b
|
||||
* to have a solution x0
|
||||
* then y = max_j ( Cj*x0 - dj ) -- due to the constraints y >= Cx - d
|
||||
*
|
||||
* WARNING: If some xj in the inequality constraints does not exist in the equality constraints,
|
||||
* set them as zero for now. If that is the case, the original problem doesn't have a unique
|
||||
* solution (it could be either infeasible or unbounded).
|
||||
* So, if the initialization fails because we enforce xj=0 in the problematic
|
||||
* inequality constraint, we can't conclude that the problem is infeasible.
|
||||
* However, whether it is infeasible or unbounded, we don't have a unique solution anyway.
|
||||
*/
|
||||
class LPInitSolver {
|
||||
private:
|
||||
const LP& lp_;
|
||||
|
||||
public:
|
||||
/// Construct with an LP problem
|
||||
LPInitSolver(const LP& lp) : lp_(lp) {}
|
||||
|
||||
///@return a feasible initialization point
|
||||
VectorValues solve() const;
|
||||
|
||||
private:
|
||||
/// build initial LP
|
||||
LP::shared_ptr buildInitialLP(Key yKey) const;
|
||||
|
||||
/**
|
||||
* Build the following graph to solve for an initial value of the initial problem
|
||||
* min ||x||^2 s.t. Ax = b
|
||||
*/
|
||||
GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const;
|
||||
|
||||
/// y = max_j ( Cj*x0 - dj ) -- due to the inequality constraints y >= Cx - d
|
||||
double compute_y0(const VectorValues& x0) const;
|
||||
|
||||
/// Collect all terms of a factor into a container.
|
||||
std::vector<std::pair<Key, Matrix>> collectTerms(
|
||||
const LinearInequality& factor) const;
|
||||
|
||||
/// Turn Cx <= d into Cx - y <= d factors
|
||||
InequalityFactorGraph addSlackVariableToInequalities(Key yKey,
|
||||
const InequalityFactorGraph& inequalities) const;
|
||||
|
||||
// friend class for unit-testing private methods
|
||||
FRIEND_TEST(LPInitSolver, initialization);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LPSolver.cpp
|
||||
* @brief
|
||||
* @author Duy Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 1/26/16
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/LPSolver.h>
|
||||
|
||||
namespace gtsam {
|
||||
constexpr double LPPolicy::maxAlpha;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LPSolver.h
|
||||
* @brief Policy of ActiveSetSolver to solve Linear Programming Problems
|
||||
* @author Duy Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 6/16/16
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/LP.h>
|
||||
#include <gtsam_unstable/linear/ActiveSetSolver.h>
|
||||
#include <gtsam_unstable/linear/LPInitSolver.h>
|
||||
|
||||
#include <limits>
|
||||
#include <algorithm>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Policy for ActivetSetSolver to solve Linear Programming \sa LP problems
|
||||
struct LPPolicy {
|
||||
/// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient
|
||||
/// For LP, maxAlpha = Infinity
|
||||
static constexpr double maxAlpha = std::numeric_limits<double>::infinity();
|
||||
|
||||
/**
|
||||
* Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution
|
||||
* on the constraint surface and g is the gradient of the linear cost,
|
||||
* i.e. -g is the direction we wish to follow to decrease the cost.
|
||||
*
|
||||
* Essentially, we try to match the direction d = x-xk with -g as much as possible
|
||||
* subject to the condition that x needs to be on the constraint surface, i.e., d is
|
||||
* along the surface's subspace.
|
||||
*
|
||||
* The least-square solution of this quadratic subject to a set of linear constraints
|
||||
* is the projection of the gradient onto the constraints' subspace
|
||||
*/
|
||||
static GaussianFactorGraph buildCostFunction(const LP& lp,
|
||||
const VectorValues& xk) {
|
||||
GaussianFactorGraph graph;
|
||||
for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
|
||||
++it) {
|
||||
size_t dim = lp.cost.getDim(it);
|
||||
Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g
|
||||
graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
|
||||
}
|
||||
|
||||
KeySet allKeys = lp.inequalities.keys();
|
||||
allKeys.merge(lp.equalities.keys());
|
||||
allKeys.merge(KeySet(lp.cost.keys()));
|
||||
// Add corresponding factors for all variables that are not explicitly in
|
||||
// the cost function. Gradients of the cost function wrt to these variables
|
||||
// are zero (g=0), so b=xk
|
||||
if (lp.cost.keys().size() != allKeys.size()) {
|
||||
KeySet difference;
|
||||
std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(),
|
||||
lp.cost.end(),
|
||||
std::inserter(difference, difference.end()));
|
||||
for (Key k : difference) {
|
||||
size_t dim = lp.constrainedKeyDimMap().at(k);
|
||||
graph.push_back(
|
||||
JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
|
||||
}
|
||||
}
|
||||
return graph;
|
||||
}
|
||||
};
|
||||
|
||||
using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LinearCost.h
|
||||
* @brief LinearCost derived from JacobianFactor to support linear cost functions c'x
|
||||
* @date Nov 27, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef Eigen::RowVectorXd RowVector;
|
||||
|
||||
/**
|
||||
* This class defines a linear cost function c'x
|
||||
* which is a JacobianFactor with only one row
|
||||
*/
|
||||
class LinearCost: public JacobianFactor {
|
||||
public:
|
||||
typedef LinearCost This; ///< Typedef to this class
|
||||
typedef JacobianFactor Base; ///< Typedef to base class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
|
||||
|
||||
public:
|
||||
/** default constructor for I/O */
|
||||
LinearCost() :
|
||||
Base() {
|
||||
}
|
||||
|
||||
/** Conversion from HessianFactor */
|
||||
explicit LinearCost(const HessianFactor& hf) {
|
||||
throw std::runtime_error("Cannot convert HessianFactor to LinearCost");
|
||||
}
|
||||
|
||||
/** Conversion from JacobianFactor */
|
||||
explicit LinearCost(const JacobianFactor& jf) :
|
||||
Base(jf) {
|
||||
if (jf.isConstrained()) {
|
||||
throw std::runtime_error(
|
||||
"Cannot convert a constrained JacobianFactor to LinearCost");
|
||||
}
|
||||
|
||||
if (jf.get_model()->dim() != 1) {
|
||||
throw std::runtime_error(
|
||||
"Only support single-valued linear cost factor!");
|
||||
}
|
||||
}
|
||||
|
||||
/** Construct unary factor */
|
||||
LinearCost(Key i1, const RowVector& A1) :
|
||||
Base(i1, A1, Vector1::Zero()) {
|
||||
}
|
||||
|
||||
/** Construct binary factor */
|
||||
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) :
|
||||
Base(i1, A1, i2, A2, Vector1::Zero()) {
|
||||
}
|
||||
|
||||
/** Construct ternary factor */
|
||||
LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
|
||||
const RowVector& A3) :
|
||||
Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) {
|
||||
}
|
||||
|
||||
/** Construct an n-ary factor
|
||||
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
|
||||
* collection of keys and matrices making up the factor. */
|
||||
template<typename TERMS>
|
||||
LinearCost(const TERMS& terms) :
|
||||
Base(terms, Vector1::Zero()) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~LinearCost() {
|
||||
}
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const {
|
||||
return Base::equals(lf, tol);
|
||||
}
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s = "", const KeyFormatter& formatter =
|
||||
DefaultKeyFormatter) const {
|
||||
Base::print(s + " LinearCost: ", formatter);
|
||||
}
|
||||
|
||||
/** Clone this LinearCost */
|
||||
virtual GaussianFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast < GaussianFactor
|
||||
> (boost::make_shared < LinearCost > (*this));
|
||||
}
|
||||
|
||||
/** Special error_vector for constraints (A*x-b) */
|
||||
Vector error_vector(const VectorValues& c) const {
|
||||
return unweighted_error(c);
|
||||
}
|
||||
|
||||
/** Special error for single-valued inequality constraints. */
|
||||
virtual double error(const VectorValues& c) const {
|
||||
return error_vector(c)[0];
|
||||
}
|
||||
};
|
||||
// \ LinearCost
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LinearCost> : public Testable<LinearCost> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
@ -9,11 +9,11 @@
|
|||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* LinearEquality.h
|
||||
* @brief: LinearEquality derived from Base with constrained noise model
|
||||
* @date: Nov 27, 2014
|
||||
* @author: thduynguyen
|
||||
/**
|
||||
* @file LinearEquality.h
|
||||
* @brief LinearEquality derived from Base with constrained noise model
|
||||
* @date Nov 27, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -23,7 +23,7 @@
|
|||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This class defines Linear constraints by inherit Base
|
||||
* This class defines a linear equality constraints, inheriting JacobianFactor
|
||||
* with the special Constrained noise model
|
||||
*/
|
||||
class LinearEquality: public JacobianFactor {
|
||||
|
|
@ -41,6 +41,17 @@ public:
|
|||
Base() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Construct from a constrained noisemodel JacobianFactor with a dual key.
|
||||
*/
|
||||
explicit LinearEquality(const JacobianFactor& jf, Key dualKey) :
|
||||
Base(jf), dualKey_(dualKey) {
|
||||
if (!jf.isConstrained()) {
|
||||
throw std::runtime_error(
|
||||
"Cannot convert an unconstrained JacobianFactor to LinearEquality");
|
||||
}
|
||||
}
|
||||
|
||||
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
|
||||
explicit LinearEquality(const HessianFactor& hf) {
|
||||
throw std::runtime_error("Cannot convert HessianFactor to LinearEquality");
|
||||
|
|
@ -90,15 +101,19 @@ public:
|
|||
|
||||
/** Clone this LinearEquality */
|
||||
virtual GaussianFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<GaussianFactor>(
|
||||
boost::make_shared<LinearEquality>(*this));
|
||||
return boost::static_pointer_cast < GaussianFactor
|
||||
> (boost::make_shared < LinearEquality > (*this));
|
||||
}
|
||||
|
||||
/// dual key
|
||||
Key dualKey() const { return dualKey_; }
|
||||
Key dualKey() const {
|
||||
return dualKey_;
|
||||
}
|
||||
|
||||
/// for active set method: equality constraints are always active
|
||||
bool active() const { return true; }
|
||||
bool active() const {
|
||||
return true;
|
||||
}
|
||||
|
||||
/** Special error_vector for constraints (A*x-b) */
|
||||
Vector error_vector(const VectorValues& c) const {
|
||||
|
|
@ -113,11 +128,12 @@ public:
|
|||
return 0.0;
|
||||
}
|
||||
|
||||
}; // \ LinearEquality
|
||||
|
||||
};
|
||||
// \ LinearEquality
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LinearEquality> : public Testable<LinearEquality> {};
|
||||
template<> struct traits<LinearEquality> : public Testable<LinearEquality> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -1,37 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* LinearEqualityFactorGraph.h
|
||||
* @brief: Factor graph of all LinearEquality factors
|
||||
* @date: Dec 8, 2014
|
||||
* @author: thduynguyen
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam_unstable/linear/LinearEquality.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class LinearEqualityFactorGraph : public FactorGraph<LinearEquality> {
|
||||
public:
|
||||
typedef boost::shared_ptr<LinearEqualityFactorGraph> shared_ptr;
|
||||
};
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LinearEqualityFactorGraph> : public Testable<
|
||||
LinearEqualityFactorGraph> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
@ -9,24 +9,26 @@
|
|||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* LinearInequality.h
|
||||
* @brief: LinearInequality derived from Base with constrained noise model
|
||||
* @date: Nov 27, 2014
|
||||
* @author: thduynguyen
|
||||
/**
|
||||
* @file LinearInequality.h
|
||||
* @brief LinearInequality derived from Base with constrained noise model
|
||||
* @date Nov 27, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef Eigen::RowVectorXd RowVector;
|
||||
|
||||
/**
|
||||
* This class defines Linear constraints by inherit Base
|
||||
* with the special Constrained noise model
|
||||
* This class defines a linear inequality constraint Ax-b <= 0,
|
||||
* inheriting JacobianFactor with the special Constrained noise model
|
||||
*/
|
||||
class LinearInequality: public JacobianFactor {
|
||||
public:
|
||||
|
|
@ -44,35 +46,49 @@ public:
|
|||
Base(), active_(true) {
|
||||
}
|
||||
|
||||
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
|
||||
/** Conversion from HessianFactor */
|
||||
explicit LinearInequality(const HessianFactor& hf) {
|
||||
throw std::runtime_error(
|
||||
"Cannot convert HessianFactor to LinearInequality");
|
||||
}
|
||||
|
||||
/** Conversion from JacobianFactor */
|
||||
explicit LinearInequality(const JacobianFactor& jf, Key dualKey) :
|
||||
Base(jf), dualKey_(dualKey), active_(true) {
|
||||
if (!jf.isConstrained()) {
|
||||
throw std::runtime_error(
|
||||
"Cannot convert an unconstrained JacobianFactor to LinearInequality");
|
||||
}
|
||||
|
||||
if (jf.get_model()->dim() != 1) {
|
||||
throw std::runtime_error("Only support single-valued inequality factor!");
|
||||
}
|
||||
}
|
||||
|
||||
/** Construct unary factor */
|
||||
LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) :
|
||||
Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
|
||||
dualKey), active_(true) {
|
||||
Base(i1, A1, (Vector(1) << b).finished(),
|
||||
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
|
||||
}
|
||||
|
||||
/** Construct binary factor */
|
||||
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b,
|
||||
Key dualKey) :
|
||||
Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
|
||||
dualKey), active_(true) {
|
||||
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
|
||||
double b, Key dualKey) :
|
||||
Base(i1, A1, i2, A2, (Vector(1) << b).finished(),
|
||||
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
|
||||
}
|
||||
|
||||
/** Construct ternary factor */
|
||||
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3,
|
||||
const RowVector& A3, double b, Key dualKey) :
|
||||
LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2,
|
||||
Key i3, const RowVector& A3, double b, Key dualKey) :
|
||||
Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(),
|
||||
noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) {
|
||||
}
|
||||
|
||||
/** Construct an n-ary factor
|
||||
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
|
||||
* collection of keys and matrices making up the factor. */
|
||||
* collection of keys and matrices making up the factor.
|
||||
* In this inequality factor, each matrix must have only one row!! */
|
||||
template<typename TERMS>
|
||||
LinearInequality(const TERMS& terms, double b, Key dualKey) :
|
||||
Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(
|
||||
|
|
@ -99,21 +115,29 @@ public:
|
|||
|
||||
/** Clone this LinearInequality */
|
||||
virtual GaussianFactor::shared_ptr clone() const {
|
||||
return boost::static_pointer_cast<GaussianFactor>(
|
||||
boost::make_shared<LinearInequality>(*this));
|
||||
return boost::static_pointer_cast < GaussianFactor
|
||||
> (boost::make_shared < LinearInequality > (*this));
|
||||
}
|
||||
|
||||
/// dual key
|
||||
Key dualKey() const { return dualKey_; }
|
||||
Key dualKey() const {
|
||||
return dualKey_;
|
||||
}
|
||||
|
||||
/// return true if this constraint is active
|
||||
bool active() const { return active_; }
|
||||
bool active() const {
|
||||
return active_;
|
||||
}
|
||||
|
||||
/// Make this inequality constraint active
|
||||
void activate() { active_ = true; }
|
||||
void activate() {
|
||||
active_ = true;
|
||||
}
|
||||
|
||||
/// Make this inequality constraint inactive
|
||||
void inactivate() { active_ = false; }
|
||||
void inactivate() {
|
||||
active_ = false;
|
||||
}
|
||||
|
||||
/** Special error_vector for constraints (A*x-b) */
|
||||
Vector error_vector(const VectorValues& c) const {
|
||||
|
|
@ -136,10 +160,12 @@ public:
|
|||
return aTp;
|
||||
}
|
||||
|
||||
}; // \ LinearInequality
|
||||
};
|
||||
// \ LinearInequality
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LinearInequality> : public Testable<LinearInequality> {};
|
||||
template<> struct traits<LinearInequality> : public Testable<LinearInequality> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -1,52 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* LinearInequalityFactorGraph.h
|
||||
* @brief: Factor graph of all LinearInequality factors
|
||||
* @date: Dec 8, 2014
|
||||
* @author: thduynguyen
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam_unstable/linear/LinearInequality.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class LinearInequalityFactorGraph: public FactorGraph<LinearInequality> {
|
||||
private:
|
||||
typedef FactorGraph<LinearInequality> Base;
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<LinearInequalityFactorGraph> shared_ptr;
|
||||
|
||||
/** print */
|
||||
void print(const std::string& str, const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
Base::print(str, keyFormatter);
|
||||
}
|
||||
|
||||
/** equals */
|
||||
bool equals(const LinearInequalityFactorGraph& other,
|
||||
double tol = 1e-9) const {
|
||||
return Base::equals(other, tol);
|
||||
}
|
||||
};
|
||||
|
||||
/// traits
|
||||
template<> struct traits<LinearInequalityFactorGraph> : public Testable<
|
||||
LinearInequalityFactorGraph> {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
||||
|
|
@ -9,29 +9,34 @@
|
|||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* QP.h
|
||||
* @brief: Factor graphs of a Quadratic Programming problem
|
||||
* @date: Dec 8, 2014
|
||||
* @author: thduynguyen
|
||||
/**
|
||||
* @file QP.h
|
||||
* @brief Factor graphs of a Quadratic Programming problem
|
||||
* @date Dec 8, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/LinearInequalityFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* struct contains factor graphs of a Quadratic Programming problem
|
||||
* Struct contains factor graphs of a Quadratic Programming problem
|
||||
*/
|
||||
struct QP {
|
||||
GaussianFactorGraph cost; //!< Quadratic cost factors
|
||||
LinearEqualityFactorGraph equalities; //!< linear equality constraints
|
||||
LinearInequalityFactorGraph inequalities; //!< linear inequality constraints
|
||||
EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0
|
||||
InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0
|
||||
|
||||
private:
|
||||
mutable VariableIndex cachedCostVariableIndex_;
|
||||
|
||||
public:
|
||||
/** default constructor */
|
||||
QP() :
|
||||
cost(), equalities(), inequalities() {
|
||||
|
|
@ -39,8 +44,8 @@ struct QP {
|
|||
|
||||
/** constructor */
|
||||
QP(const GaussianFactorGraph& _cost,
|
||||
const LinearEqualityFactorGraph& _linearEqualities,
|
||||
const LinearInequalityFactorGraph& _linearInequalities) :
|
||||
const EqualityFactorGraph& _linearEqualities,
|
||||
const InequalityFactorGraph& _linearInequalities) :
|
||||
cost(_cost), equalities(_linearEqualities), inequalities(
|
||||
_linearInequalities) {
|
||||
}
|
||||
|
|
@ -52,6 +57,23 @@ struct QP {
|
|||
equalities.print("Linear equality factors: ");
|
||||
inequalities.print("Linear inequality factors: ");
|
||||
}
|
||||
|
||||
const VariableIndex& costVariableIndex() const {
|
||||
if (cachedCostVariableIndex_.size() == 0)
|
||||
cachedCostVariableIndex_ = VariableIndex(cost);
|
||||
return cachedCostVariableIndex_;
|
||||
}
|
||||
|
||||
Vector costGradient(Key key, const VectorValues& delta) const {
|
||||
Vector g = Vector::Zero(delta.at(key).size());
|
||||
if (costVariableIndex().find(key) != costVariableIndex().end()) {
|
||||
for (size_t factorIx : costVariableIndex()[key]) {
|
||||
GaussianFactor::shared_ptr factor = cost.at(factorIx);
|
||||
g += factor->gradient(key, delta);
|
||||
}
|
||||
}
|
||||
return g;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,54 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file QPInitSolver.h
|
||||
* @brief This finds a feasible solution for a QP problem
|
||||
* @author Duy Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 6/16/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/linear/LPInitSolver.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This class finds a feasible solution for a QP problem.
|
||||
* This uses the Matlab strategy for initialization
|
||||
* For details, see
|
||||
* http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22
|
||||
*/
|
||||
class QPInitSolver {
|
||||
const QP& qp_;
|
||||
public:
|
||||
/// Constructor with a QP problem
|
||||
QPInitSolver(const QP& qp) : qp_(qp) {}
|
||||
|
||||
///@return a feasible initialization point
|
||||
VectorValues solve() const {
|
||||
// Make an LP with any linear cost function. It doesn't matter for
|
||||
// initialization.
|
||||
LP initProblem;
|
||||
// make an unrelated key for a random variable cost
|
||||
Key newKey = maxKey(qp_) + 1;
|
||||
initProblem.cost = LinearCost(newKey, Vector::Ones(1));
|
||||
initProblem.equalities = qp_.equalities;
|
||||
initProblem.inequalities = qp_.inequalities;
|
||||
LPInitSolver initSolver(initProblem);
|
||||
return initSolver.solve();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file QPParser.cpp
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 3/5/16
|
||||
*/
|
||||
|
||||
#define BOOST_SPIRIT_USE_PHOENIX_V3 1
|
||||
|
||||
#include <gtsam_unstable/linear/QPSParser.h>
|
||||
#include <gtsam_unstable/linear/QPSParserException.h>
|
||||
#include <gtsam_unstable/linear/RawQP.h>
|
||||
|
||||
#include <boost/spirit/include/qi.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#include <boost/phoenix/bind.hpp>
|
||||
#include <boost/spirit/include/classic.hpp>
|
||||
|
||||
namespace bf = boost::fusion;
|
||||
namespace qi = boost::spirit::qi;
|
||||
|
||||
namespace gtsam {
|
||||
typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar;
|
||||
|
||||
struct QPSParser::MPSGrammar: base_grammar {
|
||||
typedef std::vector<char> Chars;
|
||||
RawQP * rqp_;
|
||||
boost::function<void(bf::vector<Chars, Chars, Chars> const&)> setName;
|
||||
boost::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)> addRow;
|
||||
boost::function<
|
||||
void(bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)> rhsSingle;
|
||||
boost::function<
|
||||
void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars, Chars,
|
||||
Chars, double>)> rhsDouble;
|
||||
boost::function<
|
||||
void(bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars>)> colSingle;
|
||||
boost::function<
|
||||
void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, double, Chars, Chars, Chars,
|
||||
double> const &)> colDouble;
|
||||
boost::function<
|
||||
void(bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)> addQuadTerm;
|
||||
boost::function<
|
||||
void(
|
||||
bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars, double> const &)> addBound;
|
||||
boost::function<
|
||||
void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)> addBoundFr;
|
||||
MPSGrammar(RawQP * rqp) :
|
||||
base_grammar(start), rqp_(rqp), setName(
|
||||
boost::bind(&RawQP::setName, rqp, ::_1)), addRow(
|
||||
boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle(
|
||||
boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble(
|
||||
boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), colSingle(
|
||||
boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble(
|
||||
boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm(
|
||||
boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound(
|
||||
boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr(
|
||||
boost::bind(&RawQP::addBoundFr, rqp, ::_1)) {
|
||||
using namespace boost::spirit;
|
||||
using namespace boost::spirit::qi;
|
||||
character = lexeme[alnum | '_' | '-' | '.'];
|
||||
title = lexeme[character >> *(blank | character)];
|
||||
word = lexeme[+character];
|
||||
name = lexeme[lit("NAME") >> *blank >> title >> +space][setName];
|
||||
row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow];
|
||||
rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_
|
||||
>> *blank][rhsSingle];
|
||||
rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_
|
||||
>> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank];
|
||||
col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_
|
||||
>> *blank][colSingle];
|
||||
col_double = lexeme[*blank
|
||||
>> (word >> +blank >> word >> +blank >> double_ >> +blank >> word
|
||||
>> +blank >> double_)[colDouble] >> *blank];
|
||||
quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_
|
||||
>> *blank][addQuadTerm];
|
||||
bound = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> +blank
|
||||
>> double_ >> *blank][addBound];
|
||||
bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word
|
||||
>> *blank][addBoundFr];
|
||||
rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)];
|
||||
rhs = lexeme[lit("RHS") >> *blank >> eol
|
||||
>> +((rhs_double | rhs_single) >> eol)];
|
||||
cols = lexeme[lit("COLUMNS") >> *blank >> eol
|
||||
>> +((col_double | col_single) >> eol)];
|
||||
quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)];
|
||||
bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)];
|
||||
ranges = lexeme[lit("RANGES") >> +space];
|
||||
end = lexeme[lit("ENDATA") >> *space];
|
||||
start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad
|
||||
>> end];
|
||||
}
|
||||
|
||||
qi::rule<boost::spirit::basic_istream_iterator<char>, char()> character;
|
||||
qi::rule<boost::spirit::basic_istream_iterator<char>, Chars()> word, title;
|
||||
qi::rule<boost::spirit::basic_istream_iterator<char> > row, end, col_single,
|
||||
col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad,
|
||||
quad_l, rows, cols, rhs, name, start;
|
||||
};
|
||||
|
||||
QP QPSParser::Parse() {
|
||||
RawQP rawData;
|
||||
std::fstream stream(fileName_.c_str());
|
||||
stream.unsetf(std::ios::skipws);
|
||||
boost::spirit::basic_istream_iterator<char> begin(stream);
|
||||
boost::spirit::basic_istream_iterator<char> last;
|
||||
|
||||
if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) {
|
||||
throw QPSParserException();
|
||||
}
|
||||
|
||||
return rawData.makeQP();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file QPParser.h
|
||||
* @brief QPS parser implementation
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 3/5/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/linear/QP.h>
|
||||
#include <fstream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class QPSParser {
|
||||
|
||||
private:
|
||||
std::string fileName_;
|
||||
struct MPSGrammar;
|
||||
public:
|
||||
|
||||
QPSParser(const std::string& fileName) :
|
||||
fileName_(findExampleDataFile(fileName)) {
|
||||
}
|
||||
|
||||
QP Parse();
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file QPSParserException.h
|
||||
* @brief Exception thrown if there is an error parsing a QPS file
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 3/5/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class QPSParserException: public ThreadsafeException<QPSParserException> {
|
||||
public:
|
||||
QPSParserException() {
|
||||
}
|
||||
|
||||
virtual ~QPSParserException() throw () {
|
||||
}
|
||||
|
||||
virtual const char *what() const throw () {
|
||||
if (description_.empty())
|
||||
description_ = "There is a problem parsing the QPS file.\n";
|
||||
return description_.c_str();
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::string description_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -1,252 +1,23 @@
|
|||
/*
|
||||
* QPSolver.cpp
|
||||
* @brief:
|
||||
* @date: Apr 15, 2014
|
||||
* @author: thduynguyen
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file QPSolver.cpp
|
||||
* @brief
|
||||
* @date Apr 15, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam_unstable/linear/QPSolver.h>
|
||||
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//******************************************************************************
|
||||
QPSolver::QPSolver(const QP& qp) : qp_(qp) {
|
||||
baseGraph_ = qp_.cost;
|
||||
baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end());
|
||||
costVariableIndex_ = VariableIndex(qp_.cost);
|
||||
equalityVariableIndex_ = VariableIndex(qp_.equalities);
|
||||
inequalityVariableIndex_ = VariableIndex(qp_.inequalities);
|
||||
constrainedKeys_ = qp_.equalities.keys();
|
||||
constrainedKeys_.merge(qp_.inequalities.keys());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
VectorValues QPSolver::solveWithCurrentWorkingSet(
|
||||
const LinearInequalityFactorGraph& workingSet) const {
|
||||
GaussianFactorGraph workingGraph = baseGraph_;
|
||||
for(const LinearInequality::shared_ptr& factor: workingSet) {
|
||||
if (factor->active())
|
||||
workingGraph.push_back(factor);
|
||||
}
|
||||
return workingGraph.optimize();
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
|
||||
const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const {
|
||||
|
||||
// Transpose the A matrix of constrained factors to have the jacobian of the dual key
|
||||
std::vector<std::pair<Key, Matrix> > Aterms = collectDualJacobians
|
||||
< LinearEquality > (key, qp_.equalities, equalityVariableIndex_);
|
||||
std::vector<std::pair<Key, Matrix> > AtermsInequalities = collectDualJacobians
|
||||
< LinearInequality > (key, workingSet, inequalityVariableIndex_);
|
||||
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
|
||||
AtermsInequalities.end());
|
||||
|
||||
// Collect the gradients of unconstrained cost factors to the b vector
|
||||
if (Aterms.size() > 0) {
|
||||
Vector b = Vector::Zero(delta.at(key).size());
|
||||
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
|
||||
for(size_t factorIx: costVariableIndex_[key]) {
|
||||
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
|
||||
b += factor->gradient(key, delta);
|
||||
}
|
||||
}
|
||||
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows()));
|
||||
}
|
||||
else {
|
||||
return boost::make_shared<JacobianFactor>();
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
|
||||
const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const {
|
||||
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
|
||||
for(Key key: constrainedKeys_) {
|
||||
// Each constrained key becomes a factor in the dual graph
|
||||
JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta);
|
||||
if (!dualFactor->empty())
|
||||
dualGraph->push_back(dualFactor);
|
||||
}
|
||||
return dualGraph;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int QPSolver::identifyLeavingConstraint(
|
||||
const LinearInequalityFactorGraph& workingSet,
|
||||
const VectorValues& lambdas) const {
|
||||
int worstFactorIx = -1;
|
||||
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
|
||||
// inactive or a good inequality constraint, so we don't care!
|
||||
double maxLambda = 0.0;
|
||||
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
|
||||
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
|
||||
if (factor->active()) {
|
||||
double lambda = lambdas.at(factor->dualKey())[0];
|
||||
if (lambda > maxLambda) {
|
||||
worstFactorIx = factorIx;
|
||||
maxLambda = lambda;
|
||||
}
|
||||
}
|
||||
}
|
||||
return worstFactorIx;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints
|
||||
* If some inactive inequality constraints complain about the full step (alpha = 1),
|
||||
* we have to adjust alpha to stay within the inequality constraints' feasible regions.
|
||||
*
|
||||
* For each inactive inequality j:
|
||||
* - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints
|
||||
* - We want: aj'*(xk + alpha*p) - bj <= 0
|
||||
* - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
|
||||
* it's good!
|
||||
* - We only care when aj'*p > 0. In this case, we need to choose alpha so that
|
||||
* aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p)
|
||||
* We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
|
||||
*
|
||||
* We want the minimum of all those alphas among all inactive inequality.
|
||||
*/
|
||||
boost::tuple<double, int> QPSolver::computeStepSize(
|
||||
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||
const VectorValues& p) const {
|
||||
static bool debug = false;
|
||||
|
||||
double minAlpha = 1.0;
|
||||
int closestFactorIx = -1;
|
||||
for(size_t factorIx = 0; factorIx<workingSet.size(); ++factorIx) {
|
||||
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
|
||||
double b = factor->getb()[0];
|
||||
// only check inactive factors
|
||||
if (!factor->active()) {
|
||||
// Compute a'*p
|
||||
double aTp = factor->dotProductRow(p);
|
||||
|
||||
// Check if a'*p >0. Don't care if it's not.
|
||||
if (aTp <= 0)
|
||||
continue;
|
||||
|
||||
// Compute a'*xk
|
||||
double aTx = factor->dotProductRow(xk);
|
||||
|
||||
// alpha = (b - a'*xk) / (a'*p)
|
||||
double alpha = (b - aTx) / aTp;
|
||||
if (debug)
|
||||
cout << "alpha: " << alpha << endl;
|
||||
|
||||
// We want the minimum of all those max alphas
|
||||
if (alpha < minAlpha) {
|
||||
closestFactorIx = factorIx;
|
||||
minAlpha = alpha;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return boost::make_tuple(minAlpha, closestFactorIx);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
QPState QPSolver::iterate(const QPState& state) const {
|
||||
static bool debug = false;
|
||||
|
||||
// Solve with the current working set
|
||||
VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet);
|
||||
if (debug)
|
||||
newValues.print("New solution:");
|
||||
|
||||
// If we CAN'T move further
|
||||
if (newValues.equals(state.values, 1e-5)) {
|
||||
// Compute lambda from the dual graph
|
||||
if (debug)
|
||||
cout << "Building dual graph..." << endl;
|
||||
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues);
|
||||
if (debug)
|
||||
dualGraph->print("Dual graph: ");
|
||||
VectorValues duals = dualGraph->optimize();
|
||||
if (debug)
|
||||
duals.print("Duals :");
|
||||
|
||||
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
|
||||
if (debug)
|
||||
cout << "leavingFactor: " << leavingFactor << endl;
|
||||
|
||||
// If all inequality constraints are satisfied: We have the solution!!
|
||||
if (leavingFactor < 0) {
|
||||
return QPState(newValues, duals, state.workingSet, true);
|
||||
}
|
||||
else {
|
||||
// Inactivate the leaving constraint
|
||||
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
|
||||
newWorkingSet.at(leavingFactor)->inactivate();
|
||||
return QPState(newValues, duals, newWorkingSet, false);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// If we CAN make some progress
|
||||
// Adapt stepsize if some inactive constraints complain about this move
|
||||
double alpha;
|
||||
int factorIx;
|
||||
VectorValues p = newValues - state.values;
|
||||
boost::tie(alpha, factorIx) = //
|
||||
computeStepSize(state.workingSet, state.values, p);
|
||||
if (debug)
|
||||
cout << "alpha, factorIx: " << alpha << " " << factorIx << " "
|
||||
<< endl;
|
||||
|
||||
// also add to the working set the one that complains the most
|
||||
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
|
||||
if (factorIx >= 0)
|
||||
newWorkingSet.at(factorIx)->activate();
|
||||
|
||||
// step!
|
||||
newValues = state.values + alpha * p;
|
||||
|
||||
return QPState(newValues, state.duals, newWorkingSet, false);
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
|
||||
const LinearInequalityFactorGraph& inequalities,
|
||||
const VectorValues& initialValues) const {
|
||||
LinearInequalityFactorGraph workingSet;
|
||||
for(const LinearInequality::shared_ptr& factor: inequalities){
|
||||
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
|
||||
double error = workingFactor->error(initialValues);
|
||||
if (fabs(error)>1e-7){
|
||||
workingFactor->inactivate();
|
||||
} else {
|
||||
workingFactor->activate();
|
||||
}
|
||||
workingSet.push_back(workingFactor);
|
||||
}
|
||||
return workingSet;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
pair<VectorValues, VectorValues> QPSolver::optimize(
|
||||
const VectorValues& initialValues) const {
|
||||
|
||||
// Initialize workingSet from the feasible initialValues
|
||||
LinearInequalityFactorGraph workingSet =
|
||||
identifyActiveConstraints(qp_.inequalities, initialValues);
|
||||
QPState state(initialValues, VectorValues(), workingSet, false);
|
||||
|
||||
/// main loop of the solver
|
||||
while (!state.converged) {
|
||||
state = iterate(state);
|
||||
}
|
||||
|
||||
return make_pair(state.values, state.duals);
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
constexpr double QPPolicy::maxAlpha;
|
||||
}
|
||||
|
|
@ -1,188 +1,43 @@
|
|||
/*
|
||||
* QPSolver.h
|
||||
* @brief: A quadratic programming solver implements the active set method
|
||||
* @date: Apr 15, 2014
|
||||
* @author: thduynguyen
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file QPSolver.h
|
||||
* @brief Policy of ActiveSetSolver to solve Quadratic Programming Problems
|
||||
* @author Duy Nguyen Ta
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 6/16/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam_unstable/linear/QP.h>
|
||||
|
||||
#include <vector>
|
||||
#include <set>
|
||||
#include <gtsam_unstable/linear/ActiveSetSolver.h>
|
||||
#include <gtsam_unstable/linear/QPInitSolver.h>
|
||||
#include <limits>
|
||||
#include <algorithm>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// This struct holds the state of QPSolver at each iteration
|
||||
struct QPState {
|
||||
VectorValues values;
|
||||
VectorValues duals;
|
||||
LinearInequalityFactorGraph workingSet;
|
||||
bool converged;
|
||||
/// Policy for ActivetSetSolver to solve Linear Programming \sa QP problems
|
||||
struct QPPolicy {
|
||||
/// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient
|
||||
/// For QP, maxAlpha = 1 is the minimum point of the quadratic cost
|
||||
static constexpr double maxAlpha = 1.0;
|
||||
|
||||
/// default constructor
|
||||
QPState() :
|
||||
values(), duals(), workingSet(), converged(false) {
|
||||
}
|
||||
|
||||
/// constructor with initial values
|
||||
QPState(const VectorValues& initialValues, const VectorValues& initialDuals,
|
||||
const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) :
|
||||
values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
|
||||
_converged) {
|
||||
/// Simply the cost of the QP problem
|
||||
static const GaussianFactorGraph& buildCostFunction(
|
||||
const QP& qp, const VectorValues& xk = VectorValues()) {
|
||||
return qp.cost;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* This class implements the active set method to solve quadratic programming problems
|
||||
* encoded in a GaussianFactorGraph with special mixed constrained noise models, in which
|
||||
* a negative sigma denotes an inequality <=0 constraint,
|
||||
* a zero sigma denotes an equality =0 constraint,
|
||||
* and a positive sigma denotes a normal Gaussian noise model.
|
||||
*/
|
||||
class QPSolver {
|
||||
using QPSolver = ActiveSetSolver<QP, QPPolicy, QPInitSolver>;
|
||||
|
||||
const QP& qp_; //!< factor graphs of the QP problem, can't be modified!
|
||||
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process.
|
||||
VariableIndex costVariableIndex_, equalityVariableIndex_,
|
||||
inequalityVariableIndex_;
|
||||
KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
|
||||
|
||||
public:
|
||||
/// Constructor
|
||||
QPSolver(const QP& qp);
|
||||
|
||||
/// Find solution with the current working set
|
||||
VectorValues solveWithCurrentWorkingSet(
|
||||
const LinearInequalityFactorGraph& workingSet) const;
|
||||
|
||||
/// @name Build the dual graph
|
||||
/// @{
|
||||
|
||||
/// Collect the Jacobian terms for a dual factor
|
||||
template<typename FACTOR>
|
||||
std::vector<std::pair<Key, Matrix> > collectDualJacobians(Key key,
|
||||
const FactorGraph<FACTOR>& graph,
|
||||
const VariableIndex& variableIndex) const {
|
||||
std::vector<std::pair<Key, Matrix> > Aterms;
|
||||
if (variableIndex.find(key) != variableIndex.end()) {
|
||||
for(size_t factorIx: variableIndex[key]){
|
||||
typename FACTOR::shared_ptr factor = graph.at(factorIx);
|
||||
if (!factor->active()) continue;
|
||||
Matrix Ai = factor->getA(factor->find(key)).transpose();
|
||||
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
|
||||
}
|
||||
}
|
||||
return Aterms;
|
||||
}
|
||||
|
||||
/// Create a dual factor
|
||||
JacobianFactor::shared_ptr createDualFactor(Key key,
|
||||
const LinearInequalityFactorGraph& workingSet,
|
||||
const VectorValues& delta) const;
|
||||
|
||||
/**
|
||||
* Build the dual graph to solve for the Lagrange multipliers.
|
||||
*
|
||||
* The Lagrangian function is:
|
||||
* L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X),
|
||||
* where the unconstrained part is
|
||||
* f(X) = 0.5*X'*G*X - X'*g + 0.5*f0
|
||||
* and the linear equality constraints are
|
||||
* c1(X), c2(X), ..., cm(X)
|
||||
*
|
||||
* Take the derivative of L wrt X at the solution and set it to 0, we have
|
||||
* \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*)
|
||||
*
|
||||
* For each set of rows of (*) corresponding to a variable xi involving in some constraints
|
||||
* we have:
|
||||
* \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi
|
||||
* \grad c_k(xi) = \frac{\partial c_k}{\partial xi}'
|
||||
*
|
||||
* Note: If xi does not involve in any constraint, we have the trivial condition
|
||||
* \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables.
|
||||
*
|
||||
* So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0
|
||||
* on the constraints' lambda multipliers, as follows:
|
||||
* - The jacobian term A_k for each lambda_k is \grad c_k(xi)
|
||||
* - The constant term b is \grad f(xi), which can be computed from all unconstrained
|
||||
* Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi
|
||||
*/
|
||||
GaussianFactorGraph::shared_ptr buildDualGraph(
|
||||
const LinearInequalityFactorGraph& workingSet,
|
||||
const VectorValues& delta) const;
|
||||
|
||||
/// @}
|
||||
|
||||
/**
|
||||
* The goal of this function is to find currently active inequality constraints
|
||||
* that violate the condition to be active. The one that violates the condition
|
||||
* the most will be removed from the active set. See Nocedal06book, pg 469-471
|
||||
*
|
||||
* Find the BAD active inequality that pulls x strongest to the wrong direction
|
||||
* of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0)
|
||||
*
|
||||
* For active inequality constraints (those that are enforced as equality constraints
|
||||
* in the current working set), we want lambda < 0.
|
||||
* This is because:
|
||||
* - From the Lagrangian L = f - lambda*c, we know that the constraint force
|
||||
* is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay
|
||||
* on the constraint surface, the constraint force has to balance out with
|
||||
* other unconstrained forces that are pulling x towards the unconstrained
|
||||
* minimum point. The other unconstrained forces are pulling x toward (-\grad f),
|
||||
* hence the constraint force has to be exactly \grad f, so that the total
|
||||
* force is 0.
|
||||
* - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0),
|
||||
* while we are solving for - (<=0) constraint.
|
||||
* - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction
|
||||
* i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied.
|
||||
* That means we want lambda < 0.
|
||||
* - This is because when the constrained force pulls x towards the infeasible region (+),
|
||||
* the unconstrained force is pulling x towards the opposite direction into
|
||||
* the feasible region (again because the total force has to be 0 to make x stay still)
|
||||
* So we can drop this constraint to have a lower error but feasible solution.
|
||||
*
|
||||
* In short, active inequality constraints with lambda > 0 are BAD, because they
|
||||
* violate the condition to be active.
|
||||
*
|
||||
* And we want to remove the worst one with the largest lambda from the active set.
|
||||
*
|
||||
*/
|
||||
int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet,
|
||||
const VectorValues& lambdas) const;
|
||||
|
||||
/**
|
||||
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
|
||||
*
|
||||
* @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex)
|
||||
* is the constraint that has minimum alpha, or (-1,-1) if alpha = 1.
|
||||
* This constraint will be added to the working set and become active
|
||||
* in the next iteration
|
||||
*/
|
||||
boost::tuple<double, int> computeStepSize(
|
||||
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
|
||||
const VectorValues& p) const;
|
||||
|
||||
/** Iterate 1 step, return a new state with a new workingSet and values */
|
||||
QPState iterate(const QPState& state) const;
|
||||
|
||||
/**
|
||||
* Identify active constraints based on initial values.
|
||||
*/
|
||||
LinearInequalityFactorGraph identifyActiveConstraints(
|
||||
const LinearInequalityFactorGraph& inequalities,
|
||||
const VectorValues& initialValues) const;
|
||||
|
||||
/** Optimize with a provided initial values
|
||||
* For this version, it is the responsibility of the caller to provide
|
||||
* a feasible initial value.
|
||||
* @return a pair of <primal, dual> solutions
|
||||
*/
|
||||
std::pair<VectorValues, VectorValues> optimize(
|
||||
const VectorValues& initialValues) const;
|
||||
|
||||
};
|
||||
|
||||
} /* namespace gtsam */
|
||||
}
|
||||
|
|
@ -0,0 +1,271 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file RawQP.cpp
|
||||
* @brief
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 3/5/16
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/RawQP.h>
|
||||
#include <iostream>
|
||||
|
||||
using boost::fusion::at_c;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
void RawQP::setName(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>> const &name) {
|
||||
name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end());
|
||||
if (debug) {
|
||||
std::cout << "Parsing file: " << name_ << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void RawQP::addColumn(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const &vars) {
|
||||
|
||||
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
|
||||
|
||||
if (!varname_to_key.count(var_))
|
||||
varname_to_key[var_] = Symbol('X', varNumber++);
|
||||
if (row_ == obj_name) {
|
||||
g[varname_to_key[var_]] = coefficient;
|
||||
return;
|
||||
}
|
||||
(*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient;
|
||||
if (debug) {
|
||||
std::cout << "Added Column for Var: " << var_ << " Row: " << row_
|
||||
<< " Coefficient: " << coefficient << std::endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void RawQP::addColumnDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double> const &vars) {
|
||||
|
||||
std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
|
||||
std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
|
||||
std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end());
|
||||
Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
|
||||
Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
|
||||
if (!varname_to_key.count(var_))
|
||||
varname_to_key.insert( { var_, Symbol('X', varNumber++) });
|
||||
if (row1_ == obj_name)
|
||||
g[varname_to_key[var_]] = coefficient1;
|
||||
else
|
||||
(*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1;
|
||||
if (row2_ == obj_name)
|
||||
g[varname_to_key[var_]] = coefficient2;
|
||||
else
|
||||
(*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2;
|
||||
}
|
||||
|
||||
void RawQP::addRHS(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const &vars) {
|
||||
|
||||
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
double coefficient = at_c < 5 > (vars);
|
||||
if (row_ == obj_name)
|
||||
f = -coefficient;
|
||||
else
|
||||
b[row_] = coefficient;
|
||||
|
||||
if (debug) {
|
||||
std::cout << "Added RHS for Var: " << var_ << " Row: " << row_
|
||||
<< " Coefficient: " << coefficient << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void RawQP::addRHSDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double> const &vars) {
|
||||
|
||||
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
|
||||
double coefficient1 = at_c < 5 > (vars);
|
||||
double coefficient2 = at_c < 9 > (vars);
|
||||
if (row1_ == obj_name)
|
||||
f = -coefficient1;
|
||||
else
|
||||
b[row1_] = coefficient1;
|
||||
|
||||
if (row2_ == obj_name)
|
||||
f = -coefficient2;
|
||||
else
|
||||
b[row2_] = coefficient2;
|
||||
|
||||
if (debug) {
|
||||
std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_
|
||||
<< " Coefficient: " << coefficient1 << std::endl;
|
||||
std::cout << " " << "Row: " << row2_
|
||||
<< " Coefficient: " << coefficient2 << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void RawQP::addRow(
|
||||
boost::fusion::vector<std::vector<char>, char, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>> const &vars) {
|
||||
|
||||
std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
char type = at_c < 1 > (vars);
|
||||
switch (type) {
|
||||
case 'N':
|
||||
obj_name = name_;
|
||||
break;
|
||||
case 'L':
|
||||
row_to_constraint_v[name_] = &IL;
|
||||
break;
|
||||
case 'G':
|
||||
row_to_constraint_v[name_] = &IG;
|
||||
break;
|
||||
case 'E':
|
||||
row_to_constraint_v[name_] = &E;
|
||||
break;
|
||||
default:
|
||||
std::cout << "invalid type: " << type << std::endl;
|
||||
break;
|
||||
}
|
||||
if (debug) {
|
||||
std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void RawQP::addBound(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double> const &vars) {
|
||||
|
||||
std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
|
||||
double number = at_c < 7 > (vars);
|
||||
if (type_.compare(std::string("UP")) == 0)
|
||||
up[varname_to_key[var_]] = number;
|
||||
else if (type_.compare(std::string("LO")) == 0)
|
||||
lo[varname_to_key[var_]] = number;
|
||||
else
|
||||
std::cout << "Invalid Bound Type: " << type_ << std::endl;
|
||||
|
||||
if (debug) {
|
||||
std::cout << "Added Bound Type: " << type_ << " Var: " << var_
|
||||
<< " Amount: " << number << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void RawQP::addBoundFr(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>> const &vars) {
|
||||
std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
|
||||
Free.push_back(varname_to_key[var_]);
|
||||
if (debug) {
|
||||
std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_
|
||||
<< " Amount: " << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void RawQP::addQuadTerm(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const &vars) {
|
||||
std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
|
||||
|
||||
H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
|
||||
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
|
||||
if (debug) {
|
||||
std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
|
||||
<< " Coefficient: " << coefficient << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
QP RawQP::makeQP() {
|
||||
std::vector < Key > keys;
|
||||
std::vector < Matrix > Gs;
|
||||
std::vector < Vector > gs;
|
||||
for (auto kv : varname_to_key) {
|
||||
keys.push_back(kv.second);
|
||||
}
|
||||
std::sort(keys.begin(), keys.end());
|
||||
for (unsigned int i = 0; i < keys.size(); ++i) {
|
||||
for (unsigned int j = i; j < keys.size(); ++j) {
|
||||
Gs.push_back(H[keys[i]][keys[j]]);
|
||||
}
|
||||
}
|
||||
for (Key key1 : keys) {
|
||||
gs.push_back(-g[key1]);
|
||||
}
|
||||
int dual_key_num = keys.size() + 1;
|
||||
QP madeQP;
|
||||
auto obj = HessianFactor(keys, Gs, gs, f);
|
||||
|
||||
madeQP.cost.push_back(obj);
|
||||
|
||||
for (auto kv : E) {
|
||||
std::map<Key, Matrix11> keyMatrixMap;
|
||||
for (auto km : kv.second) {
|
||||
keyMatrixMap.insert(km);
|
||||
}
|
||||
madeQP.equalities.push_back(
|
||||
LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
|
||||
}
|
||||
|
||||
for (auto kv : IG) {
|
||||
std::map<Key, Matrix11> keyMatrixMap;
|
||||
for (auto km : kv.second) {
|
||||
km.second = -km.second;
|
||||
keyMatrixMap.insert(km);
|
||||
}
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++));
|
||||
}
|
||||
|
||||
for (auto kv : IL) {
|
||||
std::map<Key, Matrix11> keyMatrixMap;
|
||||
for (auto km : kv.second) {
|
||||
keyMatrixMap.insert(km);
|
||||
}
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++));
|
||||
}
|
||||
|
||||
for (Key k : keys) {
|
||||
if (std::find(Free.begin(), Free.end(), k) != Free.end())
|
||||
continue;
|
||||
if (up.count(k) == 1)
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(k, I_1x1, up[k], dual_key_num++));
|
||||
if (lo.count(k) == 1)
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(k, -I_1x1, lo[k], dual_key_num++));
|
||||
else
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(k, -I_1x1, 0, dual_key_num++));
|
||||
}
|
||||
return madeQP;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,106 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file RawQP.h
|
||||
* @brief
|
||||
* @author Ivan Dario Jimenez
|
||||
* @date 3/5/16
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam_unstable/linear/QP.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <boost/fusion/sequence.hpp>
|
||||
#include <boost/fusion/include/vector.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
class RawQP {
|
||||
private:
|
||||
typedef std::unordered_map<Key, Matrix11> coefficient_v;
|
||||
typedef std::unordered_map<std::string, coefficient_v> constraint_v;
|
||||
|
||||
std::unordered_map<std::string, constraint_v*> row_to_constraint_v;
|
||||
constraint_v E;
|
||||
constraint_v IG;
|
||||
constraint_v IL;
|
||||
unsigned int varNumber;
|
||||
std::unordered_map<std::string, double> b;
|
||||
std::unordered_map<Key, Vector1> g;
|
||||
std::unordered_map<std::string, Key> varname_to_key;
|
||||
std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H;
|
||||
double f;
|
||||
std::string obj_name;
|
||||
std::string name_;
|
||||
std::unordered_map<Key, double> up;
|
||||
std::unordered_map<Key, double> lo;
|
||||
std::vector<Key> Free;
|
||||
const bool debug = false;
|
||||
|
||||
public:
|
||||
RawQP() :
|
||||
row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() {
|
||||
}
|
||||
|
||||
void setName(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>> const & name);
|
||||
|
||||
void addColumn(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
|
||||
void addColumnDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double> const & vars);
|
||||
|
||||
void addRHS(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
|
||||
void addRHSDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
|
||||
|
||||
void addRow(
|
||||
boost::fusion::vector<std::vector<char>, char, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>> const & vars);
|
||||
|
||||
void addBound(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double> const & vars);
|
||||
|
||||
void addBoundFr(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>> const & vars);
|
||||
|
||||
void addQuadTerm(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
|
||||
QP makeQP();
|
||||
}
|
||||
;
|
||||
}
|
||||
|
|
@ -0,0 +1,255 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testQPSolver.cpp
|
||||
* @brief Test simple QP solver for a linear inequality constraint
|
||||
* @date Apr 10, 2014
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
|
||||
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
|
||||
#include <gtsam_unstable/linear/LPSolver.h>
|
||||
#include <gtsam_unstable/linear/LPInitSolver.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
|
||||
static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* min -x1-x2
|
||||
* s.t. x1 + 2x2 <= 4
|
||||
* 4x1 + 2x2 <= 12
|
||||
* -x1 + x2 <= 1
|
||||
* x1, x2 >= 0
|
||||
*/
|
||||
LP simpleLP1() {
|
||||
LP lp;
|
||||
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
|
||||
lp.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
|
||||
return lp;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace gtsam {
|
||||
|
||||
TEST(LPInitSolver, infinite_loop_single_var) {
|
||||
LP initchecker;
|
||||
initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(1, 0, -1), 20, 4)); // x - alpha <= 20
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(1, Vector3(0, -1, -1), 0, 5)); // -y - alpha <= 0
|
||||
LPSolver solver(initchecker);
|
||||
VectorValues starter;
|
||||
starter.insert(1, Vector3(0, 0, 2));
|
||||
VectorValues results, duals;
|
||||
boost::tie(results, duals) = solver.optimize(starter);
|
||||
VectorValues expected;
|
||||
expected.insert(1, Vector3(13.5, 6.5, -6.5));
|
||||
CHECK(assert_equal(results, expected, 1e-7));
|
||||
}
|
||||
|
||||
TEST(LPInitSolver, infinite_loop_multi_var) {
|
||||
LP initchecker;
|
||||
Key X = symbol('X', 1);
|
||||
Key Y = symbol('Y', 1);
|
||||
Key Z = symbol('Z', 1);
|
||||
initchecker.cost = LinearCost(Z, kOne); // min alpha
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
|
||||
1)); //-2x-y-alpha <= -2
|
||||
initchecker.inequalities.push_back(
|
||||
LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
|
||||
2)); // -x+2y-alpha <= 6
|
||||
initchecker.inequalities.push_back(LinearInequality(
|
||||
X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0
|
||||
initchecker.inequalities.push_back(LinearInequality(
|
||||
X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20
|
||||
initchecker.inequalities.push_back(LinearInequality(
|
||||
Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0
|
||||
LPSolver solver(initchecker);
|
||||
VectorValues starter;
|
||||
starter.insert(X, kZero);
|
||||
starter.insert(Y, kZero);
|
||||
starter.insert(Z, Vector::Constant(1, 2.0));
|
||||
VectorValues results, duals;
|
||||
boost::tie(results, duals) = solver.optimize(starter);
|
||||
VectorValues expected;
|
||||
expected.insert(X, Vector::Constant(1, 13.5));
|
||||
expected.insert(Y, Vector::Constant(1, 6.5));
|
||||
expected.insert(Z, Vector::Constant(1, -6.5));
|
||||
CHECK(assert_equal(results, expected, 1e-7));
|
||||
}
|
||||
|
||||
TEST(LPInitSolver, initialization) {
|
||||
LP lp = simpleLP1();
|
||||
LPInitSolver initSolver(lp);
|
||||
|
||||
GaussianFactorGraph::shared_ptr initOfInitGraph =
|
||||
initSolver.buildInitOfInitGraph();
|
||||
VectorValues x0 = initOfInitGraph->optimize();
|
||||
VectorValues expected_x0;
|
||||
expected_x0.insert(1, Vector::Zero(2));
|
||||
CHECK(assert_equal(expected_x0, x0, 1e-10));
|
||||
|
||||
double y0 = initSolver.compute_y0(x0);
|
||||
double expected_y0 = 0.0;
|
||||
DOUBLES_EQUAL(expected_y0, y0, 1e-7);
|
||||
|
||||
Key yKey = 2;
|
||||
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
|
||||
LP expectedInitLP;
|
||||
expectedInitLP.cost = LinearCost(yKey, kOne);
|
||||
expectedInitLP.inequalities.push_back(LinearInequality(
|
||||
1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
|
||||
expectedInitLP.inequalities.push_back(LinearInequality(
|
||||
1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4,
|
||||
3)); // x1 + 2*x2 - y <= 4
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12,
|
||||
4)); // 4x1 + 2x2 - y <= 12
|
||||
expectedInitLP.inequalities.push_back(
|
||||
LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1,
|
||||
5)); // -x1 + x2 - y <= 1
|
||||
CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
|
||||
LPSolver lpSolveInit(*initLP);
|
||||
VectorValues xy0(x0);
|
||||
xy0.insert(yKey, Vector::Constant(1, y0));
|
||||
VectorValues xyInit = lpSolveInit.optimize(xy0).first;
|
||||
VectorValues expected_init;
|
||||
expected_init.insert(1, Vector::Ones(2));
|
||||
expected_init.insert(2, Vector::Constant(1, -1));
|
||||
CHECK(assert_equal(expected_init, xyInit, 1e-10));
|
||||
|
||||
VectorValues x = initSolver.solve();
|
||||
CHECK(lp.isFeasible(x));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* TEST gtsam solver with an over-constrained system
|
||||
* x + y = 1
|
||||
* x - y = 5
|
||||
* x + 2y = 6
|
||||
*/
|
||||
TEST(LPSolver, overConstrainedLinearSystem) {
|
||||
GaussianFactorGraph graph;
|
||||
Matrix A1 = Vector3(1, 1, 1);
|
||||
Matrix A2 = Vector3(1, -1, 2);
|
||||
Vector b = Vector3(1, 5, 6);
|
||||
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
|
||||
graph.push_back(factor);
|
||||
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle
|
||||
// over-constrained system
|
||||
CHECK(factor.error(x) != 0.0);
|
||||
}
|
||||
|
||||
TEST(LPSolver, overConstrainedLinearSystem2) {
|
||||
GaussianFactorGraph graph;
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne,
|
||||
noiseModel::Constrained::All(1)));
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne,
|
||||
noiseModel::Constrained::All(1)));
|
||||
graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
|
||||
noiseModel::Constrained::All(1)));
|
||||
VectorValues x = graph.optimize();
|
||||
// This check confirms that gtsam linear constraint solver can't handle
|
||||
// over-constrained system
|
||||
CHECK(graph.error(x) != 0.0);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, simpleTest1) {
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues init;
|
||||
init.insert(1, Vector::Zero(2));
|
||||
|
||||
VectorValues x1 =
|
||||
lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize();
|
||||
VectorValues expected_x1;
|
||||
expected_x1.insert(1, Vector::Ones(2));
|
||||
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
||||
|
||||
VectorValues result, duals;
|
||||
boost::tie(result, duals) = lpSolver.optimize(init);
|
||||
VectorValues expectedResult;
|
||||
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
||||
CHECK(assert_equal(expectedResult, result, 1e-10));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, testWithoutInitialValues) {
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues result, duals, expectedResult;
|
||||
expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
|
||||
boost::tie(result, duals) = lpSolver.optimize();
|
||||
CHECK(assert_equal(expectedResult, result));
|
||||
}
|
||||
|
||||
/**
|
||||
* TODO: More TEST cases:
|
||||
* - Infeasible
|
||||
* - Unbounded
|
||||
* - Underdetermined
|
||||
*/
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, LinearCost) {
|
||||
LinearCost cost(1, Vector3(2., 4., 6.));
|
||||
VectorValues x;
|
||||
x.insert(1, Vector3(1., 3., 5.));
|
||||
double error = cost.error(x);
|
||||
double expectedError = 44.0;
|
||||
DOUBLES_EQUAL(expectedError, error, 1e-100);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -12,7 +12,7 @@
|
|||
/**
|
||||
* @file testLinearEquality.cpp
|
||||
* @brief Unit tests for LinearEquality
|
||||
* @author thduynguyen
|
||||
* @author Duy-Nguyen Ta
|
||||
**/
|
||||
|
||||
#include <gtsam_unstable/linear/LinearEquality.h>
|
||||
|
|
@ -28,20 +28,20 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using namespace boost::assign;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(LinearEquality)
|
||||
GTSAM_CONCEPT_TESTABLE_INST (LinearEquality)
|
||||
|
||||
namespace {
|
||||
namespace simple {
|
||||
// Terms we'll use
|
||||
const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
|
||||
(make_pair(5, Matrix3::Identity()))
|
||||
(make_pair(10, 2*Matrix3::Identity()))
|
||||
(make_pair(15, 3*Matrix3::Identity()));
|
||||
namespace simple {
|
||||
// Terms we'll use
|
||||
const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix>
|
||||
> (make_pair(5, Matrix3::Identity()))(
|
||||
make_pair(10, 2 * Matrix3::Identity()))(
|
||||
make_pair(15, 3 * Matrix3::Identity()));
|
||||
|
||||
// RHS and sigmas
|
||||
const Vector b = (Vector(3) << 1., 2., 3.).finished();
|
||||
const SharedDiagonal noise = noiseModel::Constrained::All(3);
|
||||
}
|
||||
// RHS and sigmas
|
||||
const Vector b = (Vector(3) << 1., 2., 3.).finished();
|
||||
const SharedDiagonal noise = noiseModel::Constrained::All(3);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors)
|
|||
{
|
||||
// One term constructor
|
||||
LinearEquality expected(
|
||||
boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
|
||||
boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
|
||||
LinearEquality actual(terms[0].first, terms[0].second, b, 0);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
|
||||
|
|
@ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors)
|
|||
{
|
||||
// Two term constructor
|
||||
LinearEquality expected(
|
||||
boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
|
||||
boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
|
||||
LinearEquality actual(terms[0].first, terms[0].second,
|
||||
terms[1].first, terms[1].second, b, 0);
|
||||
terms[1].first, terms[1].second, b, 0);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
|
||||
EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
|
||||
|
|
@ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors)
|
|||
{
|
||||
// Three term constructor
|
||||
LinearEquality expected(
|
||||
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
|
||||
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
|
||||
LinearEquality actual(terms[0].first, terms[0].second,
|
||||
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
|
||||
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
|
||||
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
|
||||
|
|
@ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors)
|
|||
/* ************************************************************************* */
|
||||
TEST(LinearEquality, Hessian_conversion) {
|
||||
HessianFactor hessian(0, (Matrix(4,4) <<
|
||||
1.57, 2.695, -1.1, -2.35,
|
||||
2.695, 11.3125, -0.65, -10.225,
|
||||
-1.1, -0.65, 1, 0.5,
|
||||
-2.35, -10.225, 0.5, 9.25).finished(),
|
||||
1.57, 2.695, -1.1, -2.35,
|
||||
2.695, 11.3125, -0.65, -10.225,
|
||||
-1.1, -0.65, 1, 0.5,
|
||||
-2.35, -10.225, 0.5, 9.25).finished(),
|
||||
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
|
||||
73.1725);
|
||||
|
||||
|
|
@ -169,8 +169,8 @@ TEST(LinearEquality, matrices)
|
|||
augmentedJacobianExpected << jacobianExpected, rhsExpected;
|
||||
|
||||
Matrix augmentedHessianExpected =
|
||||
augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
|
||||
* simple::noise->R() * augmentedJacobianExpected;
|
||||
augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
|
||||
* simple::noise->R() * augmentedJacobianExpected;
|
||||
|
||||
// Whitened Jacobian
|
||||
EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
|
||||
|
|
@ -210,8 +210,8 @@ TEST(LinearEquality, operators )
|
|||
// test gradient at zero
|
||||
Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
|
||||
VectorValues expectedG;
|
||||
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
|
||||
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
|
||||
expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
|
||||
expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
|
||||
VectorValues actualG = lf.gradientAtZero();
|
||||
EXPECT(assert_equal(expectedG, actualG));
|
||||
}
|
||||
|
|
@ -233,5 +233,8 @@ TEST(LinearEquality, empty )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -19,14 +19,14 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam_unstable/linear/QPSolver.h>
|
||||
|
||||
#include <gtsam_unstable/linear/QPSParser.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
|
||||
const Matrix One = I_1x1;
|
||||
static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create test graph according to Forst10book_pg171Ex5
|
||||
|
|
@ -37,15 +37,17 @@ QP createTestCase() {
|
|||
// Note the Hessian encodes:
|
||||
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
|
||||
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
|
||||
//TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation
|
||||
// Should be 0.5x'Gx + gx + f : Nocedal 449
|
||||
qp.cost.push_back(
|
||||
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1,
|
||||
2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0));
|
||||
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1,
|
||||
Z_1x1, 10.0));
|
||||
|
||||
// Inequality constraints
|
||||
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -53,8 +55,8 @@ QP createTestCase() {
|
|||
TEST(QPSolver, TestCase) {
|
||||
VectorValues values;
|
||||
double x1 = 5, x2 = 7;
|
||||
values.insert(X(1), x1 * Matrix::Ones(1, 1));
|
||||
values.insert(X(2), x2 * Matrix::Ones(1, 1));
|
||||
values.insert(X(1), x1 * I_1x1);
|
||||
values.insert(X(2), x2 * I_1x1);
|
||||
QP qp = createTestCase();
|
||||
DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9);
|
||||
DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9);
|
||||
|
|
@ -67,15 +69,15 @@ TEST(QPSolver, constraintsAux) {
|
|||
|
||||
VectorValues lambdas;
|
||||
lambdas.insert(0, (Vector(1) << -0.5).finished());
|
||||
lambdas.insert(1, (Vector(1) << 0.0).finished());
|
||||
lambdas.insert(2, (Vector(1) << 0.3).finished());
|
||||
lambdas.insert(3, (Vector(1) << 0.1).finished());
|
||||
lambdas.insert(1, kZero);
|
||||
lambdas.insert(2, (Vector(1) << 0.3).finished());
|
||||
lambdas.insert(3, (Vector(1) << 0.1).finished());
|
||||
int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas);
|
||||
LONGS_EQUAL(2, factorIx);
|
||||
|
||||
VectorValues lambdas2;
|
||||
lambdas2.insert(0, (Vector(1) << -0.5).finished());
|
||||
lambdas2.insert(1, (Vector(1) << 0.0).finished());
|
||||
lambdas2.insert(1, kZero);
|
||||
lambdas2.insert(2, (Vector(1) << -0.3).finished());
|
||||
lambdas2.insert(3, (Vector(1) << -0.1).finished());
|
||||
int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2);
|
||||
|
|
@ -92,14 +94,14 @@ QP createEqualityConstrainedTest() {
|
|||
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
|
||||
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
|
||||
qp.cost.push_back(
|
||||
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1,
|
||||
2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0));
|
||||
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1,
|
||||
0.0));
|
||||
|
||||
// Equality constraints
|
||||
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
|
||||
Matrix A1 = (Matrix(1, 1) << 1).finished();
|
||||
Matrix A2 = (Matrix(1, 1) << 1).finished();
|
||||
Vector b = -(Vector(1) << 1).finished();
|
||||
Matrix A1 = I_1x1;
|
||||
Matrix A2 = I_1x1;
|
||||
Vector b = -kOne;
|
||||
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0));
|
||||
|
||||
return qp;
|
||||
|
|
@ -132,20 +134,19 @@ TEST(QPSolver, indentifyActiveConstraints) {
|
|||
currentSolution.insert(X(1), Z_1x1);
|
||||
currentSolution.insert(X(2), Z_1x1);
|
||||
|
||||
LinearInequalityFactorGraph workingSet =
|
||||
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
|
||||
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
|
||||
qp.inequalities, currentSolution);
|
||||
|
||||
CHECK(!workingSet.at(0)->active()); // inactive
|
||||
CHECK(workingSet.at(1)->active()); // active
|
||||
CHECK(workingSet.at(2)->active()); // active
|
||||
CHECK(!workingSet.at(3)->active()); // inactive
|
||||
CHECK(!workingSet.at(0)->active()); // inactive
|
||||
CHECK(workingSet.at(1)->active());// active
|
||||
CHECK(workingSet.at(2)->active());// active
|
||||
CHECK(!workingSet.at(3)->active());// inactive
|
||||
|
||||
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
|
||||
VectorValues solution = solver.buildWorkingGraph(workingSet).optimize();
|
||||
VectorValues expectedSolution;
|
||||
expectedSolution.insert(X(1), (Vector(1) << 0.0).finished());
|
||||
expectedSolution.insert(X(2), (Vector(1) << 0.0).finished());
|
||||
expectedSolution.insert(X(1), kZero);
|
||||
expectedSolution.insert(X(2), kZero);
|
||||
CHECK(assert_equal(expectedSolution, solution, 1e-100));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -158,13 +159,13 @@ TEST(QPSolver, iterate) {
|
|||
currentSolution.insert(X(2), Z_1x1);
|
||||
|
||||
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
|
||||
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished());
|
||||
expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished());
|
||||
expectedSolutions[0].insert(X(1), kZero);
|
||||
expectedSolutions[0].insert(X(2), kZero);
|
||||
expectedDuals[0].insert(1, (Vector(1) << 3).finished());
|
||||
expectedDuals[0].insert(2, (Vector(1) << 0).finished());
|
||||
expectedDuals[0].insert(2, kZero);
|
||||
|
||||
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished());
|
||||
expectedSolutions[1].insert(X(2), kZero);
|
||||
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
|
||||
|
||||
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
|
|
@ -173,10 +174,11 @@ TEST(QPSolver, iterate) {
|
|||
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
|
||||
|
||||
LinearInequalityFactorGraph workingSet =
|
||||
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
|
||||
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
|
||||
qp.inequalities, currentSolution);
|
||||
|
||||
QPState state(currentSolution, VectorValues(), workingSet, false);
|
||||
QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
|
||||
100);
|
||||
|
||||
int it = 0;
|
||||
while (!state.converged) {
|
||||
|
|
@ -209,6 +211,64 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
|
|||
CHECK(assert_equal(expectedSolution, solution, 1e-100));
|
||||
}
|
||||
|
||||
pair<QP, QP> testParser(QPSParser parser) {
|
||||
QP exampleqp = parser.Parse();
|
||||
QP expectedqp;
|
||||
Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
|
||||
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
|
||||
expectedqp.cost.push_back(
|
||||
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1,
|
||||
2.0 * kOne, 4.0));
|
||||
// 2x + y >= 2
|
||||
// -x + 2y <= 6
|
||||
expectedqp.inequalities.push_back(
|
||||
LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0));
|
||||
expectedqp.inequalities.push_back(
|
||||
LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1));
|
||||
// x<= 20
|
||||
expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
|
||||
//x >= 0
|
||||
expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2));
|
||||
// y > = 0
|
||||
expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3));
|
||||
return std::make_pair(expectedqp, exampleqp);
|
||||
}
|
||||
;
|
||||
|
||||
TEST(QPSolver, ParserSyntaticTest) {
|
||||
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
|
||||
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost,
|
||||
1e-7));
|
||||
CHECK(assert_equal(expectedActual.first.inequalities,
|
||||
expectedActual.second.inequalities, 1e-7));
|
||||
CHECK(assert_equal(expectedActual.first.equalities,
|
||||
expectedActual.second.equalities, 1e-7));
|
||||
}
|
||||
|
||||
TEST(QPSolver, ParserSemanticTest) {
|
||||
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
|
||||
VectorValues actualSolution, expectedSolution;
|
||||
boost::tie(expectedSolution, boost::tuples::ignore) =
|
||||
QPSolver(expected_actual.first).optimize();
|
||||
boost::tie(actualSolution, boost::tuples::ignore) =
|
||||
QPSolver(expected_actual.second).optimize();
|
||||
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
|
||||
}
|
||||
|
||||
TEST(QPSolver, QPExampleTest){
|
||||
QP problem = QPSParser("QPExample.QPS").Parse();
|
||||
VectorValues actualSolution;
|
||||
auto solver = QPSolver(problem);
|
||||
boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize();
|
||||
VectorValues expectedSolution;
|
||||
expectedSolution.insert(Symbol('X',1),0.7625*I_1x1);
|
||||
expectedSolution.insert(Symbol('X',2),0.4750*I_1x1);
|
||||
double error_expected = problem.cost.error(expectedSolution);
|
||||
double error_actual = problem.cost.error(actualSolution);
|
||||
CHECK(assert_equal(expectedSolution, actualSolution, 1e-7))
|
||||
CHECK(assert_equal(error_expected, error_actual))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
|
||||
QP createTestMatlabQPEx() {
|
||||
|
|
@ -219,19 +279,22 @@ QP createTestMatlabQPEx() {
|
|||
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
|
||||
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
|
||||
qp.cost.push_back(
|
||||
HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1,
|
||||
2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0));
|
||||
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1,
|
||||
6 * I_1x1, 1000.0));
|
||||
|
||||
// Inequality constraints
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
TEST(QPSolver, optimizeMatlabEx) {
|
||||
QP qp = createTestMatlabQPEx();
|
||||
QPSolver solver(qp);
|
||||
|
|
@ -246,20 +309,35 @@ TEST(QPSolver, optimizeMatlabEx) {
|
|||
CHECK(assert_equal(expectedSolution, solution, 1e-7));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
TEST(QPSolver, optimizeMatlabExNoinitials) {
|
||||
QP qp = createTestMatlabQPEx();
|
||||
QPSolver solver(qp);
|
||||
VectorValues solution;
|
||||
boost::tie(solution, boost::tuples::ignore) = solver.optimize();
|
||||
VectorValues expectedSolution;
|
||||
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
|
||||
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
|
||||
CHECK(assert_equal(expectedSolution, solution, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create test graph as in Nocedal06book, Ex 16.4, pg. 475
|
||||
QP createTestNocedal06bookEx16_4() {
|
||||
QP qp;
|
||||
|
||||
qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1));
|
||||
qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1));
|
||||
qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1));
|
||||
qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1));
|
||||
|
||||
// Inequality constraints
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3));
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3));
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4));
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -280,28 +358,45 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST(QPSolver, failedSubproblem) {
|
||||
QP qp;
|
||||
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1));
|
||||
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0));
|
||||
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
|
||||
|
||||
VectorValues expected;
|
||||
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
|
||||
|
||||
VectorValues initialValues;
|
||||
initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished());
|
||||
initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished());
|
||||
|
||||
QPSolver solver(qp);
|
||||
VectorValues solution;
|
||||
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
|
||||
// graph.print("Graph: ");
|
||||
// solution.print("Solution: ");
|
||||
|
||||
CHECK(assert_equal(expected, solution, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(QPSolver, infeasibleInitial) {
|
||||
QP qp;
|
||||
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2)));
|
||||
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
|
||||
|
||||
VectorValues expected;
|
||||
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
|
||||
|
||||
VectorValues initialValues;
|
||||
initialValues.insert(X(1), (Vector(2) << -10.0, 100.0).finished());
|
||||
|
||||
QPSolver solver(qp);
|
||||
VectorValues solution;
|
||||
CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -60,15 +60,15 @@ using namespace boost; // not usual, but for conciseness of generated code
|
|||
// "Unique" key to signal calling the matlab object constructor with a raw pointer
|
||||
// to a shared pointer of the same C++ object type as the MATLAB type.
|
||||
// Also present in utilities.h
|
||||
static const uint64_t ptr_constructor_key =
|
||||
(uint64_t('G') << 56) |
|
||||
(uint64_t('T') << 48) |
|
||||
(uint64_t('S') << 40) |
|
||||
(uint64_t('A') << 32) |
|
||||
(uint64_t('M') << 24) |
|
||||
(uint64_t('p') << 16) |
|
||||
(uint64_t('t') << 8) |
|
||||
(uint64_t('r'));
|
||||
static const boost::uint64_t ptr_constructor_key =
|
||||
(boost::uint64_t('G') << 56) |
|
||||
(boost::uint64_t('T') << 48) |
|
||||
(boost::uint64_t('S') << 40) |
|
||||
(boost::uint64_t('A') << 32) |
|
||||
(boost::uint64_t('M') << 24) |
|
||||
(boost::uint64_t('p') << 16) |
|
||||
(boost::uint64_t('t') << 8) |
|
||||
(boost::uint64_t('r'));
|
||||
|
||||
//*****************************************************************************
|
||||
// Utilities
|
||||
|
|
@ -244,9 +244,9 @@ template <typename T>
|
|||
T myGetScalar(const mxArray* array) {
|
||||
switch (mxGetClassID(array)) {
|
||||
case mxINT64_CLASS:
|
||||
return (T) *(int64_t*) mxGetData(array);
|
||||
return (T) *(boost::int64_t*) mxGetData(array);
|
||||
case mxUINT64_CLASS:
|
||||
return (T) *(uint64_t*) mxGetData(array);
|
||||
return (T) *(boost::uint64_t*) mxGetData(array);
|
||||
default:
|
||||
// hope for the best!
|
||||
return (T) mxGetScalar(array);
|
||||
|
|
@ -349,7 +349,7 @@ mxArray* create_object(const std::string& classname, void *pointer, bool isVirtu
|
|||
int nargin = 2;
|
||||
// First input argument is pointer constructor key
|
||||
input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL);
|
||||
*reinterpret_cast<uint64_t*>(mxGetData(input[0])) = ptr_constructor_key;
|
||||
*reinterpret_cast<boost::uint64_t*>(mxGetData(input[0])) = ptr_constructor_key;
|
||||
// Second input argument is the pointer
|
||||
input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);
|
||||
*reinterpret_cast<void**>(mxGetData(input[1])) = pointer;
|
||||
|
|
|
|||
Loading…
Reference in New Issue