Skip to content

Commit

Permalink
Merge pull request #546 from borglab/fix/LP_QP_stype
Browse files Browse the repository at this point in the history 
LP/QP Style Update
  • Loading branch information
@dellaert
dellaert authored Oct 2, 2020
2 parents 3f3e286 + a8ea6f2 commit 188efad
Show file tree
Hide file tree
Showing 7 changed files with 221 additions and 259 deletions.
22 changes: 9 additions & 13 deletions gtsam_unstable/linear/ActiveSetSolver-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
// to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b);
} else {
return boost::make_shared<JacobianFactor>();
return nullptr;
}
}

Expand All @@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
* if lambda = 0 you are on 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 {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
GaussianFactorGraph dualGraph;
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);
auto dualFactor = createDualFactor(key, workingSet, delta);
if (dualFactor) dualGraph.push_back(dualFactor);
}
return dualGraph;
}
Expand All @@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
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);
// Solve with the current working set eqn 16.39, but solve for x not p
auto 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();
auto 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) {
Expand Down
4 changes: 2 additions & 2 deletions gtsam_unstable/linear/ActiveSetSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,8 +154,8 @@ class ActiveSetSolver {
public: /// Just for testing...

/// Builds a dual graph from the current working set.
GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet,
const VectorValues &delta) const;

/**
* Build a working graph of cost, equality and active inequality constraints
Expand Down
5 changes: 5 additions & 0 deletions gtsam_unstable/linear/EqualityFactorGraph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,11 @@ class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public:
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.
double error(const VectorValues& x) const {
double total_error = 0.;
Expand Down
5 changes: 5 additions & 0 deletions gtsam_unstable/linear/InequalityFactorGraph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,11 @@ class InequalityFactorGraph: public FactorGraph<LinearInequality> {
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.
* Infinity error if it violates an inequality; zero otherwise. */
Expand Down
3 changes: 1 addition & 2 deletions gtsam_unstable/linear/LPInitSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@

#include <gtsam_unstable/linear/LP.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/Test.h>

namespace gtsam {
/**
Expand Down Expand Up @@ -83,7 +82,7 @@ class LPInitSolver {
const InequalityFactorGraph& inequalities) const;

// friend class for unit-testing private methods
FRIEND_TEST(LPInitSolver, initialization);
friend class LPInitSolverInitializationTest;
};

}
132 changes: 59 additions & 73 deletions gtsam_unstable/linear/tests/testLPSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,21 +16,23 @@
* @author Duy-Nguyen Ta
*/

#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/LPSolver.h>

#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.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/VectorValues.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;
Expand All @@ -47,37 +49,27 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
*/
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
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0
lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0
lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4
lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12
lp.inequalities.add(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);
TEST(LPInitSolver, InfiniteLoopSingleVar) {
LP lp;
lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2
lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6
lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0
lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20
lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0
LPSolver solver(lp);
VectorValues starter;
starter.insert(1, Vector3(0, 0, 2));
VectorValues results, duals;
Expand All @@ -87,25 +79,23 @@ TEST(LPInitSolver, infinite_loop_single_var) {
CHECK(assert_equal(results, expected, 1e-7));
}

TEST(LPInitSolver, infinite_loop_multi_var) {
LP initchecker;
TEST(LPInitSolver, InfiniteLoopMultiVar) {
LP lp;
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);
lp.cost = LinearCost(Z, kOne); // min alpha
lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
1); //-2x-y-alpha <= -2
lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
2); // -x+2y-alpha <= 6
lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0,
3); // -x - alpha <= 0
lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20,
4); // x - alpha <= 20
lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0,
5); // -y - alpha <= 0
LPSolver solver(lp);
VectorValues starter;
starter.insert(X, kZero);
starter.insert(Y, kZero);
Expand All @@ -119,7 +109,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
CHECK(assert_equal(results, expected, 1e-7));
}

TEST(LPInitSolver, initialization) {
TEST(LPInitSolver, Initialization) {
LP lp = simpleLP1();
LPInitSolver initSolver(lp);

Expand All @@ -138,19 +128,19 @@ TEST(LPInitSolver, initialization) {
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
expectedInitLP.inequalities.add(1, Vector2(-1, 0), 2, Vector::Constant(1, -1),
0, 1); // -x1 - y <= 0
expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1),
0, 2); // -x2 - y <= 0
expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1),
4,
3); // x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1),
12,
4); // 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.add(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);
Expand All @@ -164,7 +154,7 @@ TEST(LPInitSolver, initialization) {
VectorValues x = initSolver.solve();
CHECK(lp.isFeasible(x));
}
}
} // namespace gtsam

/* ************************************************************************* */
/**
Expand All @@ -173,36 +163,32 @@ TEST(LPInitSolver, initialization) {
* x - y = 5
* x + 2y = 6
*/
TEST(LPSolver, overConstrainedLinearSystem) {
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);
graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(3));

VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
CHECK(factor.error(x) != 0.0);
CHECK(graph[0]->error(x) != 0.0);
}

TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph;
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, I_1x1, kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, -I_1x1, 5 * kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1));
graph.add(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) {
TEST(LPSolver, SimpleTest1) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues init;
Expand All @@ -222,7 +208,7 @@ TEST(LPSolver, simpleTest1) {
}

/* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) {
TEST(LPSolver, TestWithoutInitialValues) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues result, duals, expectedResult;
Expand Down
Loading

0 comments on commit 188efad

Please sign in to comment.