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

unittest for QPSolver without initial point

release/4.3a0
thduynguyen 2014-05-02 10:18:01 -04:00
parent 882a1fe22f
commit 2fd3cf1bd0
3 changed files with 43 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/linear/QPSolver.cpp
View File

@ -194,11 +194,6 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
else {
// Enforce constrained noise model so lambdas are solved with QR
// and should exactly satisfy all the equations
for (size_t i = 0; i<lambdaTerms.size(); ++i) {
cout << "Term " << i << ":" << endl;
cout << lambdaTerms[i].first << endl;
cout << lambdaTerms[i].second << endl;
}
if (debug) cout << gradf_xi << endl;
dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi,
noiseModel::Constrained::All(gradf_xi.size())));
@ -271,7 +266,7 @@ bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph,
*/
boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGraph& workingGraph,
const VectorValues& xk, const VectorValues& p) const {
static bool debug = true;
static bool debug = false;
double minAlpha = 1.0;
int closestFactorIx = -1, closestSigmaIx = -1;
@ -323,7 +318,7 @@ boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGra
/* ************************************************************************* */
bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution, VectorValues& lambdas) const {
static bool debug = true;
static bool debug = false;
if (debug) workingGraph.print("workingGraph: ");
// Obtain the solution from the current working graph
VectorValues newSolution = workingGraph.optimize();
@ -367,7 +362,6 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
/* ************************************************************************* */
VectorValues QPSolver::optimize(const VectorValues& initials,
boost::optional<VectorValues&> lambdas) const {
cout << "QP optimize.." << endl;
GaussianFactorGraph workingGraph = graph_.clone();
VectorValues currentSolution = initials;
VectorValues workingLambdas;
@ -468,7 +462,7 @@ std::pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP
}
/* ************************************************************************* */
VectorValues QPSolver::findFeasibleInitialValues() const {
std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
// Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
VectorValues initials;
size_t firstSlackKey;
@ -487,7 +481,9 @@ VectorValues QPSolver::findFeasibleInitialValues() const {
VectorValues solution = lpSolver.solve();
// Remove slack variables from solution
double slackSum = 0.0;
for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) {
slackSum += solution.at(key).cwiseAbs().sum();
solution.erase(key);
}
@ -500,12 +496,17 @@ VectorValues QPSolver::findFeasibleInitialValues() const {
}
}
return solution;
return make_pair(slackSum<1e-5, solution);
}
/* ************************************************************************* */
VectorValues QPSolver::optimize(boost::optional<VectorValues&> lambdas) const {
VectorValues initials = findFeasibleInitialValues();
bool isFeasible;
VectorValues initials;
boost::tie(isFeasible, initials) = findFeasibleInitialValues();
if (!isFeasible) {
throw std::runtime_error("LP subproblem is infeasible!");
}
return optimize(initials, lambdas);
}

2
gtsam/linear/QPSolver.h
View File

@ -150,7 +150,7 @@ public:
std::pair<GaussianFactorGraph::shared_ptr, VectorValues> constraintsLP(Key firstSlackKey) const;
/// Find a feasible initial point
VectorValues findFeasibleInitialValues() const;
std::pair<bool, VectorValues> findFeasibleInitialValues() const;
/// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed
/// TODO: Move to GaussianFactor?

37
gtsam/linear/tests/testQPSolver.cpp
View File

@ -249,7 +249,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
/* ************************************************************************* */
// 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
// x1 - 2 x2 - 1 >=0
// so that the trivial initial point (0,0) is infeasible
GaussianFactorGraph modifyNocedal06bookEx16_4() {
GaussianFactorGraph graph;
@ -260,7 +260,7 @@ GaussianFactorGraph modifyNocedal06bookEx16_4() {
// 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), -1*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));
@ -284,9 +284,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
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)));
@ -299,17 +297,42 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
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(3), -ones(1,1),-1*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: ");
bool isFeasible;
VectorValues initials;
boost::tie(isFeasible, initials) = solver.findFeasibleInitialValues();
EXPECT(assert_equal(1.0*ones(1), initials.at(X(1))));
EXPECT(assert_equal(0.0*ones(1), initials.at(X(2))));
VectorValues solution = solver.optimize();
EXPECT(assert_equal(2.0*ones(1), solution.at(X(1))));
EXPECT(assert_equal(0.5*ones(1), solution.at(X(2))));
}
TEST(QPSolver, optimizeNocedal06bookEx16_4_2) {
GaussianFactorGraph graph = createTestNocedal06bookEx16_4();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), (Vector(1)<<0.0));
initials.insert(X(2), (Vector(1)<<100.0));
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1)<< 1.4));
expectedSolution.insert(X(2), (Vector(1)<< 1.7));
VectorValues solution = solver.optimize(initials);
// THIS should fail because of the bad infeasible initial point!!
// CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution2 = solver.optimize();
CHECK(assert_equal(expectedSolution, solution2, 1e-7));
}
/* ************************************************************************* */
int main() {