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

Merge pull request #546 from borglab/fix/LP_QP_stype 

LP/QP Style Update
release/4.3a0
Frank Dellaert 2020-10-02 18:01:53 -04:00 committed by GitHub
parent 3f3e286a4d a8ea6f2bd2
commit 188efad2c5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 221 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam_unstable/linear/ActiveSetSolver-inl.h
View File

@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
// to compute the least-square approximation of dual variables // to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b); return boost::make_shared<JacobianFactor>(Aterms, b);
} else { } else {
return boost::make_shared<JacobianFactor>(); return nullptr;
} }
} }
@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
* if lambda = 0 you are on the constraint * if lambda = 0 you are on the constraint
* if lambda > 0 you are violating the constraint. * if lambda > 0 you are violating the constraint.
*/ */
Template GaussianFactorGraph::shared_ptr This::buildDualGraph( Template GaussianFactorGraph This::buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); GaussianFactorGraph dualGraph;
for (Key key : constrainedKeys_) { for (Key key : constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph // Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor = auto dualFactor = createDualFactor(key, workingSet, delta);
createDualFactor(key, workingSet, delta); if (dualFactor) dualGraph.push_back(dualFactor);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
} }
return dualGraph; return dualGraph;
} }
@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
Template typename This::State This::iterate( Template typename This::State This::iterate(
const typename This::State& state) const { const typename This::State& state) const {
// Algorithm 16.3 from Nocedal06book. // Algorithm 16.3 from Nocedal06book.
// Solve with the current working set eqn 16.39, but instead of solving for p // Solve with the current working set eqn 16.39, but solve for x not p
// solve for x auto workingGraph = buildWorkingGraph(state.workingSet, state.values);
GaussianFactorGraph workingGraph =
buildWorkingGraph(state.workingSet, state.values);
VectorValues newValues = workingGraph.optimize(); VectorValues newValues = workingGraph.optimize();
// If we CAN'T move further // If we CAN'T move further
// if p_k = 0 is the original condition, modified by Duy to say that the state // if p_k = 0 is the original condition, modified by Duy to say that the state
// update is zero. // update is zero.
if (newValues.equals(state.values, 1e-7)) { if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph // Compute lambda from the dual graph
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, auto dualGraph = buildDualGraph(state.workingSet, newValues);
newValues); VectorValues duals = dualGraph.optimize();
VectorValues duals = dualGraph->optimize();
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!! // If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) { if (leavingFactor < 0) {

4
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -154,8 +154,8 @@ protected:
public: /// Just for testing... public: /// Just for testing...
/// Builds a dual graph from the current working set. /// Builds a dual graph from the current working set.
GaussianFactorGraph::shared_ptr buildDualGraph( GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet,
const InequalityFactorGraph& workingSet, const VectorValues& delta) const; const VectorValues &delta) const;
/** /**
* Build a working graph of cost, equality and active inequality constraints * Build a working graph of cost, equality and active inequality constraints

5
gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -31,6 +31,11 @@ class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public: public:
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr; typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
/// Add a linear inequality, forwards arguments to LinearInequality.
template <class... Args> void add(Args &&... args) {
emplace_shared<LinearEquality>(std::forward<Args>(args)...);
}
/// Compute error of a guess. /// Compute error of a guess.
double error(const VectorValues& x) const { double error(const VectorValues& x) const {
double total_error = 0.; double total_error = 0.;

5
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -47,6 +47,11 @@ public:
return Base::equals(other, tol); return Base::equals(other, tol);
} }
/// Add a linear inequality, forwards arguments to LinearInequality.
template <class... Args> void add(Args &&... args) {
emplace_shared<LinearInequality>(std::forward<Args>(args)...);
}
/** /**
* Compute error of a guess. * Compute error of a guess.
* Infinity error if it violates an inequality; zero otherwise. */ * Infinity error if it violates an inequality; zero otherwise. */

3
gtsam_unstable/linear/LPInitSolver.h
View File

@ -21,7 +21,6 @@
#include <gtsam_unstable/linear/LP.h> #include <gtsam_unstable/linear/LP.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/Test.h>
namespace gtsam { namespace gtsam {
/** /**
@ -83,7 +82,7 @@ private:
const InequalityFactorGraph& inequalities) const; const InequalityFactorGraph& inequalities) const;
// friend class for unit-testing private methods // friend class for unit-testing private methods
FRIEND_TEST(LPInitSolver, initialization); friend class LPInitSolverInitializationTest;
}; };
} }

130
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -16,21 +16,23 @@
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
*/ */
#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h> #include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h> #include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h> #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolver.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand; using namespace gtsam::symbol_shorthand;
@ -48,36 +50,26 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
LP simpleLP1() { LP simpleLP1() {
LP lp; LP lp;
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.push_back( lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0
LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0 lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0
lp.inequalities.push_back( lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4
LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0 lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12
lp.inequalities.push_back( lp.inequalities.add(1, Vector2(-1, 1), 1, 5); // -x1 + x2 <= 1
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; return lp;
} }
/* ************************************************************************* */ /* ************************************************************************* */
namespace gtsam { namespace gtsam {
TEST(LPInitSolver, infinite_loop_single_var) { TEST(LPInitSolver, InfiniteLoopSingleVar) {
LP initchecker; LP lp;
initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
initchecker.inequalities.push_back( lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2
LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2 lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6
initchecker.inequalities.push_back( lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0
LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6 lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20
initchecker.inequalities.push_back( lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0
LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0 LPSolver solver(lp);
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; VectorValues starter;
starter.insert(1, Vector3(0, 0, 2)); starter.insert(1, Vector3(0, 0, 2));
VectorValues results, duals; VectorValues results, duals;
@ -87,25 +79,23 @@ TEST(LPInitSolver, infinite_loop_single_var) {
CHECK(assert_equal(results, expected, 1e-7)); CHECK(assert_equal(results, expected, 1e-7));
} }
TEST(LPInitSolver, infinite_loop_multi_var) { TEST(LPInitSolver, InfiniteLoopMultiVar) {
LP initchecker; LP lp;
Key X = symbol('X', 1); Key X = symbol('X', 1);
Key Y = symbol('Y', 1); Key Y = symbol('Y', 1);
Key Z = symbol('Z', 1); Key Z = symbol('Z', 1);
initchecker.cost = LinearCost(Z, kOne); // min alpha lp.cost = LinearCost(Z, kOne); // min alpha
initchecker.inequalities.push_back( lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, 1); //-2x-y-alpha <= -2
1)); //-2x-y-alpha <= -2 lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
initchecker.inequalities.push_back( 2); // -x+2y-alpha <= 6
LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0,
2)); // -x+2y-alpha <= 6 3); // -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality( lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20,
X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0 4); // x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality( lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0,
X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20 5); // -y - alpha <= 0
initchecker.inequalities.push_back(LinearInequality( LPSolver solver(lp);
Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0
LPSolver solver(initchecker);
VectorValues starter; VectorValues starter;
starter.insert(X, kZero); starter.insert(X, kZero);
starter.insert(Y, kZero); starter.insert(Y, kZero);
@ -119,7 +109,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
CHECK(assert_equal(results, expected, 1e-7)); CHECK(assert_equal(results, expected, 1e-7));
} }
TEST(LPInitSolver, initialization) { TEST(LPInitSolver, Initialization) {
LP lp = simpleLP1(); LP lp = simpleLP1();
LPInitSolver initSolver(lp); LPInitSolver initSolver(lp);
@ -138,19 +128,19 @@ TEST(LPInitSolver, initialization) {
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey); LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
LP expectedInitLP; LP expectedInitLP;
expectedInitLP.cost = LinearCost(yKey, kOne); expectedInitLP.cost = LinearCost(yKey, kOne);
expectedInitLP.inequalities.push_back(LinearInequality( expectedInitLP.inequalities.add(1, Vector2(-1, 0), 2, Vector::Constant(1, -1),
1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 0, 1); // -x1 - y <= 0
expectedInitLP.inequalities.push_back(LinearInequality( expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1),
1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 0, 2); // -x2 - y <= 0
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1),
LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4, 4,
3)); // x1 + 2*x2 - y <= 4 3); // x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1),
LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12, 12,
4)); // 4x1 + 2x2 - y <= 12 4); // 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.add(1, Vector2(-1, 1), 2, Vector::Constant(1, -1),
LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1, 1,
5)); // -x1 + x2 - y <= 1 5); // -x1 + x2 - y <= 1
CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
LPSolver lpSolveInit(*initLP); LPSolver lpSolveInit(*initLP);
VectorValues xy0(x0); VectorValues xy0(x0);
@ -164,7 +154,7 @@ TEST(LPInitSolver, initialization) {
VectorValues x = initSolver.solve(); VectorValues x = initSolver.solve();
CHECK(lp.isFeasible(x)); CHECK(lp.isFeasible(x));
} }
} } // namespace gtsam
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
@ -173,28 +163,24 @@ TEST(LPInitSolver, initialization) {
* x - y = 5 * x - y = 5
* x + 2y = 6 * x + 2y = 6
*/ */
TEST(LPSolver, overConstrainedLinearSystem) { TEST(LPSolver, OverConstrainedLinearSystem) {
GaussianFactorGraph graph; GaussianFactorGraph graph;
Matrix A1 = Vector3(1, 1, 1); Matrix A1 = Vector3(1, 1, 1);
Matrix A2 = Vector3(1, -1, 2); Matrix A2 = Vector3(1, -1, 2);
Vector b = Vector3(1, 5, 6); Vector b = Vector3(1, 5, 6);
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
graph.push_back(factor);
VectorValues x = graph.optimize(); VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle // This check confirms that gtsam linear constraint solver can't handle
// over-constrained system // over-constrained system
CHECK(factor.error(x) != 0.0); CHECK(graph[0]->error(x) != 0.0);
} }
TEST(LPSolver, overConstrainedLinearSystem2) { TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph; GaussianFactorGraph graph;
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, I_1x1, kOne, graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1));
noiseModel::Constrained::All(1)); graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, -I_1x1, 5 * kOne, graph.add(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, noiseModel::Constrained::All(1));
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1));
VectorValues x = graph.optimize(); VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle // This check confirms that gtsam linear constraint solver can't handle
// over-constrained system // over-constrained system
@ -202,7 +188,7 @@ TEST(LPSolver, overConstrainedLinearSystem2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LPSolver, simpleTest1) { TEST(LPSolver, SimpleTest1) {
LP lp = simpleLP1(); LP lp = simpleLP1();
LPSolver lpSolver(lp); LPSolver lpSolver(lp);
VectorValues init; VectorValues init;
@ -222,7 +208,7 @@ TEST(LPSolver, simpleTest1) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) { TEST(LPSolver, TestWithoutInitialValues) {
LP lp = simpleLP1(); LP lp = simpleLP1();
LPSolver lpSolver(lp); LPSolver lpSolver(lp);
VectorValues result, duals, expectedResult; VectorValues result, duals, expectedResult;

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

@ -17,10 +17,12 @@
* @author Ivan Dario Jimenez * @author Ivan Dario Jimenez
*/ */
#include <gtsam_unstable/linear/QPSParser.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/linear/QPSParser.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
@ -40,15 +42,15 @@ QP createTestCase() {
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 // 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 //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 // Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back( qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1,
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0));
Z_1x1, 10.0));
// Inequality constraints // 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.add(X(1), I_1x1, X(2), I_1x1, 2,
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 0); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 qp.inequalities.add(X(1), -I_1x1, 0, 1); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 qp.inequalities.add(X(2), -I_1x1, 0, 2); // -x2 <= 0
qp.inequalities.add(X(1), I_1x1, 1.5, 3); // x1 <= 3/2
return qp; return qp;
} }
@ -94,16 +96,15 @@ QP createEqualityConstrainedTest() {
// Note the Hessian encodes: // Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // 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 // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back( qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1,
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, 0.0));
0.0));
// Equality constraints // Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
Matrix A1 = I_1x1; Matrix A1 = I_1x1;
Matrix A2 = I_1x1; Matrix A2 = I_1x1;
Vector b = -kOne; Vector b = -kOne;
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); qp.equalities.add(X(1), A1, X(2), A2, b, 0);
return qp; return qp;
} }
@ -118,9 +119,8 @@ TEST(QPSolver, dual) {
QPSolver solver(qp); QPSolver solver(qp);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( auto dualGraph = solver.buildDualGraph(qp.inequalities, initialValues);
qp.inequalities, initialValues); VectorValues dual = dualGraph.optimize();
VectorValues dual = dualGraph->optimize();
VectorValues expectedDual; VectorValues expectedDual;
expectedDual.insert(0, (Vector(1) << 2.0).finished()); expectedDual.insert(0, (Vector(1) << 2.0).finished());
CHECK(assert_equal(expectedDual, dual, 1e-10)); CHECK(assert_equal(expectedDual, dual, 1e-10));
@ -135,19 +135,19 @@ TEST(QPSolver, indentifyActiveConstraints) {
currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1); currentSolution.insert(X(2), Z_1x1);
InequalityFactorGraph workingSet = solver.identifyActiveConstraints( auto workingSet =
qp.inequalities, currentSolution); solver.identifyActiveConstraints(qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active());// active CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active());// active CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active());// inactive CHECK(!workingSet.at(3)->active()); // inactive
VectorValues solution = solver.buildWorkingGraph(workingSet).optimize(); VectorValues solution = solver.buildWorkingGraph(workingSet).optimize();
VectorValues expectedSolution; VectorValues expected;
expectedSolution.insert(X(1), kZero); expected.insert(X(1), kZero);
expectedSolution.insert(X(2), kZero); expected.insert(X(2), kZero);
CHECK(assert_equal(expectedSolution, solution, 1e-100)); CHECK(assert_equal(expected, solution, 1e-100));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -159,24 +159,24 @@ TEST(QPSolver, iterate) {
currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1); currentSolution.insert(X(2), Z_1x1);
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4); std::vector<VectorValues> expected(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), kZero); expected[0].insert(X(1), kZero);
expectedSolutions[0].insert(X(2), kZero); expected[0].insert(X(2), kZero);
expectedDuals[0].insert(1, (Vector(1) << 3).finished()); expectedDuals[0].insert(1, (Vector(1) << 3).finished());
expectedDuals[0].insert(2, kZero); expectedDuals[0].insert(2, kZero);
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); expected[1].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[1].insert(X(2), kZero); expected[1].insert(X(2), kZero);
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); expected[2].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished()); expected[2].insert(X(2), (Vector(1) << 0.75).finished());
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expected[3].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); expected[3].insert(X(2), (Vector(1) << 0.5).finished());
InequalityFactorGraph workingSet = solver.identifyActiveConstraints( auto workingSet =
qp.inequalities, currentSolution); solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPSolver::State state(currentSolution, VectorValues(), workingSet, false, QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
100); 100);
@ -188,12 +188,12 @@ TEST(QPSolver, iterate) {
// Forst10book do not follow exactly what we implemented from Nocedal06book. // Forst10book do not follow exactly what we implemented from Nocedal06book.
// Specifically, we do not re-identify active constraints and // Specifically, we do not re-identify active constraints and
// do not recompute dual variables after every step!!! // do not recompute dual variables after every step!!!
// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); // CHECK(assert_equal(expected[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); // CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
it++; it++;
} }
CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); CHECK(assert_equal(expected[3], state.values, 1e-10));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -204,182 +204,161 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
VectorValues initialValues; VectorValues initialValues;
initialValues.insert(X(1), Z_1x1); initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1); initialValues.insert(X(2), Z_1x1);
VectorValues solution; VectorValues solution = solver.optimize(initialValues).first;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expected;
VectorValues expectedSolution; expected.insert(X(1), (Vector(1) << 1.5).finished());
expectedSolution.insert(X(1), (Vector(1) << 1.5).finished()); expected.insert(X(2), (Vector(1) << 0.5).finished());
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); CHECK(assert_equal(expected, solution, 1e-100));
CHECK(assert_equal(expectedSolution, solution, 1e-100));
} }
pair<QP, QP> testParser(QPSParser parser) { pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse(); QP exampleqp = parser.Parse();
QP expectedqp; QP expected;
Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back( expected.cost.push_back(HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1,
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, 2.0 * kOne,
2.0 * kOne, 8.0)); 8.0));
// 2x + y >= 2
// -x + 2y <= 6 expected.inequalities.add(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0); // 2x + y >= 2
expectedqp.inequalities.push_back( expected.inequalities.add(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1); // -x + 2y <= 6
LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0)); expected.inequalities.add(X1, I_1x1, 20, 4); // x<= 20
expectedqp.inequalities.push_back( expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0
LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1)); expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0
// x<= 20 return {expected, exampleqp};
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) { TEST(QPSolver, ParserSyntaticTest) {
auto expectedActual = testParser(QPSParser("QPExample.QPS")); auto result = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, CHECK(assert_equal(result.first.cost, result.second.cost, 1e-7));
CHECK(assert_equal(result.first.inequalities, result.second.inequalities,
1e-7)); 1e-7));
CHECK(assert_equal(expectedActual.first.inequalities, CHECK(assert_equal(result.first.equalities, result.second.equalities, 1e-7));
expectedActual.second.inequalities, 1e-7));
CHECK(assert_equal(expectedActual.first.equalities,
expectedActual.second.equalities, 1e-7));
} }
TEST(QPSolver, ParserSemanticTest) { TEST(QPSolver, ParserSemanticTest) {
auto expected_actual = testParser(QPSParser("QPExample.QPS")); auto result = testParser(QPSParser("QPExample.QPS"));
VectorValues actualSolution, expectedSolution; VectorValues expected = QPSolver(result.first).optimize().first;
boost::tie(expectedSolution, boost::tuples::ignore) = VectorValues actual = QPSolver(result.second).optimize().first;
QPSolver(expected_actual.first).optimize(); CHECK(assert_equal(actual, expected, 1e-7));
boost::tie(actualSolution, boost::tuples::ignore) =
QPSolver(expected_actual.second).optimize();
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
} }
TEST(QPSolver, QPExampleTest){ TEST(QPSolver, QPExampleTest) {
QP problem = QPSParser("QPExample.QPS").Parse(); QP problem = QPSParser("QPExample.QPS").Parse();
VectorValues actualSolution;
auto solver = QPSolver(problem); auto solver = QPSolver(problem);
boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize(); VectorValues actual = solver.optimize().first;
VectorValues expectedSolution; VectorValues expected;
expectedSolution.insert(Symbol('X',1),0.7625*I_1x1); expected.insert(Symbol('X', 1), 0.7625 * I_1x1);
expectedSolution.insert(Symbol('X',2),0.4750*I_1x1); expected.insert(Symbol('X', 2), 0.4750 * I_1x1);
double error_expected = problem.cost.error(expectedSolution); double error_expected = problem.cost.error(expected);
double error_actual = problem.cost.error(actualSolution); double error_actual = problem.cost.error(actual);
CHECK(assert_equal(expectedSolution, actualSolution, 1e-7)) CHECK(assert_equal(expected, actual, 1e-7))
CHECK(assert_equal(error_expected, error_actual)) CHECK(assert_equal(error_expected, error_actual))
} }
TEST(QPSolver, HS21) { TEST(QPSolver, HS21) {
QP problem = QPSParser("HS21.QPS").Parse(); QP problem = QPSParser("HS21.QPS").Parse();
VectorValues actualSolution; VectorValues expected;
VectorValues expectedSolution; expected.insert(Symbol('X', 1), 2.0 * I_1x1);
expectedSolution.insert(Symbol('X',1), 2.0*I_1x1); expected.insert(Symbol('X', 2), 0.0 * I_1x1);
expectedSolution.insert(Symbol('X',2), 0.0*I_1x1); VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(-99.9599999, error_actual, 1e-7)) CHECK(assert_equal(-99.9599999, error_actual, 1e-7))
CHECK(assert_equal(expectedSolution, actualSolution)) CHECK(assert_equal(expected, actual))
} }
TEST(QPSolver, HS35) { TEST(QPSolver, HS35) {
QP problem = QPSParser("HS35.QPS").Parse(); QP problem = QPSParser("HS35.QPS").Parse();
VectorValues actualSolution; VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution); CHECK(assert_equal(1.11111111e-01, error_actual, 1e-7))
CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7))
} }
TEST(QPSolver, HS35MOD) { TEST(QPSolver, HS35MOD) {
QP problem = QPSParser("HS35MOD.QPS").Parse(); QP problem = QPSParser("HS35MOD.QPS").Parse();
VectorValues actualSolution; VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution); CHECK(assert_equal(2.50000001e-01, error_actual, 1e-7))
CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7))
} }
TEST(QPSolver, HS51) { TEST(QPSolver, HS51) {
QP problem = QPSParser("HS51.QPS").Parse(); QP problem = QPSParser("HS51.QPS").Parse();
VectorValues actualSolution; VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution); CHECK(assert_equal(8.88178420e-16, error_actual, 1e-7))
CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7))
} }
TEST(QPSolver, HS52) { TEST(QPSolver, HS52) {
QP problem = QPSParser("HS52.QPS").Parse(); QP problem = QPSParser("HS52.QPS").Parse();
VectorValues actualSolution; VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution); CHECK(assert_equal(5.32664756, error_actual, 1e-7))
CHECK(assert_equal(5.32664756,error_actual, 1e-7))
} }
TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of
// tolerance than the rest
QP problem = QPSParser("HS268.QPS").Parse(); QP problem = QPSParser("HS268.QPS").Parse();
VectorValues actualSolution; VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution); CHECK(assert_equal(5.73107049e-07, error_actual, 1e-6))
CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6))
} }
TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix
QP problem = QPSParser("QPTEST.QPS").Parse(); QP problem = QPSParser("QPTEST.QPS").Parse();
VectorValues actualSolution; VectorValues actual = QPSolver(problem).optimize().first;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); double error_actual = problem.cost.error(actual);
double error_actual = problem.cost.error(actualSolution); CHECK(assert_equal(0.437187500e01, error_actual, 1e-7))
CHECK(assert_equal(0.437187500e01,error_actual, 1e-7))
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html // Create Matlab's test graph as in
// http://www.mathworks.com/help/optim/ug/quadprog.html
QP createTestMatlabQPEx() { QP createTestMatlabQPEx() {
QP qp; QP qp;
// Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2 // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2
// Note the Hessian encodes: // Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // 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 // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back( qp.cost.push_back(HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, 6 * I_1x1, 1000.0));
6 * I_1x1, 1000.0));
// Inequality constraints // Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, 0); // x1 + x2 <= 2
qp.inequalities.push_back( qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1); //-x1 + 2*x2 <=2
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 qp.inequalities.add(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2); // 2*x1 + x2 <=3
qp.inequalities.push_back( qp.inequalities.add(X(1), -I_1x1, 0, 3); // -x1 <= 0
LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 qp.inequalities.add(X(2), -I_1x1, 0, 4); // -x2 <= 0
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; return qp;
} }
///* ************************************************************************* */ ///* *************************************************************************
///*/
TEST(QPSolver, optimizeMatlabEx) { TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx(); QP qp = createTestMatlabQPEx();
QPSolver solver(qp); QPSolver solver(qp);
VectorValues initialValues; VectorValues initialValues;
initialValues.insert(X(1), Z_1x1); initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1); initialValues.insert(X(2), Z_1x1);
VectorValues solution; VectorValues solution = solver.optimize(initialValues).first;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expected;
VectorValues expectedSolution; expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); CHECK(assert_equal(expected, solution, 1e-7));
CHECK(assert_equal(expectedSolution, solution, 1e-7));
} }
///* ************************************************************************* */ ///* *************************************************************************
///*/
TEST(QPSolver, optimizeMatlabExNoinitials) { TEST(QPSolver, optimizeMatlabExNoinitials) {
QP qp = createTestMatlabQPEx(); QP qp = createTestMatlabQPEx();
QPSolver solver(qp); QPSolver solver(qp);
VectorValues solution; VectorValues solution = solver.optimize().first;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(); VectorValues expected;
VectorValues expectedSolution; expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); CHECK(assert_equal(expected, solution, 1e-7));
CHECK(assert_equal(expectedSolution, solution, 1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -387,18 +366,15 @@ TEST(QPSolver, optimizeMatlabExNoinitials) {
QP createTestNocedal06bookEx16_4() { QP createTestNocedal06bookEx16_4() {
QP qp; QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1)); qp.cost.add(X(1), I_1x1, I_1x1);
qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1)); qp.cost.add(X(2), I_1x1, 2.5 * I_1x1);
// Inequality constraints // Inequality constraints
qp.inequalities.push_back( qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0);
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0)); qp.inequalities.add(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1);
qp.inequalities.push_back( qp.inequalities.add(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2);
LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1)); qp.inequalities.add(X(1), -I_1x1, 0.0, 3);
qp.inequalities.push_back( qp.inequalities.add(X(2), -I_1x1, 0.0, 4);
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; return qp;
} }
@ -410,21 +386,19 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
initialValues.insert(X(1), (Vector(1) << 2.0).finished()); initialValues.insert(X(1), (Vector(1) << 2.0).finished());
initialValues.insert(X(2), Z_1x1); initialValues.insert(X(2), Z_1x1);
VectorValues solution; VectorValues solution = solver.optimize(initialValues).first;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expected;
VectorValues expectedSolution; expected.insert(X(1), (Vector(1) << 1.4).finished());
expectedSolution.insert(X(1), (Vector(1) << 1.4).finished()); expected.insert(X(2), (Vector(1) << 1.7).finished());
expectedSolution.insert(X(2), (Vector(1) << 1.7).finished()); CHECK(assert_equal(expected, solution, 1e-7));
CHECK(assert_equal(expectedSolution, solution, 1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(QPSolver, failedSubproblem) { TEST(QPSolver, failedSubproblem) {
QP qp; QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); qp.cost.add(X(1), I_2x2, Z_2x1);
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
qp.inequalities.push_back( qp.inequalities.add(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; VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -433,8 +407,7 @@ TEST(QPSolver, failedSubproblem) {
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); QPSolver solver(qp);
VectorValues solution; VectorValues solution = solver.optimize(initialValues).first;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
CHECK(assert_equal(expected, solution, 1e-7)); CHECK(assert_equal(expected, solution, 1e-7));
} }
@ -442,10 +415,9 @@ TEST(QPSolver, failedSubproblem) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(QPSolver, infeasibleInitial) { TEST(QPSolver, infeasibleInitial) {
QP qp; QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2))); qp.cost.add(X(1), I_2x2, Vector::Zero(2));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0));
qp.inequalities.push_back( qp.inequalities.add(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; VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -464,4 +436,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */