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

31
gtsam_unstable/linear/LPSolver.cpp
View File

@ -9,11 +9,11 @@
#include <gtsam_unstable/linear/LPSolver.h> #include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h> #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h> #include <gtsam_unstable/linear/LPInitSolver.h>
namespace gtsam { namespace gtsam {
LPSolver::LPSolver(const LP &lp) : LPSolver::LPSolver(const LP &lp) :
lp_(lp), addedZeroPriorsIndex_(){ lp_(lp), addedZeroPriorsIndex_() {
// Push back factors that are the same in every iteration to the base graph. // 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 // Those include the equality constraints and zero priors for keys that are
// not in the cost // not in the cost
@ -32,7 +32,6 @@ LPSolver::LPSolver(const LP &lp) :
constrainedKeys_.merge(lp_.inequalities.keys()); constrainedKeys_.merge(lp_.inequalities.keys());
} }
LPState LPSolver::iterate(const LPState &state) const { LPState LPSolver::iterate(const LPState &state) const {
// Solve with the current working set // Solve with the current working set
// LP: project the objective neg. gradient to the constraint's null space // 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) { for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) {
size_t dim = cost.getDim(it); size_t dim = cost.getDim(it);
Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g 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(); KeySet allKeys = lp_.inequalities.keys();
allKeys.merge(lp_.equalities.keys()); allKeys.merge(lp_.equalities.keys());
allKeys.merge(KeySet(lp_.cost.keys())); allKeys.merge(KeySet(lp_.cost.keys()));
// add the corresponding factors for all variables that are not explicitly in the cost // add the corresponding factors for all variables that are not explicitly in the
//function // cost function for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
// for vars that are not in the cost, the cost gradient is zero (g=0), so b=xk
if (cost.keys().size() != allKeys.size()) { if (cost.keys().size() != allKeys.size()) {
KeySet difference; KeySet difference;
std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(), std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(),
lp_.cost.end(), std::inserter(difference, difference.end())); lp_.cost.end(), std::inserter(difference, difference.end()));
for (Key k : difference) { 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; return graph;
@ -137,20 +136,20 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(Key key,
const InequalityFactorGraph &workingSet, const VectorValues &delta) const { const InequalityFactorGraph &workingSet, const VectorValues &delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the // Transpose the A matrix of constrained factors to have the jacobian of the
// dual key // dual key
TermsContainer Aterms = collectDualJacobians < LinearEquality TermsContainer Aterms = collectDualJacobians<LinearEquality>(key,
> (key, lp_.equalities, equalityVariableIndex_); lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
> (key, workingSet, inequalityVariableIndex_); key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(), Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end()); AtermsInequalities.end());
// Collect the gradients of unconstrained cost factors to the b vector // Collect the gradients of unconstrained cost factors to the b vector
if (Aterms.size() > 0) { 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); Factor::const_iterator it = lp_.cost.find(key);
if (it != lp_.cost.end()) if (it != lp_.cost.end())
b = lp_.cost.getA(it).transpose(); b = lp_.cost.getA(it).transpose();
return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables return boost::make_shared<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables
} else { } else {
return boost::make_shared<JacobianFactor>(); return boost::make_shared<JacobianFactor>();
} }
@ -203,7 +202,7 @@ boost::tuples::tuple<double, int> LPSolver::computeStepSize(
} }
pair<VectorValues, VectorValues> LPSolver::optimize() const { pair<VectorValues, VectorValues> LPSolver::optimize() const {
LPInitSolverMatlab initSolver(*this); LPInitSolver initSolver(*this);
VectorValues initValues = initSolver.solve(); VectorValues initValues = initSolver.solve();
return optimize(initValues); 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/LPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h> #include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPInitSolverMatlab.h> #include <gtsam_unstable/linear/LPInitSolver.h>
using namespace std; using namespace std;
@ -188,11 +188,11 @@ pair<VectorValues, VectorValues> QPSolver::optimize() const {
if(newKey < key) if(newKey < key)
newKey = key; newKey = key;
newKey++; newKey++;
initProblem.cost = LinearCost(newKey, ones(1)); initProblem.cost = LinearCost(newKey, Vector::Ones(1));
initProblem.equalities = qp_.equalities; initProblem.equalities = qp_.equalities;
initProblem.inequalities = qp_.inequalities; initProblem.inequalities = qp_.inequalities;
LPSolver solver(initProblem); LPSolver solver(initProblem);
LPInitSolverMatlab initSolver(solver); LPInitSolver initSolver(solver);
VectorValues initValues = initSolver.solve(); VectorValues initValues = initSolver.solve();
return optimize(initValues); 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 var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (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_)) if (!varname_to_key.count(var_))
varname_to_key[var_] = Symbol('X', varNumber++); 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 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 row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (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 coefficient1 = at_c < 4 > (vars) * I_1x1;
Matrix11 coefficient2 = at_c < 8 > (vars) * ones(1, 1); Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
if (!varname_to_key.count(var_)) if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', varNumber++) }); varname_to_key.insert( { var_, Symbol('X', varNumber++) });
if (row1_ == obj_name) if (row1_ == obj_name)
@ -174,7 +174,7 @@ void RawQP::addQuadTerm(
std::vector<char>> const &vars) { std::vector<char>> const &vars) {
std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); 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()); 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[var1_]][varname_to_key[var2_]] = coefficient;
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
@ -212,7 +212,7 @@ QP RawQP::makeQP() {
KeyMatPair.push_back(km); KeyMatPair.push_back(km);
} }
madeQP.equalities.push_back( 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) { for (auto kv : IG) {
@ -239,13 +239,13 @@ QP RawQP::makeQP() {
continue; continue;
if (up.count(k) == 1) if (up.count(k) == 1)
madeQP.inequalities.push_back( 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) if (lo.count(k) == 1)
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(k, -ones(1, 1), lo[k], dual_key_num++)); LinearInequality(k, -I_1x1, lo[k], dual_key_num++));
else else
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(k, -ones(1, 1), 0, dual_key_num++)); LinearInequality(k, -I_1x1, 0, dual_key_num++));
} }
return madeQP; return madeQP;
} }

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

@ -29,12 +29,14 @@
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPSolver.h> #include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolverMatlab.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;
static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* min -x1-x2 * min -x1-x2
@ -57,7 +59,7 @@ LP simpleLP1() {
/* ************************************************************************* */ /* ************************************************************************* */
namespace gtsam { namespace gtsam {
TEST(LPInitSolverMatlab, infinite_loop_single_var) { TEST(LPInitSolver, infinite_loop_single_var) {
LP initchecker; LP initchecker;
initchecker.cost = LinearCost(1,Vector3(0,0,1)); //min alpha 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(-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)); CHECK(assert_equal(results, expected, 1e-7));
} }
TEST(LPInitSolverMatlab, infinite_loop_multi_var) { TEST(LPInitSolver, infinite_loop_multi_var) {
LP initchecker; LP initchecker;
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, ones(1)); //min alpha initchecker.cost = LinearCost(Z, kOne); //min alpha
initchecker.inequalities.push_back( 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( 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 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 * ones(1), Z, -1.0 * ones(1), 0, 3));// -x - alpha <= 0 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 * ones(1), Z, -1.0 * ones(1), 20, 4));//x - alpha <= 20 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 * ones(1), Z, -1.0 * ones(1), 0, 5));// -y - alpha <= 0 initchecker.inequalities.push_back(LinearInequality(Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5));// -y - alpha <= 0
LPSolver solver(initchecker); LPSolver solver(initchecker);
VectorValues starter; VectorValues starter;
starter.insert(X, zero(1)); starter.insert(X, kZero);
starter.insert(Y, zero(1)); starter.insert(Y, kZero);
starter.insert(Z, 2 * ones(1)); starter.insert(Z, Vector::Constant(2, 2.0));
VectorValues results, duals; VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter); boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected; VectorValues expected;
@ -102,15 +104,15 @@ TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
CHECK(assert_equal(results, expected, 1e-7)); CHECK(assert_equal(results, expected, 1e-7));
} }
TEST(LPInitSolverMatlab, initialization) { TEST(LPInitSolver, initialization) {
LP lp = simpleLP1(); LP lp = simpleLP1();
LPSolver lpSolver(lp); LPSolver lpSolver(lp);
LPInitSolverMatlab initSolver(lpSolver); LPInitSolver initSolver(lpSolver);
GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph(); GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph();
VectorValues x0 = initOfInitGraph->optimize(); VectorValues x0 = initOfInitGraph->optimize();
VectorValues expected_x0; VectorValues expected_x0;
expected_x0.insert(1, zero(2)); expected_x0.insert(1, Vector::Zero(2));
CHECK(assert_equal(expected_x0, x0, 1e-10)); CHECK(assert_equal(expected_x0, x0, 1e-10));
double y0 = initSolver.compute_y0(x0); double y0 = initSolver.compute_y0(x0);
@ -120,7 +122,7 @@ TEST(LPInitSolverMatlab, initialization) {
Key yKey = 2; Key yKey = 2;
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey); LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
LP expectedInitLP; LP expectedInitLP;
expectedInitLP.cost = LinearCost(yKey, ones(1)); expectedInitLP.cost = LinearCost(yKey, kOne);
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
expectedInitLP.inequalities.push_back( expectedInitLP.inequalities.push_back(
@ -168,9 +170,9 @@ CHECK(factor.error(x) != 0.0);
TEST(LPSolver, overConstrainedLinearSystem2) { TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph; 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, I_1x1, 2, I_1x1, kOne, 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, I_1x1, 2, -I_1x1, 5*kOne, 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, 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 over-constrained system // This check confirms that gtsam linear constraint solver can't handle over-constrained system
CHECK(graph.error(x) != 0.0); CHECK(graph.error(x) != 0.0);
@ -181,7 +183,7 @@ TEST(LPSolver, simpleTest1) {
LP lp = simpleLP1(); LP lp = simpleLP1();
LPSolver lpSolver(lp); LPSolver lpSolver(lp);
VectorValues init; VectorValues init;
init.insert(1, zero(2)); init.insert(1, Vector::Zero(2));
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init, VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
InequalityFactorGraph()); 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){ BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
ConstrainedFactor::shared_ptr constraint ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor); = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
duals.insert(constraint->dualKey(), zero(factor->dim())); duals.insert(constraint->dualKey(), Vector::Zero(factor->dim()));
} }
return duals; return duals;
@ -93,7 +93,7 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
QPSolver qpSolver(qp); QPSolver qpSolver(qp);
VectorValues zeroInitialValues; VectorValues zeroInitialValues;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) 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, boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
params_.warmStart); params_.warmStart);