first unittest finding QP's feasible initial point works
thduynguyen
parent
6520ae8075
commit
c97f29be23
|
|
@ -87,7 +87,7 @@ void LPSolver::addConstraints(const boost::shared_ptr<lprec>& lp,
|
|||
vector<int> columnNoCopy(columnNo);
|
||||
|
||||
if (sigmas[i]>0) {
|
||||
throw runtime_error("LP can't accept Gaussian noise!");
|
||||
cout << "Warning: Ignore Gaussian noise (sigma>0) in LP constraints!" << endl;
|
||||
}
|
||||
int constraintType = (sigmas[i]<0)?LE:EQ;
|
||||
if(!add_constraintex(lp.get(), columnNoCopy.size(), r.data(), columnNoCopy.data(),
|
||||
|
|
|
|||
|
|
@ -6,8 +6,10 @@
|
|||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <gtsam/linear/QPSolver.h>
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/QPSolver.h>
|
||||
#include <gtsam/linear/LPSolver.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -15,7 +17,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
QPSolver::QPSolver(const GaussianFactorGraph& graph) :
|
||||
graph_(graph), fullFactorIndices_(graph) {
|
||||
graph_(graph), fullFactorIndices_(graph) {
|
||||
// Split the original graph into unconstrained and constrained part
|
||||
// and collect indices of constrained factors
|
||||
for (size_t i = 0; i < graph.nrFactors(); ++i) {
|
||||
|
|
@ -290,8 +292,8 @@ boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGra
|
|||
if (debug) cout << "s, ajTp, b[s]: " << s << " " << ajTp << " " << b[s] << endl;
|
||||
|
||||
// Check if aj'*p >0. Don't care if it's not.
|
||||
// if (ajTp - b[s]>0)
|
||||
// throw std::runtime_error("Infeasible point detected. Please choose a feasible initial values!");
|
||||
// if (ajTp - b[s]>0)
|
||||
// throw std::runtime_error("Infeasible point detected. Please choose a feasible initial values!");
|
||||
if (ajTp<=0) continue;
|
||||
|
||||
// Compute aj'*xk
|
||||
|
|
@ -377,5 +379,134 @@ VectorValues QPSolver::optimize(const VectorValues& initials,
|
|||
return currentSolution;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<VectorValues, Key> QPSolver::initialValuesLP() const {
|
||||
size_t firstSlackKey = 0;
|
||||
BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) {
|
||||
if (firstSlackKey < key) firstSlackKey = key;
|
||||
}
|
||||
firstSlackKey += 1;
|
||||
|
||||
VectorValues initials;
|
||||
// Create zero values for constrained vars
|
||||
BOOST_FOREACH(size_t iFactor, constraintIndices_) {
|
||||
JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor));
|
||||
KeyVector keys = jacobian->keys();
|
||||
BOOST_FOREACH(Key key, keys) {
|
||||
if (!initials.exists(key)) {
|
||||
size_t dim = jacobian->getDim(jacobian->find(key));
|
||||
initials.insert(key, zero(dim));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Insert initial values for slack variables
|
||||
size_t slackKey = firstSlackKey;
|
||||
BOOST_FOREACH(size_t iFactor, constraintIndices_) {
|
||||
JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor));
|
||||
Vector errorAtZero = jacobian->getb();
|
||||
Vector slackInit = zero(errorAtZero.size());
|
||||
Vector sigmas = jacobian->get_model()->sigmas();
|
||||
for (size_t i = 0; i<sigmas.size(); ++i) {
|
||||
if (sigmas[i] < 0) {
|
||||
slackInit[i] = std::max(errorAtZero[i], 0.0);
|
||||
} else if (sigmas[i] == 0.0) {
|
||||
errorAtZero[i] = fabs(errorAtZero[i]);
|
||||
} // if it has >0 sigma, i.e. normal Gaussian noise, initialize it at 0
|
||||
}
|
||||
initials.insert(slackKey, slackInit);
|
||||
slackKey++;
|
||||
}
|
||||
return make_pair(initials, firstSlackKey);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const {
|
||||
VectorValues slackObjective;
|
||||
for (size_t i = 0; i < constraintIndices_.size(); ++i) {
|
||||
Key key = firstSlackKey + i;
|
||||
size_t iFactor = constraintIndices_[i];
|
||||
JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor));
|
||||
size_t dim = jacobian->rows();
|
||||
Vector objective = ones(dim);
|
||||
/* We should not ignore unconstrained slack var dimensions (those rows with sigmas >0)
|
||||
* because their values might be underdetermined in the LP. Since they will have only
|
||||
* 1 constraint zi>=0, enforcing them in the min obj function won't harm the other constrained
|
||||
* slack vars, but also makes them well defined: 0 at the minimum.
|
||||
*/
|
||||
slackObjective.insert(key, ones(dim));
|
||||
}
|
||||
return slackObjective;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP(
|
||||
Key firstSlackKey) const {
|
||||
// Create constraints and 0 lower bounds (zi>=0)
|
||||
GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph());
|
||||
VectorValues slackLowerBounds;
|
||||
for (size_t key = firstSlackKey; key<firstSlackKey + constraintIndices_.size(); ++key) {
|
||||
size_t iFactor = constraintIndices_[key-firstSlackKey];
|
||||
JacobianFactor::shared_ptr jacobian = toJacobian(graph_.at(iFactor));
|
||||
// Collect old terms to form a new factor
|
||||
// TODO: it might be faster if we can get the whole block matrix at once
|
||||
// but I don't know how to extend the current VerticalBlockMatrix
|
||||
std::vector<std::pair<Key, Matrix> > terms;
|
||||
for (Factor::iterator it = jacobian->begin(); it != jacobian->end(); ++it) {
|
||||
terms.push_back(make_pair(*it, jacobian->getA(it)));
|
||||
}
|
||||
// Add the slack term to the constraint
|
||||
// Unlike Nocedal06book, pg.473, we want ax-z <= b, since we always assume
|
||||
// LE constraints ax <= b for sigma < 0.
|
||||
size_t dim = jacobian->rows();
|
||||
terms.push_back(make_pair(key, -eye(dim)));
|
||||
constraints->push_back(JacobianFactor(terms, jacobian->getb(), jacobian->get_model()));
|
||||
// Add lower bound for this slack key
|
||||
slackLowerBounds.insert(key, zero(dim));
|
||||
}
|
||||
return make_pair(constraints, slackLowerBounds);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues QPSolver::findFeasibleInitialValues() const {
|
||||
// Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
|
||||
VectorValues initials;
|
||||
size_t firstSlackKey;
|
||||
boost::tie(initials, firstSlackKey) = initialValuesLP();
|
||||
|
||||
// Coefficients for the LP subproblem objective function, min \sum_i z_i
|
||||
VectorValues objectiveLP = objectiveCoeffsLP(firstSlackKey);
|
||||
|
||||
// Create constraints and lower bounds of slack variables
|
||||
GaussianFactorGraph::shared_ptr constraints;
|
||||
VectorValues slackLowerBounds;
|
||||
boost::tie(constraints, slackLowerBounds) = constraintsLP(firstSlackKey);
|
||||
|
||||
// Solve the LP subproblem
|
||||
LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds);
|
||||
VectorValues solution = lpSolver.solve();
|
||||
|
||||
// Remove slack variables from solution
|
||||
for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) {
|
||||
solution.erase(key);
|
||||
}
|
||||
|
||||
// Insert zero vectors for free variables that are not in the constraints
|
||||
BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) {
|
||||
if (!solution.exists(key)) {
|
||||
GaussianFactor::shared_ptr factor = graph_.at(*fullFactorIndices_[key].begin());
|
||||
size_t dim = factor->getDim(factor->find(key));
|
||||
solution.insert(key, zero(dim));
|
||||
}
|
||||
}
|
||||
|
||||
return solution;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues QPSolver::optimize(boost::optional<VectorValues&> lambdas) const {
|
||||
VectorValues initials = findFeasibleInitialValues();
|
||||
return optimize(initials, lambdas);
|
||||
}
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
|
|||
|
|
@ -120,10 +120,38 @@ public:
|
|||
bool iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution,
|
||||
VectorValues& lambdas) const;
|
||||
|
||||
/** Optimize */
|
||||
/** Optimize with a provided initial values
|
||||
* For this version, it is the responsibility of the caller to provide
|
||||
* a feasible initial value, otherwise the solution will be wrong.
|
||||
* If you don't know a feasible initial value, use the other version
|
||||
* of optimize().
|
||||
*/
|
||||
VectorValues optimize(const VectorValues& initials,
|
||||
boost::optional<VectorValues&> lambdas = boost::none) const;
|
||||
|
||||
/** Optimize without an initial value.
|
||||
* This version of optimize will try to find a feasible initial value by solving
|
||||
* an LP program before solving this QP graph.
|
||||
* TODO: If no feasible initial point exists, it should throw an InfeasibilityException!
|
||||
*/
|
||||
VectorValues optimize(boost::optional<VectorValues&> lambdas = boost::none) const;
|
||||
|
||||
|
||||
/**
|
||||
* Create initial values for the LP subproblem
|
||||
* @return initial values and the key for the first slack variable
|
||||
*/
|
||||
std::pair<VectorValues, Key> initialValuesLP() const;
|
||||
|
||||
/// Create coefficients for the LP subproblem's objective function as the sum of slack var
|
||||
VectorValues objectiveCoeffsLP(Key firstSlackKey) const;
|
||||
|
||||
/// Build constraints and slacks' lower bounds for the LP subproblem
|
||||
std::pair<GaussianFactorGraph::shared_ptr, VectorValues> constraintsLP(Key firstSlackKey) const;
|
||||
|
||||
/// Find a feasible initial point
|
||||
VectorValues findFeasibleInitialValues() const;
|
||||
|
||||
/// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed
|
||||
/// TODO: Move to GaussianFactor?
|
||||
static JacobianFactor::shared_ptr toJacobian(const GaussianFactor::shared_ptr& factor) {
|
||||
|
|
|
|||
|
|
@ -246,6 +246,71 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
|
|||
CHECK(assert_equal(expectedSolution, solution, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create test graph as in Nocedal06book, Ex 16.4, pg. 475
|
||||
// with the first constraint (16.49b) is replaced by
|
||||
// x1 - 2 x2 - 2 >=0
|
||||
// so that the trivial initial point (0,0) is infeasible
|
||||
GaussianFactorGraph modifyNocedal06bookEx16_4() {
|
||||
GaussianFactorGraph graph;
|
||||
|
||||
graph.push_back(JacobianFactor(X(1), ones(1,1), ones(1)));
|
||||
graph.push_back(JacobianFactor(X(2), ones(1,1), 2.5*ones(1)));
|
||||
|
||||
// Inequality constraints
|
||||
noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas(
|
||||
(Vector(1) << -1));
|
||||
graph.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), -2*ones(1), noise));
|
||||
graph.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), 6*ones(1), noise));
|
||||
graph.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), 2*ones(1), noise));
|
||||
graph.push_back(JacobianFactor(X(1), -ones(1,1), zero(1), noise));
|
||||
graph.push_back(JacobianFactor(X(2), -ones(1,1), zero(1), noise));
|
||||
|
||||
return graph;
|
||||
}
|
||||
|
||||
TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
|
||||
GaussianFactorGraph graph = modifyNocedal06bookEx16_4();
|
||||
QPSolver solver(graph);
|
||||
VectorValues initialsLP;
|
||||
Key firstSlackKey;
|
||||
boost::tie(initialsLP, firstSlackKey) = solver.initialValuesLP();
|
||||
EXPECT(assert_equal(zero(1), initialsLP.at(X(1))));
|
||||
EXPECT(assert_equal(zero(1), initialsLP.at(X(2))));
|
||||
LONGS_EQUAL(X(2)+1, firstSlackKey);
|
||||
EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey)));
|
||||
EXPECT(assert_equal(ones(1)*6.0, initialsLP.at(firstSlackKey+1)));
|
||||
EXPECT(assert_equal(ones(1)*2.0, initialsLP.at(firstSlackKey+2)));
|
||||
EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+3)));
|
||||
EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+4)));
|
||||
|
||||
initialsLP.print("initialsLP: ");
|
||||
VectorValues objCoeffs = solver.objectiveCoeffsLP(firstSlackKey);
|
||||
cout << "done" << endl;
|
||||
for (size_t i = 0; i<5; ++i)
|
||||
EXPECT(assert_equal(ones(1), objCoeffs.at(firstSlackKey+i)));
|
||||
|
||||
GaussianFactorGraph::shared_ptr constraints;
|
||||
VectorValues lowerBounds;
|
||||
boost::tie(constraints, lowerBounds) = solver.constraintsLP(firstSlackKey);
|
||||
for (size_t i = 0; i<5; ++i)
|
||||
EXPECT(assert_equal(zero(1), lowerBounds.at(firstSlackKey+i)));
|
||||
|
||||
GaussianFactorGraph expectedConstraints;
|
||||
noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas(
|
||||
(Vector(1) << -1));
|
||||
expectedConstraints.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), X(3), -ones(1,1),-2*ones(1), noise));
|
||||
expectedConstraints.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), X(4), -ones(1,1), 6*ones(1), noise));
|
||||
expectedConstraints.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), X(5), -ones(1,1), 2*ones(1), noise));
|
||||
expectedConstraints.push_back(JacobianFactor(X(1), -ones(1,1), X(6), -ones(1,1), zero(1), noise));
|
||||
expectedConstraints.push_back(JacobianFactor(X(2), -ones(1,1), X(7), -ones(1,1), zero(1), noise));
|
||||
EXPECT(assert_equal(expectedConstraints, *constraints));
|
||||
|
||||
VectorValues initials = solver.findFeasibleInitialValues();
|
||||
initials.print("Feasible point found: ");
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue