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

[QP/LP] LP Added Zeros Error Fixed. 

[QP] Now Checks for syntactic Equality by comparing each factor imported .
[QP] Now Checks for semantic Equality by ensuring each imported QP gives the same solution.
release/4.3a0
ivan 2016-05-02 19:54:58 -04:00
parent 69c1fac81a
commit 23a1382bb2
6 changed files with 68 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/nonlinear/NonlinearFactor.h
View File

@ -131,7 +131,8 @@ public:
}
/**
* Creates a shared_ptr clone of the factor with different keys using
* Creates a shared_ptr clone of the
* factor with different keys using
* a map from old->new keys
*/
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;

35
gtsam_unstable/linear/LPSolver.cpp
View File

@ -13,7 +13,7 @@
namespace gtsam {
LPSolver::LPSolver(const LP &lp) :
lp_(lp) {
lp_(lp), addedZeroPriorsIndex_(){
// Push back factors that are the same in every iteration to the base graph.
// Those include the equality constraints and zero priors for keys that are
// not in the cost
@ -27,7 +27,11 @@ LPSolver::LPSolver(const LP &lp) :
// Create and push zero priors of constrained variables that do not exist in
// the cost function
// baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_));
auto addedZeroPriorsGraph = *createZeroPriors(lp_.cost.keys(), keysDim_);
//before we add them we fill an array with the indeces of the added zero priors
addedZeroPriorsIndex_.resize(addedZeroPriorsGraph.size());
std::iota(addedZeroPriorsIndex_.begin(), addedZeroPriorsIndex_.end(), baseGraph_.size());
baseGraph_.push_back(addedZeroPriorsGraph);
// Variable index
equalityVariableIndex_ = VariableIndex(lp_.equalities);
@ -54,8 +58,6 @@ LPState LPSolver::iterate(const LPState &state) const {
// to find the direction to move
VectorValues newValues = solveWithCurrentWorkingSet(state.values,
state.workingSet);
// GTSAM_PRINT(newValues);
// GTSAM_PRINT(state.values);
// If we CAN'T move further
// LP: projection on the constraints' nullspace is zero: we are at a vertex
if (newValues.equals(state.values, 1e-7)) {
@ -67,8 +69,6 @@ LPState LPSolver::iterate(const LPState &state) const {
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
// GTSAM_PRINT(*dualGraph);
// GTSAM_PRINT(duals);
// LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!!
@ -112,22 +112,18 @@ LPState LPSolver::iterate(const LPState &state) const {
GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
const LinearCost &cost, const VectorValues &xk) const {
GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph());
//Something in this function breaks when working with funcions that do not include all
//Variables in the cost function. When adding the missing variables as shown we still don't converge
//to the right answer as we would expect from the iterations.
// GTSAM_PRINT(xk);
// GTSAM_PRINT(cost);
for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) {
size_t dim = cost.getDim(it);
Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g
graph->push_back(JacobianFactor(*it, eye(dim), b));
}
KeySet allKeys = lp_.inequalities.keys();
allKeys.merge(lp_.equalities.keys());
allKeys.merge(KeySet(lp_.cost.keys()));
// for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
// add the corresponding factors for all variables that are not explicitly in the cost
//function
// for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
if (cost.keys().size() != allKeys.size()) {
KeySet difference;
std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(),
@ -136,20 +132,23 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
graph->push_back(JacobianFactor(k, eye(keysDim_.at(k)), xk.at(k)));
}
}
// GTSAM_PRINT(*graph);
return graph;
}
VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
const InequalityFactorGraph &workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
// We remove the old zero priors from the base graph we are going to use to solve
//This iteration's problem
for (size_t index : addedZeroPriorsIndex_) {
workingGraph.remove(index);
}
workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
for (const LinearInequality::shared_ptr &factor : workingSet) {
if (factor->active())
workingGraph.push_back(factor);
}
// GTSAM_PRINT(workingGraph);
return workingGraph.optimize();
}
@ -209,8 +208,6 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
/// main loop of the solver
while (!state.converged) {
// if(state.iterations > 100) // Temporary break to avoid infine loops
// break;
state = iterate(state);
}
return make_pair(state.values, state.duals);

2
gtsam_unstable/linear/LPSolver.h
View File

@ -23,7 +23,7 @@ typedef std::map<Key, size_t> KeyDimMap;
class LPSolver: public ActiveSetSolver {
const LP &lp_; //!< the linear programming problem
KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors
std::vector<size_t> addedZeroPriorsIndex_;
public:
/// Constructor
LPSolver(const LP &lp);

2
gtsam_unstable/linear/QPSParser.cpp
View File

@ -137,7 +137,7 @@ QP QPSParser::Parse() {
if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) {
throw QPSParserException();
} else {
std::cout << "Parse Successful." << std::endl;
// std::cout << "Parse Successful." << std::endl;
}
return rawData.makeQP();

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

@ -56,31 +56,6 @@ LP simpleLP1() {
/* ************************************************************************* */
namespace gtsam {
TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
LP initchecker;
Key X = symbol('X',1);
Key Y = symbol('Y',1);
Key Z = symbol('Z',1);
initchecker.cost = LinearCost(Z, ones(1)); //min alpha
// initchecker.cost = LinearCost(Z, ones(1), X, zero(1), Y, zero(1)); //min alpha
initchecker.inequalities.push_back(LinearInequality(X,-2.0*ones(1), Y, -1.0*ones(1), Z, -1.0*ones(1),-2,1));//-2x-y-alpha <= -2
initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Y, 2.0*ones(1), Z, -1.0*ones(1), 6, 2));// -x+2y-alpha <= 6
initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Z, -1.0*ones(1), 0,3));// -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality(X, 1.0*ones(1), Z, -1.0*ones(1), 20, 4));//x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality(Y, -1.0*ones(1), Z, -1.0*ones(1), 0, 5));// -y - alpha <= 0
LPSolver solver(initchecker);
VectorValues starter;
starter.insert(X, zero(1));
starter.insert(Y, zero(1));
starter.insert(Z, 2*ones(1));
VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected;
expected.insert(X, Vector::Constant(1, 13.5));
expected.insert(Y, Vector::Constant(1,6.5));
expected.insert(Z, Vector::Constant(1,-6.5));
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, infinite_loop_single_var) {
LP initchecker;
@ -99,6 +74,34 @@ TEST(LPInitSolverMatlab, infinite_loop_single_var) {
expected.insert(1, Vector3(13.5, 6.5, -6.5));
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
LP initchecker;
Key X = symbol('X', 1);
Key Y = symbol('Y', 1);
Key Z = symbol('Z', 1);
initchecker.cost = LinearCost(Z, ones(1)); //min alpha
initchecker.inequalities.push_back(
LinearInequality(X, -2.0 * ones(1), Y, -1.0 * ones(1), Z, -1.0 * ones(1), -2, 1));//-2x-y-alpha <= -2
initchecker.inequalities.push_back(
LinearInequality(X, -1.0 * ones(1), Y, 2.0 * ones(1), Z, -1.0 * ones(1), 6, 2));// -x+2y-alpha <= 6
initchecker.inequalities.push_back(LinearInequality(X, -1.0 * ones(1), Z, -1.0 * ones(1), 0, 3));// -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality(X, 1.0 * ones(1), Z, -1.0 * ones(1), 20, 4));//x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality(Y, -1.0 * ones(1), Z, -1.0 * ones(1), 0, 5));// -y - alpha <= 0
LPSolver solver(initchecker);
VectorValues starter;
starter.insert(X, zero(1));
starter.insert(Y, zero(1));
starter.insert(Z, 2 * ones(1));
VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected;
expected.insert(X, Vector::Constant(1, 13.5));
expected.insert(Y, Vector::Constant(1, 6.5));
expected.insert(Z, Vector::Constant(1, -6.5));
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, initialization) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);

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

@ -211,48 +211,42 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse();
// QP expectedqp = createExampleQP();
QP expectedqp;
Key X1(Symbol('X',1)), X2(Symbol('X',2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back(
HessianFactor(X(1), X(2), 8.0 * ones(1, 1), 2.0 * ones(1, 1),
HessianFactor(X1, X2, 8.0 * ones(1, 1), 2.0 * ones(1, 1),
1.5 * ones(1), 10.0 * ones(1, 1), -2.0 * ones(1), 4.0));
// 2x + y >= 2
expectedqp.inequalities.push_back(
LinearInequality(X(1), -2.0 * ones(1, 1), X(2), -ones(1, 1), -2, 0));
// -x + 2y <= 6
expectedqp.inequalities.push_back(
LinearInequality(X(1), -ones(1, 1), X(2), 2.0 * ones(1, 1), 6, 1));
//x >= 0
expectedqp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 2));
// y > = 0
expectedqp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 3));
LinearInequality(X2, -ones(1, 1), X1, -2.0 * ones(1, 1), -2, 0));
expectedqp.inequalities.push_back(
LinearInequality(X2, 2.0 * ones(1, 1), X1, -ones(1, 1), 6, 1));
// x<= 20
expectedqp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 20, 4));
expectedqp.inequalities.push_back(LinearInequality(X1, ones(1, 1), 20, 4));
//x >= 0
expectedqp.inequalities.push_back(LinearInequality(X1, -ones(1, 1), 0, 2));
// y > = 0
expectedqp.inequalities.push_back(LinearInequality(X2, -ones(1, 1), 0, 3));
return std::make_pair(expectedqp, exampleqp);
// CHECK(assert_equal(expectedqp.cost, exampleqp.cost, 1e-7));
// CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7));
// CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7));
// CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7));
};
TEST(QPSolver, QPExampleData) {
TEST(QPSolver, ParserSyntaticTest){
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
CHECK(assert_equal(expectedActual.first.inequalities, expectedActual.second.inequalities, 1e-7));
CHECK(assert_equal(expectedActual.first.equalities, expectedActual.second.equalities, 1e-7));
}
TEST(QPSolver, ParserSemanticTest) {
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
VectorValues actualSolution, expectedSolution;
expected_actual.first.print("EXPECTED GRAPH:");
expected_actual.second.print("ACTUAL GRAPH");
boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(expected_actual.first).optimize();
std::cout << "Expected Execution Works" << std::endl;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(expected_actual.second).optimize();
std::cout << "Actual Execution Works" << std::endl;
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
// testParser(QPSParser("AUG2D.QPS"));
// testParser(QPSParser("CONT-050.QPS"));
}
/* ************************************************************************* */