diff --git a/gtsam_unstable/linear/ActiveSetSolver.cpp b/gtsam_unstable/linear/ActiveSetSolver.cpp index 69de63fb6..6736de31a 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.cpp +++ b/gtsam_unstable/linear/ActiveSetSolver.cpp @@ -45,23 +45,24 @@ namespace gtsam { * */ int ActiveSetSolver::identifyLeavingConstraint( - const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const { -int worstFactorIx = -1; + const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const { + int worstFactorIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is // either // inactive or a good inequality constraint, so we don't care! -double maxLambda = 0.0; -for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - if (factor->active()) { - double lambda = lambdas.at(factor->dualKey())[0]; - if (lambda > maxLambda) { - worstFactorIx = factorIx; - maxLambda = lambda; + double maxLambda = 0.0; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { + worstFactorIx = factorIx; + maxLambda = lambda; + } } } -} -return worstFactorIx; + return worstFactorIx; } /* This function will create a dual graph that solves for the @@ -76,15 +77,16 @@ return worstFactorIx; * if lambda > 0 you are violating the constraint. */ GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph( - const InequalityFactorGraph& workingSet, const VectorValues& delta) const { -GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); -BOOST_FOREACH (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); -} -return dualGraph; + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + 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); + } + return dualGraph; } /* @@ -96,35 +98,35 @@ return dualGraph; * in the next iteration. */ boost::tuple ActiveSetSolver::computeStepSize( - const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p, const double& startAlpha) const { -double minAlpha = startAlpha; -int closestFactorIx = -1; -for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - double b = factor->getb()[0]; - // only check inactive factors - if (!factor->active()) { - // Compute a'*p - double aTp = factor->dotProductRow(p); + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& startAlpha) const { + double minAlpha = startAlpha; + int closestFactorIx = -1; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + double b = factor->getb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); - // Check if a'*p >0. Don't care if it's not. - if (aTp <= 0) - continue; + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; - // Compute a'*xk - double aTx = factor->dotProductRow(xk); + // Compute a'*xk + double aTx = factor->dotProductRow(xk); - // alpha = (b - a'*xk) / (a'*p) - double alpha = (b - aTx) / aTp; - // We want the minimum of all those max alphas - if (alpha < minAlpha) { - closestFactorIx = factorIx; - minAlpha = alpha; + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; + } } } -} -return boost::make_tuple(minAlpha, closestFactorIx); + return boost::make_tuple(minAlpha, closestFactorIx); } } diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 003187fa9..24f3654bb 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -53,7 +53,7 @@ public: */ TermsContainer Aterms; if (variableIndex.find(key) != variableIndex.end()) { - BOOST_FOREACH (size_t factorIx, variableIndex[key]) { + for(size_t factorIx: variableIndex[key]) { typename FACTOR::shared_ptr factor = graph.at(factorIx); if (!factor->active()) continue; Matrix Ai = factor->getA(factor->find(key)).transpose(); diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h index 7feb705ef..2e1c23268 100644 --- a/gtsam_unstable/linear/EqualityFactorGraph.h +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -36,7 +36,7 @@ public: */ double error(const VectorValues& x) const { double total_error = 0.; - BOOST_FOREACH(const sharedFactor& factor, *this){ + for(const sharedFactor& factor: *this){ if(factor) total_error += factor->error(x); } diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index 96fe0da4b..d7083492d 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -47,7 +47,7 @@ public: * Compute error of a guess. * Infinity error if it violates an inequality; zero otherwise. */ double error(const VectorValues& x) const { - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if(factor) if (factor->error(x) > 0) return std::numeric_limits::infinity(); diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index e674467c3..4647237d2 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -161,7 +161,8 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints( InequalityFactorGraph workingSet; for (const LinearInequality::shared_ptr &factor : inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - + GTSAM_PRINT(*workingFactor); + GTSAM_PRINT(initialValues); double error = workingFactor->error(initialValues); // TODO: find a feasible initial point for LPSolver. // For now, we just throw an exception diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index d260e1409..3223d7a59 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -43,10 +43,11 @@ public: template KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const { KeyDimMap keysDim; - BOOST_FOREACH(const typename LinearGraph::sharedFactor &factor, linearGraph) { - if (!factor) continue; - BOOST_FOREACH(Key key, factor->keys()) - keysDim[key] = factor->getDim(factor->find(key)); + for (const typename LinearGraph::sharedFactor &factor : linearGraph) { + if (!factor) + continue; + for (Key key : factor->keys()) + keysDim[key] = factor->getDim(factor->find(key)); } return keysDim; } diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index f8ea76a43..d5e3d028f 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -184,9 +184,9 @@ pair QPSolver::optimize() const { //Make an LP with any linear cost function. It doesn't matter for initialization. LP initProblem; Key newKey = 0; // make an unrelated key for a random variable cost - BOOST_FOREACH(Key key, qp_.cost.getKeyDimMap() | boost::adaptors::map_keys) - if(newKey < key) - newKey = key; + for (Key key : qp_.cost.getKeyDimMap() | boost::adaptors::map_keys) + if (newKey < key) + newKey = key; newKey++; initProblem.cost = LinearCost(newKey, Vector::Ones(1)); initProblem.equalities = qp_.equalities; diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 13f7ad946..74e775225 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -94,7 +94,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) { VectorValues starter; starter.insert(X, kZero); starter.insert(Y, kZero); - starter.insert(Z, Vector::Constant(2, 2.0)); + starter.insert(Z, Vector::Constant(1, 2.0)); VectorValues results, duals; boost::tie(results, duals) = solver.optimize(starter); VectorValues expected; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 24fd97d5e..3b517f2f7 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -149,7 +149,6 @@ TEST(QPSolver, indentifyActiveConstraints) { expectedSolution.insert(X(1), kZero); expectedSolution.insert(X(2), kZero); CHECK(assert_equal(expectedSolution, solution, 1e-100)); - } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp index a9bf7156f..f8734d81a 100644 --- a/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp +++ b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp @@ -37,7 +37,7 @@ bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state /* ************************************************************************* */ bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) { + for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearInequalities) { ConstrainedFactor::shared_ptr inequality = boost::dynamic_pointer_cast(factor); @@ -66,7 +66,7 @@ bool LinearConstraintSQP::checkConvergence(const LinearConstraintNLPState& state /* ************************************************************************* */ VectorValues LinearConstraintSQP::initializeDuals() const { VectorValues duals; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){ + for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearEqualities){ ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast(factor); duals.insert(constraint->dualKey(), Vector::Zero(factor->dim())); @@ -92,7 +92,7 @@ LinearConstraintNLPState LinearConstraintSQP::iterate( VectorValues delta, duals; QPSolver qpSolver(qp); VectorValues zeroInitialValues; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) + for(const Values::ConstKeyValuePair& key_value: state.values) zeroInitialValues.insert(key_value.key, Vector::Zero(key_value.value.dim())); boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp index d012caaf9..1dc033ed1 100644 --- a/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp +++ b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp @@ -27,7 +27,7 @@ EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize( const Values& linearizationPoint) const { EqualityFactorGraph::shared_ptr linearGraph( new EqualityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + for(const NonlinearFactor::shared_ptr& factor: *this){ JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( factor->linearize(linearizationPoint)); ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast(factor); @@ -38,7 +38,7 @@ EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize( /* ************************************************************************* */ bool LinearEqualityFactorGraph::checkFeasibility(const Values& values, double tol) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + for(const NonlinearFactor::shared_ptr& factor: *this){ NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast(factor); Vector error = noiseModelFactor->unwhitenedError(values); diff --git a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp index 2e1b617c9..ca10827c3 100644 --- a/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp +++ b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp @@ -26,7 +26,7 @@ namespace gtsam { InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize( const Values& linearizationPoint) const { InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { + for(const NonlinearFactor::shared_ptr& factor: *this) { JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( factor->linearize(linearizationPoint)); ConstrainedFactor::shared_ptr constraint @@ -40,7 +40,7 @@ InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize( bool LinearInequalityFactorGraph::checkFeasibilityAndComplimentary( const Values& values, const VectorValues& dualValues, double tol) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { + for(const NonlinearFactor::shared_ptr& factor: *this) { NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast(factor); Vector error = noiseModelFactor->unwhitenedError(values);