12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix QPSolver unit tests

release/4.3a0
thduynguyen 2014-12-09 16:27:11 -05:00
parent 9b418c98ca
commit 85397223ef
4 changed files with 144 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam_unstable/linear/LinearInequalityFactorGraph.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/LinearInequality.h>

58
gtsam_unstable/linear/QPSolver.cpp
View File

@ -14,9 +14,6 @@
using namespace std;
#define ACTIVE 0.0
#define INACTIVE std::numeric_limits<double>::infinity()
namespace gtsam {
//******************************************************************************
@ -52,11 +49,29 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
// Collect the gradients of unconstrained cost factors to the b vector
Vector b = zero(delta.at(key).size());
BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) {
GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx);
b += factor->gradient(key, delta);
if (costVariableIndex_.find(key) != costVariableIndex_.end()) {
BOOST_FOREACH(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);
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows()));
}
//******************************************************************************
GaussianFactor::shared_ptr QPSolver::createDualPrior(
const LinearInequality::shared_ptr& factor) const {
Vector sigmas = factor->get_model()->sigmas();
size_t n = sigmas.rows();
Matrix A = eye(n);
for (size_t i = 0; i<n; ++i) {
if (sigmas[i] == ACTIVE)
A(i,i) = 0;
}
Vector b = zero(n);
return boost::make_shared<JacobianFactor>(factor->dualKey(), A, b);
}
//******************************************************************************
@ -67,6 +82,11 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
// Each constrained key becomes a factor in the dual graph
dualGraph->push_back(createDualFactor(key, workingSet, delta));
}
// Add prior for inactive dual variables
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
dualGraph->push_back(createDualPrior(factor));
}
return dualGraph;
}
@ -224,13 +244,33 @@ QPState QPSolver::iterate(const QPState& state) const {
}
}
//******************************************************************************
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
const VectorValues& initialValues) const {
LinearInequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
Vector e = workingFactor->error_vector(initialValues);
Vector sigmas = zero(e.rows());
for (int i = 0; i < e.rows(); ++i){
if (fabs(e[i])>1e-7){
sigmas[i] = INACTIVE;
}
}
workingFactor->setModel(true, sigmas);
workingSet.push_back(workingFactor);
}
return workingSet;
}
//******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initialValues) const {
// TODO: initialize workingSet from the feasible initialValues
LinearInequalityFactorGraph workingSet(qp_.inequalities);
LinearInequalityFactorGraph workingSet =
identifyActiveConstraints(qp_.inequalities, initialValues);
QPState state(initialValues, VectorValues(), workingSet, false);
/// main loop of the solver

29
gtsam_unstable/linear/QPSolver.h
View File

@ -13,6 +13,9 @@
#include <vector>
#include <set>
#define ACTIVE 0.0
#define INACTIVE std::numeric_limits<double>::infinity()
namespace gtsam {
/// This struct holds the state of QPSolver at each iteration
@ -67,10 +70,12 @@ public:
const FactorGraph<FACTOR>& graph,
const VariableIndex& variableIndex) const {
std::vector<std::pair<Key, Matrix> > Aterms;
BOOST_FOREACH(size_t factorIx, variableIndex[key]){
typename FACTOR::shared_ptr factor = graph.at(factorIx);
Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
if (variableIndex.find(key) != variableIndex.end()) {
BOOST_FOREACH(size_t factorIx, variableIndex[key]){
typename FACTOR::shared_ptr factor = graph.at(factorIx);
Matrix Ai = factor->getA(factor->find(key)).transpose();
Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
}
}
return Aterms;
}
@ -80,6 +85,15 @@ public:
const LinearInequalityFactorGraph& workingSet,
const VectorValues& delta) const;
/**
* Create dummy prior for inactive dual variables
* TODO: This might be inefficient but I don't know how to avoid
* the Indeterminant exception due to no information for the duals
* on inactive components of the constraints.
*/
GaussianFactor::shared_ptr createDualPrior(
const LinearInequality::shared_ptr& factor) const;
/**
* Build the dual graph to solve for the Lagrange multipliers.
*
@ -173,6 +187,13 @@ public:
/** 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, otherwise the solution will be wrong.

100
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -26,9 +26,6 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
#define TEST_DISABLED(testGroup, testName)\
void testGroup##testName##Test(TestResult& result_, const std::string& name_)
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
QP createTestCase() {
@ -73,7 +70,7 @@ TEST(QPSolver, constraintsAux) {
int factorIx, lambdaIx;
boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint(
qp.inequalities, lambdas);
LONGS_EQUAL(1, factorIx);
LONGS_EQUAL(0, factorIx);
LONGS_EQUAL(2, lambdaIx);
VectorValues lambdas2;
@ -122,37 +119,80 @@ TEST(QPSolver, dual) {
qp.inequalities, initialValues);
VectorValues dual = dualGraph->optimize();
VectorValues expectedDual;
expectedDual.insert(1, (Vector(1) << 2.0));
expectedDual.insert(0, (Vector(1) << 2.0));
CHECK(assert_equal(expectedDual, dual, 1e-10));
}
/* ************************************************************************* */
TEST(QPSolver, indentifyActiveConstraints) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues currentSolution;
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
Vector actualSigmas = workingSet.at(0)->get_model()->sigmas();
Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE);
CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100));
VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 0.0));
expectedSolution.insert(X(2), (Vector(1) << 0.0));
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
/* ************************************************************************* */
TEST(QPSolver, iterate) {
QP qp = createTestCase();
QPSolver solver(qp);
VectorValues currentSolution;
currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1));
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0));
expectedSolutions[0].insert(X(2), (Vector(1) << 0.0));
expectedDuals[0].insert(0, (Vector(4) << 0, 3, 0, 0));
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5));
expectedSolutions[1].insert(X(2), (Vector(1) << 0.0));
expectedDuals[1].insert(0, (Vector(4) << 0, 0, 1.5, 0));
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5));
expectedSolutions[2].insert(X(2), (Vector(1) << 0.75));
expectedDuals[2].insert(0, (Vector(4) << 0, 0, 1.5, 0));
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5));
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5));
expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0));
LinearInequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPState state(currentSolution, VectorValues(), workingSet, false);
int it = 0;
while (!state.converged) {
state = solver.iterate(state);
// These checks will fail because the expected solutions obtained from
// Forst10book do not follow exactly what we implemented from Nocedal06book.
// Specifically, we do not re-identify active constraints and
// do not recompute dual variables after every step!!!
// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
it++;
}
CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10));
}
//TEST(QPSolver, iterate) {
// QP qp = createTestCase();
// QPSolver solver(qp);
//
// VectorValues currentSolution;
// currentSolution.insert(X(1), zero(1));
// currentSolution.insert(X(2), zero(1));
//
// std::vector<VectorValues> expectedSolutions(3);
// expectedSolutions[0].insert(X(1), (Vector(1) << 4.0 / 3.0));
// expectedSolutions[0].insert(X(2), (Vector(1) << 2.0 / 3.0));
// expectedSolutions[1].insert(X(1), (Vector(1) << 1.5));
// expectedSolutions[1].insert(X(2), (Vector(1) << 0.5));
// expectedSolutions[2].insert(X(1), (Vector(1) << 1.5));
// expectedSolutions[2].insert(X(2), (Vector(1) << 0.5));
//
// bool converged = false;
// int it = 0;
// while (!converged) {
// VectorValues lambdas;
// converged = solver.iterateInPlace(workingGraph, currentSolution, lambdas);
// CHECK(assert_equal(expectedSolutions[it], currentSolution, 1e-100));
// it++;
// }
//}
/* ************************************************************************* */
TEST(QPSolver, optimizeForst10book_pg171Ex5) {