diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 4abc0f81f..9a728802e 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -919,7 +919,7 @@ TEST(Matrix, weighted_elimination ) // unpack and verify i = 0; - for(boost::tie(r, di, sigma): solution){ + BOOST_FOREACH(boost::tie(r, di, sigma), solution) { EXPECT(assert_equal(r, expectedR.row(i))); // verify r DOUBLES_EQUAL(d(i), di, 1e-8); // verify d DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 52ba23155..0aa4af2e1 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -23,7 +23,6 @@ #include -#include #include #include @@ -66,7 +65,7 @@ namespace gtsam { template size_t FactorGraph::nrFactors() const { size_t size_ = 0; - BOOST_FOREACH(const sharedFactor& factor, factors_) + for(const sharedFactor& factor: factors_) if (factor) size_++; return size_; } @@ -75,7 +74,7 @@ namespace gtsam { template KeySet FactorGraph::keys() const { KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + for(const sharedFactor& factor: this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); } @@ -87,7 +86,7 @@ namespace gtsam { KeyVector FactorGraph::keyVector() const { KeyVector keys; keys.reserve(2 * size()); // guess at size - BOOST_FOREACH (const sharedFactor& factor, factors_) + for (const sharedFactor& factor: factors_) if (factor) keys.insert(keys.end(), factor->begin(), factor->end()); std::sort(keys.begin(), keys.end()); diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 90c7b32c9..2a895d763 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -193,7 +193,7 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap // attach the relative poses to the edges PoseEdge edge12, edge21; bool found1, found2; - BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) { + for(typename G::sharedFactor nl_factor: graph) { if (nl_factor->keys().size() > 2) throw std::invalid_argument("composePoses: only support factors with at most two keys"); @@ -243,7 +243,7 @@ PredecessorMap findMinimumSpanningTree(const G& fg) { // convert edge to string pairs PredecessorMap tree; typename SDGraph::vertex_iterator itVertex = boost::vertices(g).first; - BOOST_FOREACH(const typename SDGraph::Vertex& vi, p_map){ + for(const typename SDGraph::Vertex& vi: p_map){ KEY key = boost::get(boost::vertex_name, g, *itVertex); KEY parent = boost::get(boost::vertex_name, g, vi); tree.insert(key, parent); @@ -258,7 +258,7 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { typedef typename G::sharedFactor F ; - BOOST_FOREACH(const F& factor, g) + for(const F& factor: g) { if (factor->keys().size() > 2) throw(std::invalid_argument("split: only support factors with at most two keys")); diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index d40250266..41e2d8c84 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -17,7 +17,6 @@ * @author Christian Potthast */ -#include #include #include #include @@ -31,7 +30,7 @@ Errors::Errors(){} /* ************************************************************************* */ Errors::Errors(const VectorValues& V) { - BOOST_FOREACH(const Vector& e, V | boost::adaptors::map_values) { + for(const Vector& e: V | boost::adaptors::map_values) { push_back(e); } } @@ -39,7 +38,7 @@ Errors::Errors(const VectorValues& V) { /* ************************************************************************* */ void Errors::print(const std::string& s) const { cout << s << endl; - BOOST_FOREACH(const Vector& v, *this) + for(const Vector& v: *this) gtsam::print(v); } @@ -66,7 +65,7 @@ Errors Errors::operator+(const Errors& b) const { #endif Errors result; Errors::const_iterator it = b.begin(); - BOOST_FOREACH(const Vector& ai, *this) + for(const Vector& ai: *this) result.push_back(ai + *(it++)); return result; } @@ -81,7 +80,7 @@ Errors Errors::operator-(const Errors& b) const { #endif Errors result; Errors::const_iterator it = b.begin(); - BOOST_FOREACH(const Vector& ai, *this) + for(const Vector& ai: *this) result.push_back(ai - *(it++)); return result; } @@ -89,7 +88,7 @@ Errors Errors::operator-(const Errors& b) const { /* ************************************************************************* */ Errors Errors::operator-() const { Errors result; - BOOST_FOREACH(const Vector& ai, *this) + for(const Vector& ai: *this) result.push_back(-ai); return result; } @@ -105,7 +104,7 @@ double dot(const Errors& a, const Errors& b) { #endif double result = 0.0; Errors::const_iterator it = b.begin(); - BOOST_FOREACH(const Vector& ai, a) + for(const Vector& ai: a) result += gtsam::dot(ai, *(it++)); return result; } @@ -114,7 +113,7 @@ double dot(const Errors& a, const Errors& b) { template<> void axpy(double alpha, const Errors& x, Errors& y) { Errors::const_iterator it = x.begin(); - BOOST_FOREACH(Vector& yi, y) + for(Vector& yi: y) axpy(alpha,*(it++),yi); } diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 948daf2ad..8b982bcc8 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -25,7 +25,7 @@ using namespace std; using namespace gtsam; -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL):COL) #define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL) namespace gtsam { @@ -107,7 +107,7 @@ namespace gtsam { // we loop from first-eliminated to last-eliminated // i^th part of L*gy=gx is done block-column by block-column of L - BOOST_FOREACH(const sharedConditional& cg, *this) + for(const sharedConditional& cg: *this) cg->solveTransposeInPlace(gy); return gy; @@ -146,15 +146,15 @@ namespace gtsam { KeySet keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; - BOOST_FOREACH (const sharedConditional& cg, *this) + for (const sharedConditional& cg: *this) if (cg) { - BOOST_FOREACH (Key key, cg->frontals()) { + for (Key key: cg->frontals()) { ordering.push_back(key); keys.erase(key); } } // add remaining keys in case Bayes net is incomplete - BOOST_FOREACH (Key key, keys) + for (Key key: keys) ordering.push_back(key); // return matrix and RHS return factorGraph.jacobian(ordering); @@ -182,7 +182,7 @@ namespace gtsam { double GaussianBayesNet::logDeterminant() const { double logDet = 0.0; - BOOST_FOREACH(const sharedConditional& cg, *this) { + for(const sharedConditional& cg: *this) { if(cg->get_model()) { Vector diag = cg->get_R().diagonal(); cg->get_model()->whitenInPlace(diag); diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index 1f6c8e9f5..ab311fdf2 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -19,8 +19,6 @@ #pragma once -#include - #include // Only to help Eclipse #include @@ -35,7 +33,7 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue clique->conditional()->solveInPlace(result); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + for(const typename BAYESTREE::sharedClique& child: clique->children_) optimizeInPlace(child, result); } @@ -48,7 +46,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) { result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); // sum of children - BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) + for(const typename BAYESTREE::sharedClique& child: clique->children_) result += logDeterminant(child); return result; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 19d6bd607..ff67b9cb1 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -30,8 +30,6 @@ #include #include -#include - using namespace std; using namespace gtsam; @@ -50,7 +48,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, *this) + for(const sharedFactor& factor: *this) if (factor) keys.insert(factor->begin(), factor->end()); return keys; @@ -59,7 +57,7 @@ namespace gtsam { /* ************************************************************************* */ std::map GaussianFactorGraph::getKeyDimMap() const { map spec; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) { + for ( const GaussianFactor::shared_ptr &gf: *this ) { for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) { map::iterator it2 = spec.find(*it); if ( it2 == spec.end() ) { @@ -80,7 +78,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::clone() const { GaussianFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for(const sharedFactor& f: *this) { if (f) result.push_back(f->clone()); else @@ -92,7 +90,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph GaussianFactorGraph::negate() const { GaussianFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for(const sharedFactor& f: *this) { if (f) result.push_back(f->negate()); else @@ -106,7 +104,7 @@ namespace gtsam { // First find dimensions of each variable typedef std::map KeySizeMap; KeySizeMap dims; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if (!static_cast(factor)) continue; for (GaussianFactor::const_iterator key = factor->begin(); @@ -118,7 +116,7 @@ namespace gtsam { // Compute first scalar column of each variable size_t currentColIndex = 0; KeySizeMap columnIndices = dims; - BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { + for(const KeySizeMap::value_type& col: dims) { columnIndices[col.first] = currentColIndex; currentColIndex += dims[col.first]; } @@ -127,7 +125,7 @@ namespace gtsam { typedef boost::tuple triplet; vector entries; size_t row = 0; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if (!static_cast(factor)) continue; // Convert to JacobianFactor if necessary @@ -229,7 +227,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianFactorGraph::hessianDiagonal() const { VectorValues d; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if(factor){ VectorValues di = factor->hessianDiagonal(); d.addInPlace_(di); @@ -241,7 +239,7 @@ namespace gtsam { /* ************************************************************************* */ map GaussianFactorGraph::hessianBlockDiagonal() const { map blocks; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if (!factor) continue; map BD = factor->hessianBlockDiagonal(); map::const_iterator it = BD.begin(); @@ -279,7 +277,7 @@ namespace gtsam { VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const { VectorValues g = VectorValues::Zero(x0); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + for(const sharedFactor& Ai_G: *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); Vector e = Ai->error_vector(x0); Ai->transposeMultiplyAdd(1.0, e, g); @@ -291,7 +289,7 @@ namespace gtsam { VectorValues GaussianFactorGraph::gradientAtZero() const { // Zero-out the gradient VectorValues g; - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if (!factor) continue; VectorValues gi = factor->gradientAtZero(); g.addInPlace_(gi); @@ -331,7 +329,7 @@ namespace gtsam { /* ************************************************************************* */ Errors GaussianFactorGraph::operator*(const VectorValues& x) const { Errors e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { + for(const GaussianFactor::shared_ptr& Ai_G: *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); e.push_back((*Ai) * x); } @@ -341,7 +339,7 @@ namespace gtsam { /* ************************************************************************* */ void GaussianFactorGraph::multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + for(const GaussianFactor::shared_ptr& f: *this) f->multiplyHessianAdd(alpha, x, y); } @@ -353,7 +351,7 @@ namespace gtsam { /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const { Errors::iterator ei = e; - BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, *this) { + for(const GaussianFactor::shared_ptr& Ai_G: *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); *ei = (*Ai)*x; ei++; @@ -363,7 +361,7 @@ namespace gtsam { /* ************************************************************************* */ bool hasConstraints(const GaussianFactorGraph& factors) { typedef JacobianFactor J; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(const GaussianFactor::shared_ptr& factor: factors) { J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { return true; @@ -378,7 +376,7 @@ namespace gtsam { VectorValues& x) const { // For each factor add the gradient contribution Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + for(const sharedFactor& Ai_G: *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); Ai->transposeMultiplyAdd(alpha, *(ei++), x); } @@ -387,7 +385,7 @@ namespace gtsam { ///* ************************************************************************* */ //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { // Key i = 0 ; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + // for(const GaussianFactor::shared_ptr& Ai_G: fg) { // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); // r[i] = Ai->getb(); // i++; @@ -401,7 +399,7 @@ namespace gtsam { //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { // r.setZero(); // Key i = 0; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { + // for(const GaussianFactor::shared_ptr& Ai_G: fg) { // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); // Vector &y = r[i]; // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { @@ -416,7 +414,7 @@ namespace gtsam { { VectorValues x; Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + for(const sharedFactor& Ai_G: *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { // Create the value as a zero vector if it does not exist. @@ -434,7 +432,7 @@ namespace gtsam { Errors GaussianFactorGraph::gaussianErrors(const VectorValues& x) const { Errors e; - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + for(const sharedFactor& Ai_G: *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); e.push_back(Ai->error_vector(x)); } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index ae552adcd..d969f77ad 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -144,7 +144,7 @@ namespace gtsam { /** unnormalized error */ 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/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 1c55d293b..2d635d3c6 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -29,7 +29,6 @@ #include #include -#include #include #include #include @@ -241,7 +240,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, keys_.resize(n); FastVector dims(n + 1); DenseIndex slot = 0; - BOOST_FOREACH(const SlotEntry& slotentry, *scatter) { + for(const SlotEntry& slotentry: *scatter) { keys_[slot] = slotentry.key; dims[slot] = slotentry.dimension; ++slot; @@ -253,7 +252,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) + for(const GaussianFactor::shared_ptr& factor: factors) if (factor) factor->updateHessian(keys_, &info_); gttoc(update); diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 79ade1c8a..411feb7a9 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include using namespace std; @@ -127,7 +126,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { + for ( const KeyInfo::value_type &item: *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -136,7 +135,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { + for ( const KeyInfo::value_type &item: *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 7fecefe3c..999d7a325 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,7 +32,6 @@ #include #include -#include #include #include #include @@ -185,12 +184,12 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( "Unable to determine dimensionality for all variables"); } - BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { + for(const JacobianFactor::shared_ptr& factor: factors) { m += factor->rows(); } #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(DenseIndex d, varDims) { + for(DenseIndex d: varDims) { assert(d != numeric_limits::max()); } #endif @@ -204,7 +203,7 @@ FastVector _convertOrCastToJacobians( gttic(Convert_to_Jacobians); FastVector jacobians; jacobians.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(const GaussianFactor::shared_ptr& factor: factors) { if (factor) { if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< JacobianFactor>(factor)) @@ -262,7 +261,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, "The ordering provided to the JacobianFactor combine constructor\n" "contained extra variables that did not appear in the factors to combine."); // Add the remaining slots - BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) { + for(VariableSlots::const_iterator item: unorderedSlots) { orderedSlots.push_back(item); } } else { @@ -292,7 +291,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, // Loop over slots in combined factor and copy blocks from source factors gttic(copy_blocks); size_t combinedSlot = 0; - BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) { + for(VariableSlots::const_iterator varslot: orderedSlots) { JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot)); // Loop over source jacobians DenseIndex nextRow = 0; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5bcf3d635..02dd37ea9 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -211,7 +210,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { } void Gaussian::WhitenSystem(vector& A, Vector& b) const { - BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); } + for(Matrix& Aj: A) { WhitenInPlace(Aj); } whitenInPlace(b); } @@ -542,7 +541,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { size_t i = 0; // start with first row bool mixed = false; Ab.setZero(); // make sure we don't look below - BOOST_FOREACH (const Triple& t, Rd) { + for (const Triple& t: Rd) { const size_t& j = t.get<0>(); const Matrix& rd = t.get<1>(); precisions(i) = t.get<2>(); @@ -647,14 +646,14 @@ void Base::reweight(Vector& error) const { void Base::reweight(vector &A, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); - BOOST_FOREACH(Matrix& Aj, A) { + for(Matrix& Aj: A) { Aj *= w; } error *= w; } else { const Vector W = sqrtWeight(error); - BOOST_FOREACH(Matrix& Aj, A) { + for(Matrix& Aj: A) { vector_scale_inplace(W,Aj); } error = W.cwiseProduct(error); diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index caf7d0095..f9ef756f1 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -150,7 +149,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; - BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { + for ( const KeyInfo::value_type &item: keyInfo ) { result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 538d886b4..d83385d32 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build( /* getting the block diagonals over the factors */ std::map hessianMap =gfg.hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + for ( const Matrix hessian: hessianMap | boost::adaptors::map_values) blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index e5e545c36..cf714e71d 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -20,7 +20,6 @@ #include #include -#include #include namespace gtsam { @@ -114,7 +113,7 @@ public: double* yvalues) const { // Create a vector of temporary y_ values, corresponding to rows i y_.resize(size()); - BOOST_FOREACH(VectorD & yi, y_) + for(VectorD & yi: y_) yi.setZero(); // Accessing the VectorValues one by one is expensive @@ -147,7 +146,7 @@ public: // Create a vector of temporary y_ values, corresponding to rows i y_.resize(size()); - BOOST_FOREACH(VectorD & yi, y_) + for(VectorD & yi: y_) yi.setZero(); // Accessing the VectorValues one by one is expensive diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index ed2af529f..719b3d16c 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -20,7 +20,6 @@ #include #include -#include #include using namespace std; @@ -41,13 +40,13 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // If we have an ordering, pre-fill the ordered variables first if (ordering) { - BOOST_FOREACH (Key key, *ordering) { + for (Key key: *ordering) { push_back(SlotEntry(key, 0)); } } // Now, find dimensions of variables and/or extend - BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { + for (const GaussianFactor::shared_ptr& factor: gfg) { if (!factor) continue; // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index ebe5bcc9b..99df41706 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -59,7 +59,7 @@ namespace gtsam { /* ************************************************************************* */ static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + for(const GaussianFactor::shared_ptr &gf: gfg) { JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); if( !jf ) { jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) @@ -122,7 +122,7 @@ vector uniqueSampler(const vector &weight, const size_t n) { /* sampling and cache results */ vector samples = iidSampler(localWeights, n-count); - BOOST_FOREACH ( const size_t &id, samples ) { + for ( const size_t &id: samples ) { if ( touched[id] == false ) { touched[id] = true ; result.push_back(id); @@ -136,7 +136,7 @@ vector uniqueSampler(const vector &weight, const size_t n) { /****************************************************************************/ Subgraph::Subgraph(const std::vector &indices) { edges_.reserve(indices.size()); - BOOST_FOREACH ( const size_t &idx, indices ) { + for ( const size_t &idx: indices ) { edges_.push_back(SubgraphEdge(idx, 1.0)); } } @@ -144,7 +144,7 @@ Subgraph::Subgraph(const std::vector &indices) { /****************************************************************************/ std::vector Subgraph::edgeIndices() const { std::vector eid; eid.reserve(size()); - BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) { + for ( const SubgraphEdge &edge: edges_ ) { eid.push_back(edge.index_); } return eid; @@ -180,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { /****************************************************************************/ std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { os << "Subgraph" << endl; - BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) { + for ( const SubgraphEdge &e: subgraph.edges() ) { os << e << ", " ; } return os; @@ -286,7 +286,7 @@ std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { std::vector result ; size_t idx = 0; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + for ( const GaussianFactor::shared_ptr &gf: gfg ) { if ( gf->size() == 1 ) { result.push_back(idx); } @@ -299,7 +299,7 @@ std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { std::vector result ; size_t idx = 0; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + for ( const GaussianFactor::shared_ptr &gf: gfg ) { if ( gf->size() == 2 ) { const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; if ( (k1-k0) == 1 || (k0-k1) == 1 ) @@ -332,9 +332,9 @@ std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { /* traversal */ while ( !q.empty() ) { const size_t head = q.front(); q.pop(); - BOOST_FOREACH ( const size_t id, variableIndex[head] ) { + for ( const size_t id: variableIndex[head] ) { const GaussianFactor &gf = *gfg[id]; - BOOST_FOREACH ( const size_t key, gf.keys() ) { + for ( const size_t key: gf.keys() ) { if ( flags.count(key) == 0 ) { q.push(key); flags.insert(key); @@ -360,7 +360,7 @@ std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con DSFVector D(n) ; size_t count = 0 ; double sum = 0.0 ; - BOOST_FOREACH (const size_t id, idx) { + for (const size_t id: idx) { const GaussianFactor &gf = *gfg[id]; if ( gf.keys().size() != 2 ) continue; const size_t u = ordering.find(gf.keys()[0])->second, @@ -399,7 +399,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg } /* down weight the tree edges to zero */ - BOOST_FOREACH ( const size_t id, tree ) { + for ( const size_t id: tree ) { w[id] = 0.0; } @@ -419,7 +419,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg const size_t m = gfg.size() ; Weights weight; weight.reserve(m); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) { + for(const GaussianFactor::shared_ptr &gf: gfg ) { switch ( parameters_.skeletonWeight_ ) { case SubgraphBuilderParameters::EQUAL: weight.push_back(1.0); @@ -589,7 +589,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - BOOST_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + for(const boost::shared_ptr & cg: *Rc1_) { const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); // const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); @@ -634,7 +634,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< /* figure out dimension by traversing the keys */ size_t d = 0; - BOOST_FOREACH ( const Key &key, keys ) { + for ( const Key &key: keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; cache.push_back(make_pair(entry.colstart(), entry.dim())); d += entry.dim(); @@ -643,7 +643,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< /* use the cache to fill the result */ Vector result = Vector::Zero(d, 1); size_t idx = 0; - BOOST_FOREACH ( const Cache::value_type &p, cache ) { + for ( const Cache::value_type &p: cache ) { result.segment(idx, p.second) = src.segment(p.first, p.second) ; idx += p.second; } @@ -655,7 +655,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst) { /* use the cache */ size_t idx = 0; - BOOST_FOREACH ( const Key &key, keys ) { + for ( const Key &key: keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; idx += entry.dim(); @@ -668,7 +668,7 @@ buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, co GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); result->reserve(subgraph.size()); - BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) { + for ( const SubgraphEdge &e: subgraph ) { const size_t idx = e.index(); if ( clone ) result->push_back(gfg[idx]->clone()); else result->push_back(gfg[idx]); diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 3c7256c29..70212b5b1 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -131,7 +131,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { + for ( const GaussianFactor::shared_ptr &gf: jfg ) { if (gf->keys().size() > 2) { throw runtime_error( diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 1439d4b61..bce339dca 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -18,7 +18,6 @@ #include -#include #include #include #include @@ -48,7 +47,7 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Dims& dims) { typedef pair Pair; size_t j = 0; - BOOST_FOREACH(const Pair& v, dims) { + for(const Pair& v: dims) { Key key; size_t n; boost::tie(key, n) = v; @@ -61,7 +60,7 @@ namespace gtsam { VectorValues VectorValues::Zero(const VectorValues& other) { VectorValues result; - BOOST_FOREACH(const KeyValuePair& v, other) + for(const KeyValuePair& v: other) result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); return result; } @@ -82,7 +81,7 @@ namespace gtsam { void VectorValues::update(const VectorValues& values) { iterator hint = begin(); - BOOST_FOREACH(const KeyValuePair& key_value, values) + for(const KeyValuePair& key_value: values) { // Use this trick to find the value using a hint, since we are inserting from another sorted map size_t oldSize = values_.size(); @@ -108,14 +107,14 @@ namespace gtsam { /* ************************************************************************* */ void VectorValues::setZero() { - BOOST_FOREACH(Vector& v, values_ | map_values) + for(Vector& v: values_ | map_values) v.setZero(); } /* ************************************************************************* */ void VectorValues::print(const string& str, const KeyFormatter& formatter) const { cout << str << ": " << size() << " elements\n"; - BOOST_FOREACH(const value_type& key_value, *this) + for(const value_type& key_value: *this) cout << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n"; cout.flush(); } @@ -125,7 +124,7 @@ namespace gtsam { if(this->size() != x.size()) return false; typedef boost::tuple ValuePair; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, x)) { + for(const ValuePair& values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -138,13 +137,13 @@ namespace gtsam { { // Count dimensions DenseIndex totalDim = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) + for(const Vector& v: *this | map_values) totalDim += v.size(); // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - BOOST_FOREACH(const Vector& v, *this | map_values) { + for(const Vector& v: *this | map_values) { result.segment(pos, v.size()) = v; pos += v.size(); } @@ -166,7 +165,7 @@ namespace gtsam { // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - BOOST_FOREACH(const Vector *v, items) { + for(const Vector *v: items) { result.segment(pos, v->size()) = *v; pos += v->size(); } @@ -179,11 +178,11 @@ namespace gtsam { { // Count dimensions DenseIndex totalDim = 0; - BOOST_FOREACH(size_t dim, keys | map_values) + for(size_t dim: keys | map_values) totalDim += dim; Vector result(totalDim); size_t j = 0; - BOOST_FOREACH(const Dims::value_type& it, keys) { + for(const Dims::value_type& it: keys) { result.segment(j,it.second) = at(it.first); j += it.second; } @@ -221,7 +220,7 @@ namespace gtsam { double result = 0.0; typedef boost::tuple ValuePair; using boost::adaptors::map_values; - BOOST_FOREACH(const ValuePair& values, boost::combine(*this, v)) { + for(const ValuePair& values: boost::combine(*this, v)) { assert_throw(values.get<0>().first == values.get<1>().first, invalid_argument("VectorValues::dot called with a VectorValues of different structure")); assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), @@ -240,7 +239,7 @@ namespace gtsam { double VectorValues::squaredNorm() const { double sumSquares = 0.0; using boost::adaptors::map_values; - BOOST_FOREACH(const Vector& v, *this | map_values) + for(const Vector& v: *this | map_values) sumSquares += v.squaredNorm(); return sumSquares; } @@ -329,7 +328,7 @@ namespace gtsam { VectorValues operator*(const double a, const VectorValues &v) { VectorValues result; - BOOST_FOREACH(const VectorValues::KeyValuePair& key_v, v) + for(const VectorValues::KeyValuePair& key_v: v) result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); return result; } @@ -343,7 +342,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues& VectorValues::operator*=(double alpha) { - BOOST_FOREACH(Vector& v, *this | map_values) + for(Vector& v: *this | map_values) v *= alpha; return *this; } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 4722a9b6d..0583d1803 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -55,7 +55,7 @@ namespace gtsam OptimizeData myData; myData.parentData = parentData; // Take any ancestor results we'll need - BOOST_FOREACH(Key parent, clique->conditional_->parents()) + for(Key parent: clique->conditional_->parents()) myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); // Solve and store in our results //collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/)); @@ -68,7 +68,7 @@ namespace gtsam DenseIndex dim = 0; FastVector parentPointers; parentPointers.reserve(clique->conditional()->nrParents()); - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { parentPointers.push_back(myData.cliqueResults.at(parent)); dim += parentPointers.back()->second.size(); } @@ -76,7 +76,7 @@ namespace gtsam // Fill parent vector xS.resize(dim); DenseIndex vectorPos = 0; - BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + for(const VectorValues::const_iterator& parentPointer: parentPointers) { const Vector& parentVector = parentPointer->second; xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); vectorPos += parentVector.size(); @@ -108,7 +108,7 @@ namespace gtsam // OptimizeData myData; // myData.parentData = parentData; // // Take any ancestor results we'll need - // BOOST_FOREACH(Key parent, clique->conditional_->parents()) + // for(Key parent: clique->conditional_->parents()) // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); // // Solve and store in our results // myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 43ce271f3..d4403d3e3 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -22,7 +22,6 @@ #include -#include #include #include @@ -58,32 +57,32 @@ TEST(NoiseModel, constructors) m.push_back(Isotropic::Precision(3, prc,false)); // test kSigmas - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(kSigmas,mi->sigmas())); // test whiten - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(whitened,mi->whiten(unwhitened))); // test unwhiten - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); // test Mahalanobis distance double distance = 5*5+10*10+15*15; - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); // test R matrix - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(R,mi->R())); // test covariance - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(kCovariance,mi->covariance())); // test covariance - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(kCovariance.inverse(),mi->information())); // test Whiten operator @@ -92,7 +91,7 @@ TEST(NoiseModel, constructors) 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0).finished()); Matrix expected = kInverseSigma * H; - BOOST_FOREACH(Gaussian::shared_ptr mi, m) + for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(expected,mi->Whiten(H))); // can only test inplace version once :-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 0211df735..a0fc117d9 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -93,7 +93,7 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector(&relinearizeThreshold)) { - BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + for(const VectorValues::KeyValuePair& key_delta: delta) { double maxDelta = key_delta.second.lpNorm(); if(maxDelta >= *threshold) relinKeys.insert(key_delta.first); @@ -120,7 +120,7 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } else if(const FastMap* thresholds = boost::get >(&relinearizeThreshold)) { - BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + for(const VectorValues::KeyValuePair& key_delta: delta) { const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; if(threshold.rows() != key_delta.second.rows()) throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); @@ -138,7 +138,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, { // Check the current clique for relinearization bool relinearize = false; - BOOST_FOREACH(Key var, *clique->conditional()) { + for(Key var: *clique->conditional()) { double maxDelta = delta[var].lpNorm(); if(maxDelta >= threshold) { relinKeys.insert(var); @@ -148,7 +148,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, // If this node was relinearized, also check its children if(relinearize) { - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); } } @@ -161,7 +161,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMapconditional()) { + for(Key var: *clique->conditional()) { // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(var).chr())->second; @@ -180,7 +180,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMapchildren) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); } } @@ -192,7 +192,7 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector(relinearizeThreshold), delta, root); else if(relinearizeThreshold.type() == typeid(FastMap)) @@ -207,7 +207,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke static const bool debug = false; // does the separator contain any of the variables? bool found = false; - BOOST_FOREACH(Key key, clique->conditional()->parents()) { + for(Key key: clique->conditional()->parents()) { if (markedMask.exists(key)) { found = true; break; @@ -219,7 +219,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke if(debug) clique->print("Key(s) marked in clique "); if(debug) cout << "so marking key " << clique->conditional()->front() << endl; } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { FindAll(child, keys, markedMask); } } @@ -267,7 +267,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, result.update(clique->conditional()->solve(result)); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr& child, clique->children) + for(const boost::shared_ptr& child: clique->children) optimizeInPlace(child, result); } } @@ -280,14 +280,14 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + for(const ISAM2::sharedClique& root: roots) internal::optimizeInPlace(root, delta); lastBacksubVariableCount = delta.size(); } else { // Optimize with wildfire lastBacksubVariableCount = 0; - BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + for(const ISAM2::sharedClique& root: roots) lastBacksubVariableCount += optimizeWildfireNonRecursive( root, wildfireThreshold, replacedKeys, delta); // modifies delta @@ -309,7 +309,7 @@ void updateRgProd(const boost::shared_ptr& clique, const KeySet& re // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - BOOST_FOREACH(Key j, *clique->conditional()) { + for(Key j: *clique->conditional()) { if(replacedKeys.exists(j)) { anyReplaced = true; break; @@ -327,7 +327,7 @@ void updateRgProd(const boost::shared_ptr& clique, const KeySet& re // Write into RgProd vector DenseIndex vectorPosition = 0; - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { Vector& RgProdValue = RgProd[frontal]; RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); vectorPosition += RgProdValue.size(); @@ -339,7 +339,7 @@ void updateRgProd(const boost::shared_ptr& clique, const KeySet& re varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + for(const ISAM2Clique::shared_ptr& child: clique->children) { updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } } } @@ -351,7 +351,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replac // Update variables size_t varsUpdated = 0; - BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { + for(const ISAM2::sharedClique& root: roots) { internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 538c66068..ccaf898d3 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -45,7 +45,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { assert(cliqueReplaced == replaced.exists(frontal)); } #endif @@ -53,7 +53,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { if(changed.exists(parent)) { recalculate = true; break; @@ -94,7 +94,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { changed.insert(frontal); } } else { @@ -105,7 +105,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, } // Recurse to children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { + for(const typename CLIQUE::shared_ptr& child: clique->children) { optimizeWildfire(child, threshold, changed, replaced, delta, count); } } @@ -122,7 +122,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { assert(cliqueReplaced == replaced.exists(frontal)); } #endif @@ -130,7 +130,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { if(changed.exists(parent)) { recalculate = true; break; @@ -156,9 +156,9 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh boost::shared_ptr parent = clique->parent_.lock(); if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) { - BOOST_FOREACH(Key key, clique->conditional()->frontals()) + for(Key key: clique->conditional()->frontals()) clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); - BOOST_FOREACH(Key key, clique->conditional()->parents()) + for(Key key: clique->conditional()->parents()) clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); } @@ -174,7 +174,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh DenseIndex dim = 0; FastVector parentPointers; parentPointers.reserve(clique->conditional()->nrParents()); - BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + for(Key parent: clique->conditional()->parents()) { parentPointers.push_back(clique->solnPointers_.at(parent)); dim += parentPointers.back()->second.size(); } @@ -182,7 +182,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Fill parent vector xS.resize(dim); DenseIndex vectorPos = 0; - BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { + for(const VectorValues::const_iterator& parentPointer: parentPointers) { const Vector& parentVector = parentPointer->second; xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); vectorPos += parentVector.size(); @@ -227,7 +227,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { changed.insert(frontal); } } else { @@ -270,7 +270,7 @@ size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, doubl travStack.pop(); bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); if (recalculate) { - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children) { + for(const typename CLIQUE::shared_ptr& child: currentNode->children) { travStack.push(child); } } @@ -287,7 +287,7 @@ void nnz_internal(const boost::shared_ptr& clique, int& result) { int dimSep = (int)clique->conditional()->get_S().cols(); result += ((dimR+1)*dimR)/2 + dimSep*dimR; // traverse the children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { + for(const typename CLIQUE::shared_ptr& child: clique->children) { nnz_internal(child, result); } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d6f1a636a..47ebbd7af 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -174,17 +174,17 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; - if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } + if(debug) { for(const Key key: keys) { cout << key << " "; } } if(debug) cout << endl; NonlinearFactorGraph allAffected; KeySet indices; - BOOST_FOREACH(const Key key, keys) { + for(const Key key: keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } if(debug) cout << "Affected factors are: "; - if(debug) { BOOST_FOREACH(const size_t index, indices) { cout << index << " "; } } + if(debug) { for(const size_t index: indices) { cout << index << " "; } } if(debug) cout << endl; return indices; } @@ -210,10 +210,10 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttic(check_candidates_and_linearize); GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - BOOST_FOREACH(Key idx, candidates) { + for(Key idx: candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; - BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { + for(Key key: nonlinearFactors_[idx]->keys()) { if(affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; @@ -251,7 +251,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { GaussianFactorGraph cachedBoundary; - BOOST_FOREACH(sharedClique orphan, orphans) { + for(sharedClique orphan: orphans) { // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); } @@ -292,10 +292,10 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke if(debug) { cout << "markedKeys: "; - BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; } + for(const Key key: markedKeys) { cout << key << " "; } cout << endl; cout << "observedKeys: "; - BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; } + for(const Key key: observedKeys) { cout << key << " "; } cout << endl; } @@ -322,7 +322,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // ordering provides all keys in conditionals, there cannot be others because path to root included gttic(affectedKeys); FastList affectedKeys; - BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet) + for(const ConditionalType::shared_ptr& conditional: affectedBayesNet) affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); @@ -349,7 +349,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke { // Only if some variables are unconstrained FastMap constraintGroups; - BOOST_FOREACH(Key var, observedKeys) + for(Key var: observedKeys) constraintGroups[var] = 1; order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); } @@ -386,7 +386,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // Reeliminated keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, theta_.keys()) { + for(Key key: theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -406,11 +406,11 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke if(debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); - if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Key key, affectedKeys) { cout << key << " "; } cout << endl; } + if(debug) { cout << "Affected keys: "; for(const Key key: affectedKeys) { cout << key << " "; } cout << endl; } // Reeliminated keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, affectedAndNewKeys) { + for(Key key: affectedAndNewKeys) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -437,7 +437,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(orphans); // Add the orphaned subtrees - BOOST_FOREACH(const sharedClique& orphan, orphans) + for(const sharedClique& orphan: orphans) factors += boost::make_shared >(orphan); gttoc(orphans); @@ -465,7 +465,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke } else { constraintGroups = FastMap(); const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - BOOST_FOREACH (Key var, observedKeys) + for (Key var: observedKeys) constraintGroups.insert(make_pair(var, group)); } @@ -500,8 +500,8 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke // Root clique variables for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(const sharedNode& root, this->roots()) - BOOST_FOREACH(Key var, *root->conditional()) + for(const sharedNode& root: this->roots()) + for(Key var: *root->conditional()) result.detail->variableStatus[var].inRootClique = true; } @@ -554,7 +554,7 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - BOOST_FOREACH(size_t index, removeFactorIndices) { + for(size_t index: removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if(params_.cacheLinearizedFactors) @@ -571,7 +571,7 @@ ISAM2Result ISAM2::update( // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. KeySet removedAndEmpty; - BOOST_FOREACH(Key key, removeFactors.keys()) { + for(Key key: removeFactors.keys()) { if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } @@ -580,7 +580,7 @@ ISAM2Result ISAM2::update( newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); // Get indices for unused keys - BOOST_FOREACH(Key key, unusedKeys) { + for(Key key: unusedKeys) { unusedIndices.insert(unusedIndices.end(), key); } } @@ -591,7 +591,7 @@ ISAM2Result ISAM2::update( Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); // New keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } + for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } gttoc(add_new_variables); gttic(evaluate_error_before); @@ -609,14 +609,14 @@ ISAM2Result ISAM2::update( } // Also mark any provided extra re-eliminate keys if(extraReelimKeys) { - BOOST_FOREACH(Key key, *extraReelimKeys) { + for(Key key: *extraReelimKeys) { markedKeys.insert(key); } } // Observed keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, markedKeys) { + for(Key key: markedKeys) { result.detail->variableStatus[key].isObserved = true; } } @@ -624,7 +624,7 @@ ISAM2Result ISAM2::update( // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Key value) instead of the iterator constructor. FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - BOOST_FOREACH(Key index, markedKeys) { + for(Key index: markedKeys) { if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them } @@ -642,24 +642,24 @@ ISAM2Result ISAM2::update( if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed - BOOST_FOREACH(Key key, fixedVariables_) { + for(Key key: fixedVariables_) { relinKeys.erase(key); } if(noRelinKeys) { - BOOST_FOREACH(Key key, *noRelinKeys) { + for(Key key: *noRelinKeys) { relinKeys.erase(key); } } // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Key key, relinKeys) { + for(Key key: relinKeys) { result.detail->variableStatus[key].isAboveRelinThreshold = true; result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys KeySet markedRelinMask; - BOOST_FOREACH(const Key key, relinKeys) + for(const Key key: relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); @@ -667,16 +667,16 @@ ISAM2Result ISAM2::update( gttic(fluid_find_all); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. if (!relinKeys.empty()) { - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) // add other cliques that have the marked ones in the separator Impl::FindAll(root, markedKeys, markedRelinMask); // Relin involved keys for detailed results if(params_.enableDetailedResults) { KeySet involvedRelinKeys; - BOOST_FOREACH(const sharedClique& root, roots_) + for(const sharedClique& root: roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); - BOOST_FOREACH(Key key, involvedRelinKeys) { + for(Key key: involvedRelinKeys) { if(!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; result.detail->variableStatus[key].isRelinearized = true; } } @@ -771,7 +771,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, KeySet leafKeysRemoved; // Remove each variable and its subtrees - BOOST_FOREACH(Key j, leafKeys) { + for(Key j: leafKeys) { if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree // Traverse up the tree to find the root of the marginalized subtree @@ -789,7 +789,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // See if we should remove the whole clique bool marginalizeEntireClique = true; - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { if(!leafKeys.exists(frontal)) { marginalizeEntireClique = false; break; } } @@ -806,10 +806,10 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques - BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { + for(const sharedClique& removedClique: removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + for(Key frontal: removedClique->conditional()->frontals()) { // Add to factors to remove const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; @@ -832,9 +832,9 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, GaussianFactorGraph graph; KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; - BOOST_FOREACH(const sharedClique& child, clique->children) { + for(const sharedClique& child: clique->children) { // Remove subtree if child depends on any marginalized keys - BOOST_FOREACH(Key parent, child->conditional()->parents()) { + for(Key parent: child->conditional()->parents()) { if(leafKeys.exists(parent)) { subtreesToRemove.push_back(child); graph.push_back(child->cachedFactor()); // Add child marginal @@ -843,13 +843,13 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, } } Cliques childrenRemoved; - BOOST_FOREACH(const sharedClique& childToRemove, subtreesToRemove) { + for(const sharedClique& childToRemove: subtreesToRemove) { const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); - BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { + for(const sharedClique& removedClique: removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) + for(Key frontal: removedClique->conditional()->frontals()) { // Add to factors to remove const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; @@ -867,16 +867,16 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // but do not involve frontal variables of any of its children. // TODO: reuse cached linear factors KeySet factorsFromMarginalizedInClique_step1; - BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + for(Key frontal: clique->conditional()->frontals()) { if(leafKeys.exists(frontal)) factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } // Remove any factors in subtrees that we're removing at this step - BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { - BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { - BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) { + for(const sharedClique& removedChild: childrenRemoved) { + for(Key indexInClique: removedChild->conditional()->frontals()) { + for(Key factorInvolving: variableIndex_[indexInClique]) { factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } // Create factor graph from factor indices - BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { + for(size_t i: factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional @@ -908,7 +908,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, clique->conditional()->nrFrontals() -= nToRemove; // Add to factors to remove factors involved in frontals of current clique - BOOST_FOREACH(Key frontal, cliqueFrontalsToEliminate) + for(Key frontal: cliqueFrontalsToEliminate) { const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); @@ -925,8 +925,8 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; typedef pair > Key_Factors; - BOOST_FOREACH(const Key_Factors& key_factors, marginalFactors) { - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, key_factors.second) { + for(const Key_Factors& key_factors: marginalFactors) { + for(const GaussianFactor::shared_ptr& factor: key_factors.second) { if(factor) { factorsToAdd.push_back(factor); if(marginalFactorsIndices) @@ -935,7 +935,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, factor)); if(params_.cacheLinearizedFactors) linearFactors_.push_back(factor); - BOOST_FOREACH(Key factorKey, *factor) { + for(Key factorKey: *factor) { fixedVariables_.insert(factorKey); } } } @@ -944,7 +944,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // Remove the factors to remove that have been summarized in the newly-added marginal factors NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t i, factorIndicesToRemove) { + for(size_t i: factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); if(params_.cacheLinearizedFactors) @@ -1065,7 +1065,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - BOOST_FOREACH(const sharedClique& child, root->children) { + for(const sharedClique& child: root->children) { gradientAtZeroTreeAdder(child, g); } } @@ -1077,7 +1077,7 @@ VectorValues ISAM2::gradientAtZero() const VectorValues g; // Sum up contributions for each clique - BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) + for(const ISAM2::sharedClique& root: this->roots()) gradientAtZeroTreeAdder(root, g); return g; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 27f21be12..114c04018 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -200,7 +200,7 @@ struct GTSAM_EXPORT ISAM2Params { else { std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; - BOOST_FOREACH(const ISAM2ThresholdMapValue& value, boost::get(relinearizeThreshold)) { + for(const ISAM2ThresholdMapValue& value: boost::get(relinearizeThreshold)) { std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; } } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 9e42afa33..fdd86d36b 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -157,7 +157,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( // Only retrieve diagonal vector when reuse_diagonal = false if (params_.diagonalDamping && state_.reuseDiagonal == false) { state_.hessianDiagonal = linear.hessianDiagonal(); - BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { + for(Vector& v: state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { v(aa) = std::min(std::max(v(aa), params_.minDiagonal), params_.maxDiagonal); @@ -172,7 +172,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( GaussianFactorGraph &damped = (*dampedPtr); damped.reserve(damped.size() + state_.values.size()); if (params_.diagonalDamping) { - BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { + for(const VectorValues::KeyValuePair& key_vector: state_.hessianDiagonal) { // Fill in the diagonal of A with diag(hessian) try { Matrix A = Eigen::DiagonalMatrix( @@ -192,7 +192,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( // initialize noise model cache to a reasonable default size NoiseCacheVector noises(6); - BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { + for(const Values::KeyValuePair& key_value: state_.values) { size_t dim = key_value.value.dim(); if (dim > noises.size()) diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index f26001b16..0b5622f28 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -11,7 +11,6 @@ #include #include -#include namespace gtsam { @@ -19,7 +18,7 @@ namespace gtsam { void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) { if (!linearizationPoint.empty()) { linearizationPoint_ = Values(); - BOOST_FOREACH(const gtsam::Key& key, this->keys()) { + for(const gtsam::Key& key: this->keys()) { linearizationPoint_->insert(key, linearizationPoint.at(key)); } } else { @@ -82,7 +81,7 @@ double LinearContainerFactor::error(const Values& c) const { // Extract subset of values for comparison Values csub; - BOOST_FOREACH(const gtsam::Key& key, keys()) + for(const gtsam::Key& key: keys()) csub.insert(key, c.at(key)); // create dummy ordering for evaluation @@ -111,7 +110,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con // Extract subset of values Values subsetC; - BOOST_FOREACH(const gtsam::Key& key, this->keys()) + for(const gtsam::Key& key: this->keys()) subsetC.insert(key, c.at(key)); // Determine delta between linearization points using new ordering @@ -170,7 +169,7 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph( const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) { NonlinearFactorGraph result; - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) + for(const GaussianFactor::shared_ptr& f: linear_graph) if (f) result.push_back(NonlinearFactorGraph::sharedFactor( new LinearContainerFactor(f, linearizationPoint))); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 40d943656..ac8fa4e89 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -127,7 +127,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab // Get dimensions from factor graph std::vector dims; dims.reserve(variablesSorted.size()); - BOOST_FOREACH(Key key, variablesSorted) { + for(Key key: variablesSorted) { dims.push_back(values_.at(key).dim()); } @@ -144,7 +144,7 @@ VectorValues Marginals::optimize() const { void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; bool first = true; - BOOST_FOREACH(Key key, keys_) { + for(Key key: keys_) { if(!first) cout << ", "; else diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7c8337fc8..5ff022023 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -26,7 +26,7 @@ namespace gtsam { void NonlinearFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << " keys = { "; - BOOST_FOREACH(Key key, keys()) { + for(Key key: keys()) { std::cout << keyFormatter(key) << " "; } std::cout << "}" << std::endl; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 77809b51e..ae95fa72b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -31,7 +31,6 @@ # include #endif -#include #include #include @@ -133,7 +132,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Find bounds double minX = numeric_limits::infinity(), maxX = -numeric_limits::infinity(); double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { if(values.exists(key)) { boost::optional xy = getXY(values.at(key), formatting); if(xy) { @@ -150,7 +149,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph - BOOST_FOREACH(Key key, keys){ + for(Key key: keys){ // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if(values.exists(key)) { @@ -165,7 +164,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if (formatting.mergeSimilarFactors) { // Remove duplicate factors std::set > structure; - BOOST_FOREACH(const sharedFactor& factor, *this){ + for(const sharedFactor& factor: *this){ if(factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); @@ -175,7 +174,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - BOOST_FOREACH(const vector& factorKeys, structure){ + for(const vector& factorKeys: structure){ // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { @@ -187,7 +186,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, stm << "];\n"; // Make factor-variable connections - BOOST_FOREACH(Key key, factorKeys) { + for(Key key: factorKeys) { stm << " var" << key << "--" << "factor" << i << ";\n"; } @@ -214,7 +213,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Make factor-variable connections if(formatting.connectKeysToFactor && factor) { - BOOST_FOREACH(Key key, *factor) { + for(Key key: *factor) { stm << " var" << key << "--" << "factor" << i << ";\n"; } } @@ -224,7 +223,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if(factor) { Key k; bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { + for(Key key: *this->at(i)) { if(firstTime) { k = key; firstTime = false; @@ -246,7 +245,7 @@ double NonlinearFactorGraph::error(const Values& values) const { gttic(NonlinearFactorGraph_error); double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + for(const sharedFactor& factor: this->factors_) { if(factor) total_error += factor->error(values); } @@ -272,7 +271,7 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared(); symbolic->reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, *this) { + for(const sharedFactor& factor: *this) { if(factor) *symbolic += SymbolicFactor(*factor); else @@ -330,7 +329,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li linearFG->reserve(this->size()); // linearize all factors - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + for(const sharedFactor& factor: this->factors_) { if(factor) { (*linearFG) += factor->linearize(linearizationPoint); } else @@ -345,7 +344,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li /* ************************************************************************* */ NonlinearFactorGraph NonlinearFactorGraph::clone() const { NonlinearFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for(const sharedFactor& f: *this) { if (f) result.push_back(f->clone()); else @@ -357,7 +356,7 @@ NonlinearFactorGraph NonlinearFactorGraph::clone() const { /* ************************************************************************* */ NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map& rekey_mapping) const { NonlinearFactorGraph result; - BOOST_FOREACH(const sharedFactor& f, *this) { + for(const sharedFactor& f: *this) { if (f) result.push_back(f->rekey(rekey_mapping)); else diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4f333f05d..40979b86c 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -26,8 +26,6 @@ #include -#include - #include #include // Only so Eclipse finds class definition @@ -220,7 +218,7 @@ namespace gtsam { /** Constructor from a Filtered view copies out all values */ template Values::Values(const Values::Filtered& view) { - BOOST_FOREACH(const typename Filtered::KeyValuePair& key_value, view) { + for(const typename Filtered::KeyValuePair& key_value: view) { Key key = key_value.key; insert(key, static_cast(key_value.value)); } @@ -229,7 +227,7 @@ namespace gtsam { /* ************************************************************************* */ template Values::Values(const Values::ConstFiltered& view) { - BOOST_FOREACH(const typename ConstFiltered::KeyValuePair& key_value, view) { + for(const typename ConstFiltered::KeyValuePair& key_value: view) { Key key = key_value.key; insert(key, static_cast(key_value.value)); } diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index ba7a040cd..e21661fee 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,7 +25,6 @@ #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" @@ -194,7 +193,7 @@ namespace gtsam { /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; - BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + for(const ConstKeyValuePair& key_value: *this) { result += key_value.value.dim(); } return result; @@ -203,7 +202,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues Values::zeroVectors() const { VectorValues result; - BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) + for(const ConstKeyValuePair& key_value: *this) result.insert(key_value.key, Vector::Zero(key_value.value.dim())); return result; } diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index a256f8fe2..e7e24de5a 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -49,7 +49,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, // Loop over all variables const double one_over_2delta = 1.0 / (2.0 * delta); VectorValues dX = values.zeroVectors(); - BOOST_FOREACH(Key key, factor) { + for(Key key: factor) { // Compute central differences using the values struct. const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 9911da450..b7c4efeca 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -366,7 +366,7 @@ TEST(Values, filter) { int i = 0; Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); EXPECT_LONGS_EQUAL(2, (long)filtered.size()); - BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { + for(const Values::Filtered<>::KeyValuePair& key_value: filtered) { if(i == 0) { LONGS_EQUAL(2, (long)key_value.key); try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} @@ -401,7 +401,7 @@ TEST(Values, filter) { i = 0; Values::ConstFiltered pose_filtered = values.filter(); EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size()); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, pose_filtered) { + for(const Values::ConstFiltered::KeyValuePair& key_value: pose_filtered) { if(i == 0) { EXPECT_LONGS_EQUAL(1, (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value)); @@ -437,7 +437,7 @@ TEST(Values, Symbol_filter) { values.insert(Symbol('y', 3), pose3); int i = 0; - BOOST_FOREACH(const Values::Filtered::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { + for(const Values::Filtered::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) { if(i == 0) { LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); EXPECT(assert_equal(pose1, key_value.value.cast())); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 180e5cd24..f049e9d62 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -43,7 +43,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - BOOST_FOREACH(const boost::shared_ptr& factor, g) { + for(const boost::shared_ptr& factor: g) { Matrix3 Rij; boost::shared_ptr > pose3Between = @@ -75,7 +75,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { + for(const VectorValues::value_type& it: relaxedRot3) { Key key = it.first; if (key != keyAnchor) { const Vector& rotVector = it.second; @@ -108,7 +108,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + for(const boost::shared_ptr& factor: graph) { // recast to a between on Pose3 boost::shared_ptr > pose3Between = @@ -150,7 +150,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; inverseRot.insert(keyAnchor, Rot3()); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) { + for(const Values::ConstKeyValuePair& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -165,7 +165,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + for(const Values::ConstKeyValuePair& key_value: inverseRot) { Key key = key_value.key; grad.insert(key,Vector3::Zero()); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); @@ -191,12 +191,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + for(const Values::ConstKeyValuePair& key_value: inverseRot) { Key key = key_value.key; //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; Vector gradKey = Vector3::Zero(); // collect the gradient for each edge incident on key - BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){ + for(const size_t& factorId: adjEdgesMap.at(key)){ Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); if( key == (pose3Graph.at(factorId))->keys()[0] ){ @@ -236,7 +236,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // Return correct rotations const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior Values estimateRot; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + for(const Values::ConstKeyValuePair& key_value: inverseRot) { Key key = key_value.key; if (key != keyAnchor) { const Rot3& R = inverseRot.at(key); @@ -252,7 +252,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const /* ************************************************************************* */ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ size_t factorId = 0; - BOOST_FOREACH(const boost::shared_ptr& factor, pose3Graph) { + for(const boost::shared_ptr& factor: pose3Graph) { boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ @@ -321,7 +321,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values initialPose; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ + for(const Values::ConstKeyValuePair& key_value: initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); @@ -346,7 +346,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values estimate; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) { + for(const Values::ConstKeyValuePair& key_value: GNresult) { Key key = key_value.key; if (key != keyAnchor) { const Pose3& pose = GNresult.at(key); @@ -391,7 +391,7 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - // BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { + // for(const Values::ConstKeyValuePair& key_value: orientations) { // Key key = key_value.key; // if (key != keyAnchor) { // const Point3& pos = givenGuess.at(key).translation(); diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index f9f258055..d09627b77 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -44,7 +44,7 @@ public: Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); - BOOST_FOREACH(const Key& key, keys) + for(const Key& key: keys) QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index dabb7600d..8afe0bcbf 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -45,7 +45,7 @@ public: Vector zeroVector = Vector::Zero(0); std::vector QF; QF.reserve(keys.size()); - BOOST_FOREACH(const Key& key, keys) + for(const Key& key: keys) QF.push_back(KeyMatrix(key, zeroMatrix)); JacobianFactor::fillTerms(QF, zeroVector, model); } @@ -67,7 +67,7 @@ public: size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); - // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) + // for(const KeyMatrixZD& it: Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // JacobianFactor factor(QF, Q * b); std::vector QF; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 07bd20d4d..3128390c7 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -161,7 +161,7 @@ public: /// Collect all cameras: important that in key order virtual Cameras cameras(const Values& values) const { Cameras cameras; - BOOST_FOREACH(const Key& k, this->keys_) + for(const Key& k: this->keys_) cameras.push_back(values.at(k)); return cameras; } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 01f89e526..af7ca64c6 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -290,9 +290,9 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian - BOOST_FOREACH(Matrix& m, Gs) + for(Matrix& m: Gs) m = Matrix::Zero(Base::Dim, Base::Dim); - BOOST_FOREACH(Vector& v, gs) + for(Vector& v: gs) v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 455e0de87..bb261a9c4 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -123,7 +123,7 @@ public: */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; - BOOST_FOREACH(const Key& k, this->keys_) { + for(const Key& k: this->keys_) { Pose3 pose = values.at(k); if (Base::body_P_sensor_) pose = pose.compose(*(Base::body_P_sensor_)); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b2a90cb84..dc25026d2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -68,8 +67,8 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name + ".out"); // Find first name that exists - BOOST_FOREACH(const fs::path& root, rootsToSearch) { - BOOST_FOREACH(const fs::path& name, namesToSearch) { + for(const fs::path& root: rootsToSearch) { + for(const fs::path& name: namesToSearch) { if (fs::is_regular_file(root / name)) return (root / name).string(); } @@ -366,7 +365,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, // save poses - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { + for(const Values::ConstKeyValuePair& key_value: config) { const Pose2& pose = key_value.value.cast(); stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; @@ -375,7 +374,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, // save edges Matrix R = model->R(); Matrix RR = trans(R) * R; //prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr factor_, graph) { + for(boost::shared_ptr factor_: graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -413,13 +412,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, // save 2D & 3D poses Values::ConstFiltered viewPose2 = estimate.filter(); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose2) { + for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose2) { stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " << key_value.value.y() << " " << key_value.value.theta() << endl; } Values::ConstFiltered viewPose3 = estimate.filter(); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, viewPose3) { + for(const Values::ConstFiltered::KeyValuePair& key_value: viewPose3) { Point3 p = key_value.value.translation(); Rot3 R = key_value.value.rotation(); stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() @@ -428,7 +427,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } // save edges (2D or 3D) - BOOST_FOREACH(boost::shared_ptr factor_, graph) { + for(boost::shared_ptr factor_: graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (factor){ @@ -898,7 +897,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values initialCamerasEstimate(const SfM_data& db) { Values initial; size_t i = 0; // NO POINTS: j = 0; - BOOST_FOREACH(const SfM_Camera& camera, db.cameras) + for(const SfM_Camera& camera: db.cameras) initial.insert(i++, camera); return initial; } @@ -906,9 +905,9 @@ Values initialCamerasEstimate(const SfM_data& db) { Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH(const SfM_Camera& camera, db.cameras) + for(const SfM_Camera& camera: db.cameras) initial.insert((i++), camera); - BOOST_FOREACH(const SfM_Track& track, db.tracks) + for(const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); return initial; } diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index fa6fce37a..e1cf9cea2 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -82,7 +82,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, thetaToRootMap.insert(pair(keyAnchor, 0.0)); // for all nodes in the tree - BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { + for(const key2doubleMap::value_type& it: deltaThetaMap) { // compute the orientation wrt root Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, @@ -101,7 +101,7 @@ void getSymbolicGraph( // Get keys for which you want the orientation size_t id = 0; // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g) { + for(const boost::shared_ptr& factor: g) { if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; @@ -167,14 +167,14 @@ GaussianFactorGraph buildLinearOrientationGraph( noiseModel::Diagonal::shared_ptr model_deltaTheta; // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { + for(const size_t& factorId: spanningTreeIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); } // put regularized measurements in the chordsIds - BOOST_FOREACH(const size_t& factorId, chordsIds) { + for(const size_t& factorId: chordsIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); @@ -199,7 +199,7 @@ static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { gttic(lago_buildPose2graph); NonlinearFactorGraph pose2Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + for(const boost::shared_ptr& factor: graph) { // recast to a between on Pose2 boost::shared_ptr > pose2Between = @@ -226,7 +226,7 @@ static PredecessorMap findOdometricPath( Key minKey = keyAnchor; // this initialization does not matter bool minUnassigned = true; - BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { + for(const boost::shared_ptr& factor: pose2Graph) { Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]); @@ -299,7 +299,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, GaussianFactorGraph linearPose2graph; // We include the linear version of each between factor - BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { + for(const boost::shared_ptr& factor: pose2graph) { boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); @@ -346,7 +346,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, // put into Values structure Values initialGuessLago; - BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { + for(const VectorValues::value_type& it: posesLago) { Key key = it.first; if (key != keyAnchor) { const Vector& poseVector = it.second; @@ -383,7 +383,7 @@ Values initialize(const NonlinearFactorGraph& graph, VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree - BOOST_FOREACH(const VectorValues::value_type& it, orientations) { + for(const VectorValues::value_type& it: orientations) { Key key = it.first; if (key != keyAnchor) { const Pose2& pose = initialGuess.at(key); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 48e2e8b2f..b99cb5d9c 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -465,7 +465,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { } // Now loop over all these noise models - BOOST_FOREACH(SharedNoiseModel model, models) { + for(SharedNoiseModel model: models) { Projection factor(measurement, model, X(1), L(1)); // Test linearize diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 7db71d8ce..f8157c116 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -285,7 +285,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ + for(const Values::KeyValuePair& key_val: *expected){ Key k = key_val.key; EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); } @@ -311,7 +311,7 @@ TEST( Lago, largeGraphNoisy ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){ + for(const Values::KeyValuePair& key_val: *expected){ Key k = key_val.key; EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); } diff --git a/gtsam/symbolic/SymbolicBayesNet.cpp b/gtsam/symbolic/SymbolicBayesNet.cpp index c5a04eb0d..a80bf0faa 100644 --- a/gtsam/symbolic/SymbolicBayesNet.cpp +++ b/gtsam/symbolic/SymbolicBayesNet.cpp @@ -20,6 +20,8 @@ #include #include +#include + #include namespace gtsam { @@ -43,7 +45,7 @@ namespace gtsam { SymbolicConditional::Frontals frontals = conditional->frontals(); Key me = frontals.front(); SymbolicConditional::Parents parents = conditional->parents(); - BOOST_FOREACH(Key p, parents) + for(Key p: parents) of << p << "->" << me << std::endl; } diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index f1e2b48c2..47bb06515 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -24,7 +24,6 @@ #include #include -#include #include #include @@ -43,12 +42,12 @@ namespace gtsam // Gather all keys KeySet allKeys; - BOOST_FOREACH(const boost::shared_ptr& factor, factors) { + for(const boost::shared_ptr& factor: factors) { allKeys.insert(factor->begin(), factor->end()); } // Check keys - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { if(allKeys.find(key) == allKeys.end()) throw std::runtime_error("Requested to eliminate a key that is not in the factors"); } diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 13849895d..1b84e291f 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -213,7 +213,7 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayes if (subtree) { cliques.push_back(subtree); // Recursive call over all child cliques - BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children) { + for(SymbolicBayesTree::sharedClique& childClique: subtree->children) { getAllCliques(childClique,cliques); } } @@ -241,7 +241,7 @@ TEST( BayesTree, shortcutCheck ) SymbolicBayesTree::Cliques allCliques; getAllCliques(rootClique,allCliques); - BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + for(SymbolicBayesTree::sharedClique& clique: allCliques) { //clique->print("Clique#"); SymbolicBayesNet bn = clique->shortcut(rootClique); //bn.print("Shortcut:\n"); @@ -250,13 +250,13 @@ TEST( BayesTree, shortcutCheck ) // Check if all the cached shortcuts are cleared rootClique->deleteCachedShortcuts(); - BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + for(SymbolicBayesTree::sharedClique& clique: allCliques) { bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); CHECK( notCleared == false); } EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); -// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { +// for(SymbolicBayesTree::sharedClique& clique: allCliques) { // clique->print("Clique#"); // if(clique->cachedShortcut()){ // bn = clique->cachedShortcut().get();