From a05857f56ba51f97ba693687a8ef782906474945 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 6 May 2016 09:23:41 -0700 Subject: [PATCH] Removed deprecated functions --- gtsam_unstable/linear/LPSolver.cpp | 31 +++++++------- gtsam_unstable/linear/QPSolver.cpp | 6 +-- gtsam_unstable/linear/RawQP.cpp | 16 +++---- gtsam_unstable/linear/tests/testLPSolver.cpp | 42 ++++++++++--------- .../nonlinear/LinearConstraintSQP.cpp | 4 +- 5 files changed, 50 insertions(+), 49 deletions(-) diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 3dd826d43..e674467c3 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -9,11 +9,11 @@ #include #include #include -#include +#include namespace gtsam { 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. // Those include the equality constraints and zero priors for keys that are // not in the cost @@ -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())); + 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,20 +136,20 @@ boost::shared_ptr 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(key, + lp_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians( + 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(); - return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables + return boost::make_shared(Aterms, b); // compute the least-square approximation of dual variables } else { return boost::make_shared(); } @@ -203,7 +202,7 @@ boost::tuples::tuple LPSolver::computeStepSize( } pair LPSolver::optimize() const { - LPInitSolverMatlab initSolver(*this); + LPInitSolver initSolver(*this); VectorValues initValues = initSolver.solve(); return optimize(initValues); } diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index c045c85a2..f8ea76a43 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include using namespace std; @@ -188,11 +188,11 @@ pair 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); diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp index 069b0328d..e12525c6f 100644 --- a/gtsam_unstable/linear/RawQP.cpp +++ b/gtsam_unstable/linear/RawQP.cpp @@ -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> 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; } diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 8839ed6c7..ccd624ab2 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -29,12 +29,14 @@ #include #include -#include +#include 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()); diff --git a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp index c2500f23b..a9bf7156f 100644 --- a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp +++ b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp @@ -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(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);