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

Removed deprecated functions

release/4.3a0
dellaert 2016-05-06 09:23:41 -07:00
parent 652242bcaa
commit a05857f56b
5 changed files with 50 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam_unstable/linear/LPSolver.cpp
View File

@ -9,7 +9,7 @@
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
#include <gtsam_unstable/linear/LPInitSolver.h>
namespace gtsam {
LPSolver::LPSolver(const LP &lp) :
@ -32,7 +32,6 @@ LPSolver::LPSolver(const LP &lp) :
constrainedKeys_.merge(lp_.inequalities.keys());
}
LPState LPSolver::iterate(const LPState &state) const {
// Solve with the current working set
// LP: project the objective neg. gradient to the constraint's null space
@ -96,21 +95,21 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
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));
graph->push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
}
KeySet allKeys = lp_.inequalities.keys();
allKeys.merge(lp_.equalities.keys());
allKeys.merge(KeySet(lp_.cost.keys()));
// 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
// 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(),
lp_.cost.end(), std::inserter(difference, difference.end()));
for (Key k : difference) {
graph->push_back(JacobianFactor(k, eye(keysDim_.at(k)), xk.at(k)));
size_t dim = keysDim_.at(k);
graph->push_back(JacobianFactor(k, Matrix::Identity(dim,dim), xk.at(k)));
}
}
return graph;
@ -137,16 +136,16 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the
// dual key
TermsContainer Aterms = collectDualJacobians < LinearEquality
> (key, lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
> (key, workingSet, inequalityVariableIndex_);
TermsContainer Aterms = collectDualJacobians<LinearEquality>(key,
lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end());
// Collect the gradients of unconstrained cost factors to the b vector
if (Aterms.size() > 0) {
Vector b = zero(delta.at(key).size());
Vector b = Vector::Zero(delta.at(key).size());
Factor::const_iterator it = lp_.cost.find(key);
if (it != lp_.cost.end())
b = lp_.cost.getA(it).transpose();
@ -203,7 +202,7 @@ boost::tuples::tuple<double, int> LPSolver::computeStepSize(
}
pair<VectorValues, VectorValues> LPSolver::optimize() const {
LPInitSolverMatlab initSolver(*this);
LPInitSolver initSolver(*this);
VectorValues initValues = initSolver.solve();
return optimize(initValues);
}

6
gtsam_unstable/linear/QPSolver.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
#include <gtsam_unstable/linear/LPInitSolver.h>
using namespace std;
@ -188,11 +188,11 @@ pair<VectorValues, VectorValues> QPSolver::optimize() const {
if(newKey < key)
newKey = key;
newKey++;
initProblem.cost = LinearCost(newKey, ones(1));
initProblem.cost = LinearCost(newKey, Vector::Ones(1));
initProblem.equalities = qp_.equalities;
initProblem.inequalities = qp_.inequalities;
LPSolver solver(initProblem);
LPInitSolverMatlab initSolver(solver);
LPInitSolver initSolver(solver);
VectorValues initValues = initSolver.solve();
return optimize(initValues);

16
gtsam_unstable/linear/RawQP.cpp
View File

@ -21,7 +21,7 @@ void RawQP::addColumn(
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
Matrix11 coefficient = at_c < 5 > (vars) * ones(1, 1);
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
if (!varname_to_key.count(var_))
varname_to_key[var_] = Symbol('X', varNumber++);
@ -45,8 +45,8 @@ void RawQP::addColumnDouble(
std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end());
Matrix11 coefficient1 = at_c < 4 > (vars) * ones(1, 1);
Matrix11 coefficient2 = at_c < 8 > (vars) * ones(1, 1);
Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', varNumber++) });
if (row1_ == obj_name)
@ -174,7 +174,7 @@ void RawQP::addQuadTerm(
std::vector<char>> const &vars) {
std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
Matrix11 coefficient = at_c < 5 > (vars) * ones(1, 1);
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
@ -212,7 +212,7 @@ QP RawQP::makeQP() {
KeyMatPair.push_back(km);
}
madeQP.equalities.push_back(
LinearEquality(KeyMatPair, b[kv.first] * ones(1, 1), dual_key_num++));
LinearEquality(KeyMatPair, b[kv.first] * I_1x1, dual_key_num++));
}
for (auto kv : IG) {
@ -239,13 +239,13 @@ QP RawQP::makeQP() {
continue;
if (up.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, ones(1, 1), up[k], dual_key_num++));
LinearInequality(k, I_1x1, up[k], dual_key_num++));
if (lo.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, -ones(1, 1), lo[k], dual_key_num++));
LinearInequality(k, -I_1x1, lo[k], dual_key_num++));
else
madeQP.inequalities.push_back(
LinearInequality(k, -ones(1, 1), 0, dual_key_num++));
LinearInequality(k, -I_1x1, 0, dual_key_num++));
}
return madeQP;
}

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

@ -29,12 +29,14 @@
#include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h>
#include <gtsam_unstable/linear/LPInitSolver.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
/* ************************************************************************* */
/**
* min -x1-x2
@ -57,7 +59,7 @@ LP simpleLP1() {
/* ************************************************************************* */
namespace gtsam {
TEST(LPInitSolverMatlab, infinite_loop_single_var) {
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
@ -75,24 +77,24 @@ TEST(LPInitSolverMatlab, infinite_loop_single_var) {
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
TEST(LPInitSolver, 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, kOne); //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
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 * 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
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);
VectorValues starter;
starter.insert(X, zero(1));
starter.insert(Y, zero(1));
starter.insert(Z, 2 * ones(1));
starter.insert(X, kZero);
starter.insert(Y, kZero);
starter.insert(Z, Vector::Constant(2, 2.0));
VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected;
@ -102,15 +104,15 @@ TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, initialization) {
TEST(LPInitSolver, initialization) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
LPInitSolverMatlab initSolver(lpSolver);
LPInitSolver initSolver(lpSolver);
GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph();
VectorValues x0 = initOfInitGraph->optimize();
VectorValues expected_x0;
expected_x0.insert(1, zero(2));
expected_x0.insert(1, Vector::Zero(2));
CHECK(assert_equal(expected_x0, x0, 1e-10));
double y0 = initSolver.compute_y0(x0);
@ -120,7 +122,7 @@ TEST(LPInitSolverMatlab, initialization) {
Key yKey = 2;
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
LP expectedInitLP;
expectedInitLP.cost = LinearCost(yKey, ones(1));
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(
@ -168,9 +170,9 @@ CHECK(factor.error(x) != 0.0);
TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph;
graph.push_back(JacobianFactor(1, ones(1, 1), 2, ones(1, 1), ones(1), noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, ones(1, 1), 2, -ones(1, 1), 5*ones(1), noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, ones(1, 1), 2, 2*ones(1, 1), 6*ones(1), noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5*kOne, noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(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);
@ -181,7 +183,7 @@ TEST(LPSolver, simpleTest1) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues init;
init.insert(1, zero(2));
init.insert(1, Vector::Zero(2));
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
InequalityFactorGraph());

4
gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -69,7 +69,7 @@ VectorValues LinearConstraintSQP::initializeDuals() const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
duals.insert(constraint->dualKey(), zero(factor->dim()));
duals.insert(constraint->dualKey(), Vector::Zero(factor->dim()));
}
return duals;
@ -93,7 +93,7 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
QPSolver qpSolver(qp);
VectorValues zeroInitialValues;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values)
zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
zeroInitialValues.insert(key_value.key, Vector::Zero(key_value.value.dim()));
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
params_.warmStart);