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

Almost done. Still need to figure out how to remove the remaining 3 occurrences.

release/4.3a0
yao 2016-05-21 12:44:46 -04:00
parent f7ec58cde0
commit a0661b3813
47 changed files with 270 additions and 290 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/tests/testMatrix.cpp
View File

@ -919,7 +919,7 @@ TEST(Matrix, weighted_elimination )
// unpack and verify // unpack and verify
i = 0; 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 EXPECT(assert_equal(r, expectedR.row(i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma

7
gtsam/inference/FactorGraph-inst.h
View File

@ -23,7 +23,6 @@
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <stdio.h> #include <stdio.h>
@ -66,7 +65,7 @@ namespace gtsam {
template<class FACTOR> template<class FACTOR>
size_t FactorGraph<FACTOR>::nrFactors() const { size_t FactorGraph<FACTOR>::nrFactors() const {
size_t size_ = 0; size_t size_ = 0;
BOOST_FOREACH(const sharedFactor& factor, factors_) for(const sharedFactor& factor: factors_)
if (factor) size_++; if (factor) size_++;
return size_; return size_;
} }
@ -75,7 +74,7 @@ namespace gtsam {
template<class FACTOR> template<class FACTOR>
KeySet FactorGraph<FACTOR>::keys() const { KeySet FactorGraph<FACTOR>::keys() const {
KeySet keys; KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { for(const sharedFactor& factor: this->factors_) {
if(factor) if(factor)
keys.insert(factor->begin(), factor->end()); keys.insert(factor->begin(), factor->end());
} }
@ -87,7 +86,7 @@ namespace gtsam {
KeyVector FactorGraph<FACTOR>::keyVector() const { KeyVector FactorGraph<FACTOR>::keyVector() const {
KeyVector keys; KeyVector keys;
keys.reserve(2 * size()); // guess at size keys.reserve(2 * size()); // guess at size
BOOST_FOREACH (const sharedFactor& factor, factors_) for (const sharedFactor& factor: factors_)
if (factor) if (factor)
keys.insert(keys.end(), factor->begin(), factor->end()); keys.insert(keys.end(), factor->begin(), factor->end());
std::sort(keys.begin(), keys.end()); std::sort(keys.begin(), keys.end());

6
gtsam/inference/graph-inl.h
View File

@ -193,7 +193,7 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
// attach the relative poses to the edges // attach the relative poses to the edges
PoseEdge edge12, edge21; PoseEdge edge12, edge21;
bool found1, found2; bool found1, found2;
BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) { for(typename G::sharedFactor nl_factor: graph) {
if (nl_factor->keys().size() > 2) if (nl_factor->keys().size() > 2)
throw std::invalid_argument("composePoses: only support factors with at most two keys"); throw std::invalid_argument("composePoses: only support factors with at most two keys");
@ -243,7 +243,7 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// convert edge to string pairs // convert edge to string pairs
PredecessorMap<KEY> tree; PredecessorMap<KEY> tree;
typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first; typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
BOOST_FOREACH(const typename SDGraph<KEY>::Vertex& vi, p_map){ for(const typename SDGraph<KEY>::Vertex& vi: p_map){
KEY key = boost::get(boost::vertex_name, g, *itVertex); KEY key = boost::get(boost::vertex_name, g, *itVertex);
KEY parent = boost::get(boost::vertex_name, g, vi); KEY parent = boost::get(boost::vertex_name, g, vi);
tree.insert(key, parent); tree.insert(key, parent);
@ -258,7 +258,7 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
typedef typename G::sharedFactor F ; typedef typename G::sharedFactor F ;
BOOST_FOREACH(const F& factor, g) for(const F& factor: g)
{ {
if (factor->keys().size() > 2) if (factor->keys().size() > 2)
throw(std::invalid_argument("split: only support factors with at most two keys")); throw(std::invalid_argument("split: only support factors with at most two keys"));

15
gtsam/linear/Errors.cpp
View File

@ -17,7 +17,6 @@
* @author Christian Potthast * @author Christian Potthast
*/ */
#include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <gtsam/linear/Errors.h> #include <gtsam/linear/Errors.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
@ -31,7 +30,7 @@ Errors::Errors(){}
/* ************************************************************************* */ /* ************************************************************************* */
Errors::Errors(const VectorValues& V) { 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); push_back(e);
} }
} }
@ -39,7 +38,7 @@ Errors::Errors(const VectorValues& V) {
/* ************************************************************************* */ /* ************************************************************************* */
void Errors::print(const std::string& s) const { void Errors::print(const std::string& s) const {
cout << s << endl; cout << s << endl;
BOOST_FOREACH(const Vector& v, *this) for(const Vector& v: *this)
gtsam::print(v); gtsam::print(v);
} }
@ -66,7 +65,7 @@ Errors Errors::operator+(const Errors& b) const {
#endif #endif
Errors result; Errors result;
Errors::const_iterator it = b.begin(); Errors::const_iterator it = b.begin();
BOOST_FOREACH(const Vector& ai, *this) for(const Vector& ai: *this)
result.push_back(ai + *(it++)); result.push_back(ai + *(it++));
return result; return result;
} }
@ -81,7 +80,7 @@ Errors Errors::operator-(const Errors& b) const {
#endif #endif
Errors result; Errors result;
Errors::const_iterator it = b.begin(); Errors::const_iterator it = b.begin();
BOOST_FOREACH(const Vector& ai, *this) for(const Vector& ai: *this)
result.push_back(ai - *(it++)); result.push_back(ai - *(it++));
return result; return result;
} }
@ -89,7 +88,7 @@ Errors Errors::operator-(const Errors& b) const {
/* ************************************************************************* */ /* ************************************************************************* */
Errors Errors::operator-() const { Errors Errors::operator-() const {
Errors result; Errors result;
BOOST_FOREACH(const Vector& ai, *this) for(const Vector& ai: *this)
result.push_back(-ai); result.push_back(-ai);
return result; return result;
} }
@ -105,7 +104,7 @@ double dot(const Errors& a, const Errors& b) {
#endif #endif
double result = 0.0; double result = 0.0;
Errors::const_iterator it = b.begin(); Errors::const_iterator it = b.begin();
BOOST_FOREACH(const Vector& ai, a) for(const Vector& ai: a)
result += gtsam::dot(ai, *(it++)); result += gtsam::dot(ai, *(it++));
return result; return result;
} }
@ -114,7 +113,7 @@ double dot(const Errors& a, const Errors& b) {
template<> template<>
void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) { void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) {
Errors::const_iterator it = x.begin(); Errors::const_iterator it = x.begin();
BOOST_FOREACH(Vector& yi, y) for(Vector& yi: y)
axpy(alpha,*(it++),yi); axpy(alpha,*(it++),yi);
} }

12
gtsam/linear/GaussianBayesNet.cpp
View File

@ -25,7 +25,7 @@
using namespace std; using namespace std;
using namespace gtsam; 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) #define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam { namespace gtsam {
@ -107,7 +107,7 @@ namespace gtsam {
// we loop from first-eliminated to last-eliminated // we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L // 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); cg->solveTransposeInPlace(gy);
return gy; return gy;
@ -146,15 +146,15 @@ namespace gtsam {
KeySet keys = factorGraph.keys(); KeySet keys = factorGraph.keys();
// add frontal keys in order // add frontal keys in order
Ordering ordering; Ordering ordering;
BOOST_FOREACH (const sharedConditional& cg, *this) for (const sharedConditional& cg: *this)
if (cg) { if (cg) {
BOOST_FOREACH (Key key, cg->frontals()) { for (Key key: cg->frontals()) {
ordering.push_back(key); ordering.push_back(key);
keys.erase(key); keys.erase(key);
} }
} }
// add remaining keys in case Bayes net is incomplete // add remaining keys in case Bayes net is incomplete
BOOST_FOREACH (Key key, keys) for (Key key: keys)
ordering.push_back(key); ordering.push_back(key);
// return matrix and RHS // return matrix and RHS
return factorGraph.jacobian(ordering); return factorGraph.jacobian(ordering);
@ -182,7 +182,7 @@ namespace gtsam {
double GaussianBayesNet::logDeterminant() const double GaussianBayesNet::logDeterminant() const
{ {
double logDet = 0.0; double logDet = 0.0;
BOOST_FOREACH(const sharedConditional& cg, *this) { for(const sharedConditional& cg: *this) {
if(cg->get_model()) { if(cg->get_model()) {
Vector diag = cg->get_R().diagonal(); Vector diag = cg->get_R().diagonal();
cg->get_model()->whitenInPlace(diag); cg->get_model()->whitenInPlace(diag);

6
gtsam/linear/GaussianBayesTree-inl.h
View File

@ -19,8 +19,6 @@
#pragma once #pragma once
#include <boost/foreach.hpp>
#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse #include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
#include <stdarg.h> #include <stdarg.h>
@ -35,7 +33,7 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue
clique->conditional()->solveInPlace(result); clique->conditional()->solveInPlace(result);
// starting from the root, call optimize on each conditional // 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<BAYESTREE>(child, result); optimizeInPlace<BAYESTREE>(child, result);
} }
@ -48,7 +46,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum(); result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
// sum of children // sum of children
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_) for(const typename BAYESTREE::sharedClique& child: clique->children_)
result += logDeterminant<BAYESTREE>(child); result += logDeterminant<BAYESTREE>(child);
return result; return result;

42
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -30,8 +30,6 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <boost/foreach.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -50,7 +48,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
KeySet keys; KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this) for(const sharedFactor& factor: *this)
if (factor) if (factor)
keys.insert(factor->begin(), factor->end()); keys.insert(factor->begin(), factor->end());
return keys; return keys;
@ -59,7 +57,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const { std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const {
map<Key, size_t> spec; map<Key, size_t> 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++ ) { for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) {
map<Key,size_t>::iterator it2 = spec.find(*it); map<Key,size_t>::iterator it2 = spec.find(*it);
if ( it2 == spec.end() ) { if ( it2 == spec.end() ) {
@ -80,7 +78,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::clone() const { GaussianFactorGraph GaussianFactorGraph::clone() const {
GaussianFactorGraph result; GaussianFactorGraph result;
BOOST_FOREACH(const sharedFactor& f, *this) { for(const sharedFactor& f: *this) {
if (f) if (f)
result.push_back(f->clone()); result.push_back(f->clone());
else else
@ -92,7 +90,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::negate() const { GaussianFactorGraph GaussianFactorGraph::negate() const {
GaussianFactorGraph result; GaussianFactorGraph result;
BOOST_FOREACH(const sharedFactor& f, *this) { for(const sharedFactor& f: *this) {
if (f) if (f)
result.push_back(f->negate()); result.push_back(f->negate());
else else
@ -106,7 +104,7 @@ namespace gtsam {
// First find dimensions of each variable // First find dimensions of each variable
typedef std::map<Key, size_t> KeySizeMap; typedef std::map<Key, size_t> KeySizeMap;
KeySizeMap dims; KeySizeMap dims;
BOOST_FOREACH(const sharedFactor& factor, *this) { for(const sharedFactor& factor: *this) {
if (!static_cast<bool>(factor)) continue; if (!static_cast<bool>(factor)) continue;
for (GaussianFactor::const_iterator key = factor->begin(); for (GaussianFactor::const_iterator key = factor->begin();
@ -118,7 +116,7 @@ namespace gtsam {
// Compute first scalar column of each variable // Compute first scalar column of each variable
size_t currentColIndex = 0; size_t currentColIndex = 0;
KeySizeMap columnIndices = dims; KeySizeMap columnIndices = dims;
BOOST_FOREACH(const KeySizeMap::value_type& col, dims) { for(const KeySizeMap::value_type& col: dims) {
columnIndices[col.first] = currentColIndex; columnIndices[col.first] = currentColIndex;
currentColIndex += dims[col.first]; currentColIndex += dims[col.first];
} }
@ -127,7 +125,7 @@ namespace gtsam {
typedef boost::tuple<size_t, size_t, double> triplet; typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> entries; vector<triplet> entries;
size_t row = 0; size_t row = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) { for(const sharedFactor& factor: *this) {
if (!static_cast<bool>(factor)) continue; if (!static_cast<bool>(factor)) continue;
// Convert to JacobianFactor if necessary // Convert to JacobianFactor if necessary
@ -229,7 +227,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues GaussianFactorGraph::hessianDiagonal() const { VectorValues GaussianFactorGraph::hessianDiagonal() const {
VectorValues d; VectorValues d;
BOOST_FOREACH(const sharedFactor& factor, *this) { for(const sharedFactor& factor: *this) {
if(factor){ if(factor){
VectorValues di = factor->hessianDiagonal(); VectorValues di = factor->hessianDiagonal();
d.addInPlace_(di); d.addInPlace_(di);
@ -241,7 +239,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal() const { map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal() const {
map<Key,Matrix> blocks; map<Key,Matrix> blocks;
BOOST_FOREACH(const sharedFactor& factor, *this) { for(const sharedFactor& factor: *this) {
if (!factor) continue; if (!factor) continue;
map<Key,Matrix> BD = factor->hessianBlockDiagonal(); map<Key,Matrix> BD = factor->hessianBlockDiagonal();
map<Key,Matrix>::const_iterator it = BD.begin(); map<Key,Matrix>::const_iterator it = BD.begin();
@ -279,7 +277,7 @@ namespace gtsam {
VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const
{ {
VectorValues g = VectorValues::Zero(x0); 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); JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Vector e = Ai->error_vector(x0); Vector e = Ai->error_vector(x0);
Ai->transposeMultiplyAdd(1.0, e, g); Ai->transposeMultiplyAdd(1.0, e, g);
@ -291,7 +289,7 @@ namespace gtsam {
VectorValues GaussianFactorGraph::gradientAtZero() const { VectorValues GaussianFactorGraph::gradientAtZero() const {
// Zero-out the gradient // Zero-out the gradient
VectorValues g; VectorValues g;
BOOST_FOREACH(const sharedFactor& factor, *this) { for(const sharedFactor& factor: *this) {
if (!factor) continue; if (!factor) continue;
VectorValues gi = factor->gradientAtZero(); VectorValues gi = factor->gradientAtZero();
g.addInPlace_(gi); g.addInPlace_(gi);
@ -331,7 +329,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Errors GaussianFactorGraph::operator*(const VectorValues& x) const { Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
Errors e; 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); JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back((*Ai) * x); e.push_back((*Ai) * x);
} }
@ -341,7 +339,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::multiplyHessianAdd(double alpha, void GaussianFactorGraph::multiplyHessianAdd(double alpha,
const VectorValues& x, VectorValues& y) const { 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); f->multiplyHessianAdd(alpha, x, y);
} }
@ -353,7 +351,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const { void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const {
Errors::iterator ei = e; 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); JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
*ei = (*Ai)*x; *ei = (*Ai)*x;
ei++; ei++;
@ -363,7 +361,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
bool hasConstraints(const GaussianFactorGraph& factors) { bool hasConstraints(const GaussianFactorGraph& factors) {
typedef JacobianFactor J; 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<J>(factor)); J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
return true; return true;
@ -378,7 +376,7 @@ namespace gtsam {
VectorValues& x) const { VectorValues& x) const {
// For each factor add the gradient contribution // For each factor add the gradient contribution
Errors::const_iterator ei = e.begin(); 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); JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
Ai->transposeMultiplyAdd(alpha, *(ei++), x); Ai->transposeMultiplyAdd(alpha, *(ei++), x);
} }
@ -387,7 +385,7 @@ namespace gtsam {
///* ************************************************************************* */ ///* ************************************************************************* */
//void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
// Key i = 0 ; // 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); // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// r[i] = Ai->getb(); // r[i] = Ai->getb();
// i++; // i++;
@ -401,7 +399,7 @@ namespace gtsam {
//void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
// r.setZero(); // r.setZero();
// Key i = 0; // 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); // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
// Vector &y = r[i]; // Vector &y = r[i];
// for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
@ -416,7 +414,7 @@ namespace gtsam {
{ {
VectorValues x; VectorValues x;
Errors::const_iterator ei = e.begin(); 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); JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
// Create the value as a zero vector if it does not exist. // 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 GaussianFactorGraph::gaussianErrors(const VectorValues& x) const
{ {
Errors e; Errors e;
BOOST_FOREACH(const sharedFactor& Ai_G, *this) { for(const sharedFactor& Ai_G: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
e.push_back(Ai->error_vector(x)); e.push_back(Ai->error_vector(x));
} }

2
gtsam/linear/GaussianFactorGraph.h
View File

@ -144,7 +144,7 @@ namespace gtsam {
/** unnormalized error */ /** unnormalized error */
double error(const VectorValues& x) const { double error(const VectorValues& x) const {
double total_error = 0.; double total_error = 0.;
BOOST_FOREACH(const sharedFactor& factor, *this){ for(const sharedFactor& factor: *this){
if(factor) if(factor)
total_error += factor->error(x); total_error += factor->error(x);
} }

5
gtsam/linear/HessianFactor.cpp
View File

@ -29,7 +29,6 @@
#include <gtsam/base/ThreadsafeException.h> #include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
@ -241,7 +240,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
keys_.resize(n); keys_.resize(n);
FastVector<DenseIndex> dims(n + 1); FastVector<DenseIndex> dims(n + 1);
DenseIndex slot = 0; DenseIndex slot = 0;
BOOST_FOREACH(const SlotEntry& slotentry, *scatter) { for(const SlotEntry& slotentry: *scatter) {
keys_[slot] = slotentry.key; keys_[slot] = slotentry.key;
dims[slot] = slotentry.dimension; dims[slot] = slotentry.dimension;
++slot; ++slot;
@ -253,7 +252,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
// Form A' * A // Form A' * A
gttic(update); gttic(update);
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) for(const GaussianFactor::shared_ptr& factor: factors)
if (factor) if (factor)
factor->updateHessian(keys_, &info_); factor->updateHessian(keys_, &info_);
gttoc(update); gttoc(update);

5
gtsam/linear/IterativeSolver.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <iostream> #include <iostream>
using namespace std; using namespace std;
@ -127,7 +126,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) {
/****************************************************************************/ /****************************************************************************/
vector<size_t> KeyInfo::colSpec() const { vector<size_t> KeyInfo::colSpec() const {
std::vector<size_t> result(size(), 0); std::vector<size_t> 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(); result[item.second.index()] = item.second.dim();
} }
return result; return result;
@ -136,7 +135,7 @@ vector<size_t> KeyInfo::colSpec() const {
/****************************************************************************/ /****************************************************************************/
VectorValues KeyInfo::x0() const { VectorValues KeyInfo::x0() const {
VectorValues result; 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())); result.insert(item.first, Vector::Zero(item.second.dim()));
} }
return result; return result;

11
gtsam/linear/JacobianFactor.cpp
View File

@ -32,7 +32,6 @@
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/array.hpp> #include <boost/array.hpp>
@ -185,12 +184,12 @@ boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
"Unable to determine dimensionality for all variables"); "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(); m += factor->rows();
} }
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(DenseIndex d, varDims) { for(DenseIndex d: varDims) {
assert(d != numeric_limits<DenseIndex>::max()); assert(d != numeric_limits<DenseIndex>::max());
} }
#endif #endif
@ -204,7 +203,7 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
gttic(Convert_to_Jacobians); gttic(Convert_to_Jacobians);
FastVector<JacobianFactor::shared_ptr> jacobians; FastVector<JacobianFactor::shared_ptr> jacobians;
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { for(const GaussianFactor::shared_ptr& factor: factors) {
if (factor) { if (factor) {
if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
JacobianFactor>(factor)) JacobianFactor>(factor))
@ -262,7 +261,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
"The ordering provided to the JacobianFactor combine constructor\n" "The ordering provided to the JacobianFactor combine constructor\n"
"contained extra variables that did not appear in the factors to combine."); "contained extra variables that did not appear in the factors to combine.");
// Add the remaining slots // Add the remaining slots
BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) { for(VariableSlots::const_iterator item: unorderedSlots) {
orderedSlots.push_back(item); orderedSlots.push_back(item);
} }
} else { } else {
@ -292,7 +291,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
// Loop over slots in combined factor and copy blocks from source factors // Loop over slots in combined factor and copy blocks from source factors
gttic(copy_blocks); gttic(copy_blocks);
size_t combinedSlot = 0; 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)); JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot));
// Loop over source jacobians // Loop over source jacobians
DenseIndex nextRow = 0; DenseIndex nextRow = 0;

9
gtsam/linear/NoiseModel.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/random/linear_congruential.hpp> #include <boost/random/linear_congruential.hpp>
@ -211,7 +210,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const {
} }
void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const { void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); } for(Matrix& Aj: A) { WhitenInPlace(Aj); }
whitenInPlace(b); whitenInPlace(b);
} }
@ -542,7 +541,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const {
size_t i = 0; // start with first row size_t i = 0; // start with first row
bool mixed = false; bool mixed = false;
Ab.setZero(); // make sure we don't look below 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 size_t& j = t.get<0>();
const Matrix& rd = t.get<1>(); const Matrix& rd = t.get<1>();
precisions(i) = t.get<2>(); precisions(i) = t.get<2>();
@ -647,14 +646,14 @@ void Base::reweight(Vector& error) const {
void Base::reweight(vector<Matrix> &A, Vector &error) const { void Base::reweight(vector<Matrix> &A, Vector &error) const {
if ( reweight_ == Block ) { if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm()); const double w = sqrtWeight(error.norm());
BOOST_FOREACH(Matrix& Aj, A) { for(Matrix& Aj: A) {
Aj *= w; Aj *= w;
} }
error *= w; error *= w;
} }
else { else {
const Vector W = sqrtWeight(error); const Vector W = sqrtWeight(error);
BOOST_FOREACH(Matrix& Aj, A) { for(Matrix& Aj: A) {
vector_scale_inplace(W,Aj); vector_scale_inplace(W,Aj);
} }
error = W.cwiseProduct(error); error = W.cwiseProduct(error);

3
gtsam/linear/PCGSolver.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <algorithm> #include <algorithm>
@ -150,7 +149,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
/**********************************************************************************/ /**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
VectorValues result; VectorValues result;
BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { for ( const KeyInfo::value_type &item: keyInfo ) {
result.insert(item.first, result.insert(item.first,
v.segment(item.second.colstart(), item.second.dim())); v.segment(item.second.colstart(), item.second.dim()));
} }

2
gtsam/linear/Preconditioner.cpp
View File

@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build(
/* getting the block diagonals over the factors */ /* getting the block diagonals over the factors */
std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal(); std::map<Key, Matrix> 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); blocks.push_back(hessian);
/* if necessary, allocating the memory for cacheing the factorization results */ /* if necessary, allocating the memory for cacheing the factorization results */

5
gtsam/linear/RegularHessianFactor.h
View File

@ -20,7 +20,6 @@
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h> #include <gtsam/linear/RegularJacobianFactor.h>
#include <boost/foreach.hpp>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -114,7 +113,7 @@ public:
double* yvalues) const { double* yvalues) const {
// Create a vector of temporary y_ values, corresponding to rows i // Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size()); y_.resize(size());
BOOST_FOREACH(VectorD & yi, y_) for(VectorD & yi: y_)
yi.setZero(); yi.setZero();
// Accessing the VectorValues one by one is expensive // Accessing the VectorValues one by one is expensive
@ -147,7 +146,7 @@ public:
// Create a vector of temporary y_ values, corresponding to rows i // Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size()); y_.resize(size());
BOOST_FOREACH(VectorD & yi, y_) for(VectorD & yi: y_)
yi.setZero(); yi.setZero();
// Accessing the VectorValues one by one is expensive // Accessing the VectorValues one by one is expensive

5
gtsam/linear/Scatter.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/linear/Scatter.h> #include <gtsam/linear/Scatter.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <boost/foreach.hpp>
#include <algorithm> #include <algorithm>
using namespace std; using namespace std;
@ -41,13 +40,13 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
// If we have an ordering, pre-fill the ordered variables first // If we have an ordering, pre-fill the ordered variables first
if (ordering) { if (ordering) {
BOOST_FOREACH (Key key, *ordering) { for (Key key: *ordering) {
push_back(SlotEntry(key, 0)); push_back(SlotEntry(key, 0));
} }
} }
// Now, find dimensions of variables and/or extend // 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; if (!factor) continue;
// TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers

34
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -59,7 +59,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); 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<JacobianFactor>(gf); JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) { if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
@ -122,7 +122,7 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
/* sampling and cache results */ /* sampling and cache results */
vector<size_t> samples = iidSampler(localWeights, n-count); vector<size_t> samples = iidSampler(localWeights, n-count);
BOOST_FOREACH ( const size_t &id, samples ) { for ( const size_t &id: samples ) {
if ( touched[id] == false ) { if ( touched[id] == false ) {
touched[id] = true ; touched[id] = true ;
result.push_back(id); result.push_back(id);
@ -136,7 +136,7 @@ vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
/****************************************************************************/ /****************************************************************************/
Subgraph::Subgraph(const std::vector<size_t> &indices) { Subgraph::Subgraph(const std::vector<size_t> &indices) {
edges_.reserve(indices.size()); edges_.reserve(indices.size());
BOOST_FOREACH ( const size_t &idx, indices ) { for ( const size_t &idx: indices ) {
edges_.push_back(SubgraphEdge(idx, 1.0)); edges_.push_back(SubgraphEdge(idx, 1.0));
} }
} }
@ -144,7 +144,7 @@ Subgraph::Subgraph(const std::vector<size_t> &indices) {
/****************************************************************************/ /****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const { std::vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size()); std::vector<size_t> eid; eid.reserve(size());
BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) { for ( const SubgraphEdge &edge: edges_ ) {
eid.push_back(edge.index_); eid.push_back(edge.index_);
} }
return eid; return eid;
@ -180,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
/****************************************************************************/ /****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl; os << "Subgraph" << endl;
BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) { for ( const SubgraphEdge &e: subgraph.edges() ) {
os << e << ", " ; os << e << ", " ;
} }
return os; return os;
@ -286,7 +286,7 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ; std::vector<size_t> result ;
size_t idx = 0; size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 1 ) { if ( gf->size() == 1 ) {
result.push_back(idx); result.push_back(idx);
} }
@ -299,7 +299,7 @@ std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ; std::vector<size_t> result ;
size_t idx = 0; size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) { if ( gf->size() == 2 ) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ( (k1-k0) == 1 || (k0-k1) == 1 ) if ( (k1-k0) == 1 || (k0-k1) == 1 )
@ -332,9 +332,9 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
/* traversal */ /* traversal */
while ( !q.empty() ) { while ( !q.empty() ) {
const size_t head = q.front(); q.pop(); 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]; 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 ) { if ( flags.count(key) == 0 ) {
q.push(key); q.push(key);
flags.insert(key); flags.insert(key);
@ -360,7 +360,7 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
DSFVector D(n) ; DSFVector D(n) ;
size_t count = 0 ; double sum = 0.0 ; 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]; const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue; if ( gf.keys().size() != 2 ) continue;
const size_t u = ordering.find(gf.keys()[0])->second, 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 */ /* down weight the tree edges to zero */
BOOST_FOREACH ( const size_t id, tree ) { for ( const size_t id: tree ) {
w[id] = 0.0; w[id] = 0.0;
} }
@ -419,7 +419,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
const size_t m = gfg.size() ; const size_t m = gfg.size() ;
Weights weight; weight.reserve(m); Weights weight; weight.reserve(m);
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) { for(const GaussianFactor::shared_ptr &gf: gfg ) {
switch ( parameters_.skeletonWeight_ ) { switch ( parameters_.skeletonWeight_ ) {
case SubgraphBuilderParameters::EQUAL: case SubgraphBuilderParameters::EQUAL:
weight.push_back(1.0); 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()); std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */ /* in place back substitute */
BOOST_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) { for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals())); const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal); // const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal); const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
@ -634,7 +634,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
/* figure out dimension by traversing the keys */ /* figure out dimension by traversing the keys */
size_t d = 0; size_t d = 0;
BOOST_FOREACH ( const Key &key, keys ) { for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second; const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.push_back(make_pair(entry.colstart(), entry.dim())); cache.push_back(make_pair(entry.colstart(), entry.dim()));
d += 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 */ /* use the cache to fill the result */
Vector result = Vector::Zero(d, 1); Vector result = Vector::Zero(d, 1);
size_t idx = 0; 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) ; result.segment(idx, p.second) = src.segment(p.first, p.second) ;
idx += 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<Key> &keys, Vector &dst) { void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
/* use the cache */ /* use the cache */
size_t idx = 0; size_t idx = 0;
BOOST_FOREACH ( const Key &key, keys ) { for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second; const KeyInfoEntry &entry = keyInfo.find(key)->second;
dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
idx += entry.dim(); idx += entry.dim();
@ -668,7 +668,7 @@ buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, co
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
result->reserve(subgraph.size()); result->reserve(subgraph.size());
BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) { for ( const SubgraphEdge &e: subgraph ) {
const size_t idx = e.index(); const size_t idx = e.index();
if ( clone ) result->push_back(gfg[idx]->clone()); if ( clone ) result->push_back(gfg[idx]->clone());
else result->push_back(gfg[idx]); else result->push_back(gfg[idx]);

2
gtsam/linear/SubgraphSolver.cpp
View File

@ -131,7 +131,7 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
size_t t = 0; size_t t = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { for ( const GaussianFactor::shared_ptr &gf: jfg ) {
if (gf->keys().size() > 2) { if (gf->keys().size() > 2) {
throw runtime_error( throw runtime_error(

31
gtsam/linear/VectorValues.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <boost/range/combine.hpp> #include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp> #include <boost/range/numeric.hpp>
@ -48,7 +47,7 @@ namespace gtsam {
VectorValues::VectorValues(const Vector& x, const Dims& dims) { VectorValues::VectorValues(const Vector& x, const Dims& dims) {
typedef pair<Key, size_t> Pair; typedef pair<Key, size_t> Pair;
size_t j = 0; size_t j = 0;
BOOST_FOREACH(const Pair& v, dims) { for(const Pair& v: dims) {
Key key; Key key;
size_t n; size_t n;
boost::tie(key, n) = v; boost::tie(key, n) = v;
@ -61,7 +60,7 @@ namespace gtsam {
VectorValues VectorValues::Zero(const VectorValues& other) VectorValues VectorValues::Zero(const VectorValues& other)
{ {
VectorValues result; 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()))); result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size())));
return result; return result;
} }
@ -82,7 +81,7 @@ namespace gtsam {
void VectorValues::update(const VectorValues& values) void VectorValues::update(const VectorValues& values)
{ {
iterator hint = begin(); 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 // Use this trick to find the value using a hint, since we are inserting from another sorted map
size_t oldSize = values_.size(); size_t oldSize = values_.size();
@ -108,14 +107,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::setZero() void VectorValues::setZero()
{ {
BOOST_FOREACH(Vector& v, values_ | map_values) for(Vector& v: values_ | map_values)
v.setZero(); v.setZero();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::print(const string& str, const KeyFormatter& formatter) const { void VectorValues::print(const string& str, const KeyFormatter& formatter) const {
cout << str << ": " << size() << " elements\n"; 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 << " " << formatter(key_value.first) << ": " << key_value.second.transpose() << "\n";
cout.flush(); cout.flush();
} }
@ -125,7 +124,7 @@ namespace gtsam {
if(this->size() != x.size()) if(this->size() != x.size())
return false; return false;
typedef boost::tuple<value_type, value_type> ValuePair; typedef boost::tuple<value_type, value_type> 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 || if(values.get<0>().first != values.get<1>().first ||
!equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol))
return false; return false;
@ -138,13 +137,13 @@ namespace gtsam {
{ {
// Count dimensions // Count dimensions
DenseIndex totalDim = 0; DenseIndex totalDim = 0;
BOOST_FOREACH(const Vector& v, *this | map_values) for(const Vector& v: *this | map_values)
totalDim += v.size(); totalDim += v.size();
// Copy vectors // Copy vectors
Vector result(totalDim); Vector result(totalDim);
DenseIndex pos = 0; DenseIndex pos = 0;
BOOST_FOREACH(const Vector& v, *this | map_values) { for(const Vector& v: *this | map_values) {
result.segment(pos, v.size()) = v; result.segment(pos, v.size()) = v;
pos += v.size(); pos += v.size();
} }
@ -166,7 +165,7 @@ namespace gtsam {
// Copy vectors // Copy vectors
Vector result(totalDim); Vector result(totalDim);
DenseIndex pos = 0; DenseIndex pos = 0;
BOOST_FOREACH(const Vector *v, items) { for(const Vector *v: items) {
result.segment(pos, v->size()) = *v; result.segment(pos, v->size()) = *v;
pos += v->size(); pos += v->size();
} }
@ -179,11 +178,11 @@ namespace gtsam {
{ {
// Count dimensions // Count dimensions
DenseIndex totalDim = 0; DenseIndex totalDim = 0;
BOOST_FOREACH(size_t dim, keys | map_values) for(size_t dim: keys | map_values)
totalDim += dim; totalDim += dim;
Vector result(totalDim); Vector result(totalDim);
size_t j = 0; 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); result.segment(j,it.second) = at(it.first);
j += it.second; j += it.second;
} }
@ -221,7 +220,7 @@ namespace gtsam {
double result = 0.0; double result = 0.0;
typedef boost::tuple<value_type, value_type> ValuePair; typedef boost::tuple<value_type, value_type> ValuePair;
using boost::adaptors::map_values; 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, assert_throw(values.get<0>().first == values.get<1>().first,
invalid_argument("VectorValues::dot called with a VectorValues of different structure")); invalid_argument("VectorValues::dot called with a VectorValues of different structure"));
assert_throw(values.get<0>().second.size() == values.get<1>().second.size(), assert_throw(values.get<0>().second.size() == values.get<1>().second.size(),
@ -240,7 +239,7 @@ namespace gtsam {
double VectorValues::squaredNorm() const { double VectorValues::squaredNorm() const {
double sumSquares = 0.0; double sumSquares = 0.0;
using boost::adaptors::map_values; using boost::adaptors::map_values;
BOOST_FOREACH(const Vector& v, *this | map_values) for(const Vector& v: *this | map_values)
sumSquares += v.squaredNorm(); sumSquares += v.squaredNorm();
return sumSquares; return sumSquares;
} }
@ -329,7 +328,7 @@ namespace gtsam {
VectorValues operator*(const double a, const VectorValues &v) VectorValues operator*(const double a, const VectorValues &v)
{ {
VectorValues result; 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)); result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second));
return result; return result;
} }
@ -343,7 +342,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues& VectorValues::operator*=(double alpha) VectorValues& VectorValues::operator*=(double alpha)
{ {
BOOST_FOREACH(Vector& v, *this | map_values) for(Vector& v: *this | map_values)
v *= alpha; v *= alpha;
return *this; return *this;
} }

8
gtsam/linear/linearAlgorithms-inst.h
View File

@ -55,7 +55,7 @@ namespace gtsam
OptimizeData myData; OptimizeData myData;
myData.parentData = parentData; myData.parentData = parentData;
// Take any ancestor results we'll need // 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))); myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent)));
// Solve and store in our results // Solve and store in our results
//collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/)); //collectedResult.insert(clique->conditional()->solve(collectedResult/*myData.ancestorResults*/));
@ -68,7 +68,7 @@ namespace gtsam
DenseIndex dim = 0; DenseIndex dim = 0;
FastVector<VectorValues::const_iterator> parentPointers; FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents()); 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)); parentPointers.push_back(myData.cliqueResults.at(parent));
dim += parentPointers.back()->second.size(); dim += parentPointers.back()->second.size();
} }
@ -76,7 +76,7 @@ namespace gtsam
// Fill parent vector // Fill parent vector
xS.resize(dim); xS.resize(dim);
DenseIndex vectorPos = 0; DenseIndex vectorPos = 0;
BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { for(const VectorValues::const_iterator& parentPointer: parentPointers) {
const Vector& parentVector = parentPointer->second; const Vector& parentVector = parentPointer->second;
xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
vectorPos += parentVector.size(); vectorPos += parentVector.size();
@ -108,7 +108,7 @@ namespace gtsam
// OptimizeData myData; // OptimizeData myData;
// myData.parentData = parentData; // myData.parentData = parentData;
// // Take any ancestor results we'll need // // 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]); // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]);
// // Solve and store in our results // // Solve and store in our results
// myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); // myData.results.insert(clique->conditional()->solve(myData.ancestorResults));

17
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -22,7 +22,6 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <iostream> #include <iostream>
@ -58,32 +57,32 @@ TEST(NoiseModel, constructors)
m.push_back(Isotropic::Precision(3, prc,false)); m.push_back(Isotropic::Precision(3, prc,false));
// test kSigmas // test kSigmas
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(kSigmas,mi->sigmas())); EXPECT(assert_equal(kSigmas,mi->sigmas()));
// test whiten // test whiten
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(whitened,mi->whiten(unwhitened))); EXPECT(assert_equal(whitened,mi->whiten(unwhitened)));
// test unwhiten // test unwhiten
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
// test Mahalanobis distance // test Mahalanobis distance
double distance = 5*5+10*10+15*15; 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); DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
// test R matrix // test R matrix
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(R,mi->R())); EXPECT(assert_equal(R,mi->R()));
// test covariance // test covariance
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(kCovariance,mi->covariance())); EXPECT(assert_equal(kCovariance,mi->covariance()));
// test covariance // test covariance
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(kCovariance.inverse(),mi->information())); EXPECT(assert_equal(kCovariance.inverse(),mi->information()));
// test Whiten operator // test Whiten operator
@ -92,7 +91,7 @@ TEST(NoiseModel, constructors)
0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0,
1.0, 0.0, 0.0, 1.0).finished()); 1.0, 0.0, 0.0, 1.0).finished());
Matrix expected = kInverseSigma * H; Matrix expected = kInverseSigma * H;
BOOST_FOREACH(Gaussian::shared_ptr mi, m) for(Gaussian::shared_ptr mi: m)
EXPECT(assert_equal(expected,mi->Whiten(H))); EXPECT(assert_equal(expected,mi->Whiten(H)));
// can only test inplace version once :-) // can only test inplace version once :-)

34
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -93,7 +93,7 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISA
KeySet& fixedVariables) KeySet& fixedVariables)
{ {
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
BOOST_FOREACH(Key key, unusedKeys) { for(Key key: unusedKeys) {
delta.erase(key); delta.erase(key);
deltaNewton.erase(key); deltaNewton.erase(key);
RgProd.erase(key); RgProd.erase(key);
@ -112,7 +112,7 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
if(const double* threshold = boost::get<double>(&relinearizeThreshold)) if(const double* threshold = boost::get<double>(&relinearizeThreshold))
{ {
BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { for(const VectorValues::KeyValuePair& key_delta: delta) {
double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>(); double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
if(maxDelta >= *threshold) if(maxDelta >= *threshold)
relinKeys.insert(key_delta.first); relinKeys.insert(key_delta.first);
@ -120,7 +120,7 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
} }
else if(const FastMap<char,Vector>* thresholds = boost::get<FastMap<char,Vector> >(&relinearizeThreshold)) else if(const FastMap<char,Vector>* thresholds = boost::get<FastMap<char,Vector> >(&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; const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second;
if(threshold.rows() != key_delta.second.rows()) 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."); 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 // Check the current clique for relinearization
bool relinearize = false; bool relinearize = false;
BOOST_FOREACH(Key var, *clique->conditional()) { for(Key var: *clique->conditional()) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>(); double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
if(maxDelta >= threshold) { if(maxDelta >= threshold) {
relinKeys.insert(var); relinKeys.insert(var);
@ -148,7 +148,7 @@ void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
// If this node was relinearized, also check its children // If this node was relinearized, also check its children
if(relinearize) { if(relinearize) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child);
} }
} }
@ -161,7 +161,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vect
{ {
// Check the current clique for relinearization // Check the current clique for relinearization
bool relinearize = false; bool relinearize = false;
BOOST_FOREACH(Key var, *clique->conditional()) { for(Key var: *clique->conditional()) {
// Find the threshold for this variable type // Find the threshold for this variable type
const Vector& threshold = thresholds.find(Symbol(var).chr())->second; const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
@ -180,7 +180,7 @@ void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vect
// If this node was relinearized, also check its children // If this node was relinearized, also check its children
if(relinearize) { if(relinearize) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { for(const ISAM2Clique::shared_ptr& child: clique->children) {
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child);
} }
} }
@ -192,7 +192,7 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedCl
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{ {
KeySet relinKeys; KeySet relinKeys;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { for(const ISAM2::sharedClique& root: roots) {
if(relinearizeThreshold.type() == typeid(double)) if(relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root); CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>))
@ -207,7 +207,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke
static const bool debug = false; static const bool debug = false;
// does the separator contain any of the variables? // does the separator contain any of the variables?
bool found = false; bool found = false;
BOOST_FOREACH(Key key, clique->conditional()->parents()) { for(Key key: clique->conditional()->parents()) {
if (markedMask.exists(key)) { if (markedMask.exists(key)) {
found = true; found = true;
break; 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) clique->print("Key(s) marked in clique ");
if(debug) cout << "so marking key " << clique->conditional()->front() << endl; 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); FindAll(child, keys, markedMask);
} }
} }
@ -267,7 +267,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
result.update(clique->conditional()->solve(result)); result.update(clique->conditional()->solve(result));
// starting from the root, call optimize on each conditional // starting from the root, call optimize on each conditional
BOOST_FOREACH(const boost::shared_ptr<ISAM2Clique>& child, clique->children) for(const boost::shared_ptr<ISAM2Clique>& child: clique->children)
optimizeInPlace(child, result); optimizeInPlace(child, result);
} }
} }
@ -280,14 +280,14 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
if (wildfireThreshold <= 0.0) { if (wildfireThreshold <= 0.0) {
// Threshold is zero or less, so do a full recalculation // 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); internal::optimizeInPlace(root, delta);
lastBacksubVariableCount = delta.size(); lastBacksubVariableCount = delta.size();
} else { } else {
// Optimize with wildfire // Optimize with wildfire
lastBacksubVariableCount = 0; lastBacksubVariableCount = 0;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) for(const ISAM2::sharedClique& root: roots)
lastBacksubVariableCount += optimizeWildfireNonRecursive( lastBacksubVariableCount += optimizeWildfireNonRecursive(
root, wildfireThreshold, replacedKeys, delta); // modifies delta root, wildfireThreshold, replacedKeys, delta); // modifies delta
@ -309,7 +309,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& re
// update deltas and recurse to children, but if not, we do not need to // update deltas and recurse to children, but if not, we do not need to
// recurse further because of the running separator property. // recurse further because of the running separator property.
bool anyReplaced = false; bool anyReplaced = false;
BOOST_FOREACH(Key j, *clique->conditional()) { for(Key j: *clique->conditional()) {
if(replacedKeys.exists(j)) { if(replacedKeys.exists(j)) {
anyReplaced = true; anyReplaced = true;
break; break;
@ -327,7 +327,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& re
// Write into RgProd vector // Write into RgProd vector
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
Vector& RgProdValue = RgProd[frontal]; Vector& RgProdValue = RgProd[frontal];
RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size());
vectorPosition += RgProdValue.size(); vectorPosition += RgProdValue.size();
@ -339,7 +339,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& re
varsUpdated += clique->conditional()->nrFrontals(); varsUpdated += clique->conditional()->nrFrontals();
// Recurse to children // 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); } updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); }
} }
} }
@ -351,7 +351,7 @@ size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replac
// Update variables // Update variables
size_t varsUpdated = 0; size_t varsUpdated = 0;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { for(const ISAM2::sharedClique& root: roots) {
internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated);
} }

26
gtsam/nonlinear/ISAM2-inl.h
View File

@ -45,7 +45,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
// Are any clique variables part of the tree that has been redone? // Are any clique variables part of the tree that has been redone?
bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); bool cliqueReplaced = replaced.exists((*clique)->frontals().front());
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
assert(cliqueReplaced == replaced.exists(frontal)); assert(cliqueReplaced == replaced.exists(frontal));
} }
#endif #endif
@ -53,7 +53,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
// If not redone, then has one of the separator variables changed significantly? // If not redone, then has one of the separator variables changed significantly?
bool recalculate = cliqueReplaced; bool recalculate = cliqueReplaced;
if(!recalculate) { if(!recalculate) {
BOOST_FOREACH(Key parent, clique->conditional()->parents()) { for(Key parent: clique->conditional()->parents()) {
if(changed.exists(parent)) { if(changed.exists(parent)) {
recalculate = true; recalculate = true;
break; break;
@ -94,7 +94,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
// If the values were above the threshold or this clique was replaced // If the values were above the threshold or this clique was replaced
if(valuesChanged) { if(valuesChanged) {
// Set changed flag for each frontal variable and leave the new values // 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); changed.insert(frontal);
} }
} else { } else {
@ -105,7 +105,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
} }
// Recurse to children // 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); optimizeWildfire(child, threshold, changed, replaced, delta, count);
} }
} }
@ -122,7 +122,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
// Are any clique variables part of the tree that has been redone? // Are any clique variables part of the tree that has been redone?
bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front());
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
assert(cliqueReplaced == replaced.exists(frontal)); assert(cliqueReplaced == replaced.exists(frontal));
} }
#endif #endif
@ -130,7 +130,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
// If not redone, then has one of the separator variables changed significantly? // If not redone, then has one of the separator variables changed significantly?
bool recalculate = cliqueReplaced; bool recalculate = cliqueReplaced;
if(!recalculate) { if(!recalculate) {
BOOST_FOREACH(Key parent, clique->conditional()->parents()) { for(Key parent: clique->conditional()->parents()) {
if(changed.exists(parent)) { if(changed.exists(parent)) {
recalculate = true; recalculate = true;
break; break;
@ -156,9 +156,9 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
boost::shared_ptr<CLIQUE> parent = clique->parent_.lock(); boost::shared_ptr<CLIQUE> parent = clique->parent_.lock();
if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) 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))); 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))); clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key)));
} }
@ -174,7 +174,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
DenseIndex dim = 0; DenseIndex dim = 0;
FastVector<VectorValues::const_iterator> parentPointers; FastVector<VectorValues::const_iterator> parentPointers;
parentPointers.reserve(clique->conditional()->nrParents()); 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)); parentPointers.push_back(clique->solnPointers_.at(parent));
dim += parentPointers.back()->second.size(); dim += parentPointers.back()->second.size();
} }
@ -182,7 +182,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
// Fill parent vector // Fill parent vector
xS.resize(dim); xS.resize(dim);
DenseIndex vectorPos = 0; DenseIndex vectorPos = 0;
BOOST_FOREACH(const VectorValues::const_iterator& parentPointer, parentPointers) { for(const VectorValues::const_iterator& parentPointer: parentPointers) {
const Vector& parentVector = parentPointer->second; const Vector& parentVector = parentPointer->second;
xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1);
vectorPos += parentVector.size(); vectorPos += parentVector.size();
@ -227,7 +227,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
// If the values were above the threshold or this clique was replaced // If the values were above the threshold or this clique was replaced
if(valuesChanged) { if(valuesChanged) {
// Set changed flag for each frontal variable and leave the new values // 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); changed.insert(frontal);
} }
} else { } else {
@ -270,7 +270,7 @@ size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, doubl
travStack.pop(); travStack.pop();
bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count);
if (recalculate) { if (recalculate) {
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children) { for(const typename CLIQUE::shared_ptr& child: currentNode->children) {
travStack.push(child); travStack.push(child);
} }
} }
@ -287,7 +287,7 @@ void nnz_internal(const boost::shared_ptr<CLIQUE>& clique, int& result) {
int dimSep = (int)clique->conditional()->get_S().cols(); int dimSep = (int)clique->conditional()->get_S().cols();
result += ((dimR+1)*dimR)/2 + dimSep*dimR; result += ((dimR+1)*dimR)/2 + dimSep*dimR;
// traverse the children // 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); nnz_internal(child, result);
} }
} }

104
gtsam/nonlinear/ISAM2.cpp
View File

@ -174,17 +174,17 @@ bool ISAM2::equals(const ISAM2& other, double tol) const {
KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
static const bool debug = false; static const bool debug = false;
if(debug) cout << "Getting affected factors for "; 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; if(debug) cout << endl;
NonlinearFactorGraph allAffected; NonlinearFactorGraph allAffected;
KeySet indices; KeySet indices;
BOOST_FOREACH(const Key key, keys) { for(const Key key: keys) {
const VariableIndex::Factors& factors(variableIndex_[key]); const VariableIndex::Factors& factors(variableIndex_[key]);
indices.insert(factors.begin(), factors.end()); indices.insert(factors.begin(), factors.end());
} }
if(debug) cout << "Affected factors are: "; 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; if(debug) cout << endl;
return indices; return indices;
} }
@ -210,10 +210,10 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
gttic(check_candidates_and_linearize); gttic(check_candidates_and_linearize);
GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>(); GaussianFactorGraph::shared_ptr linearized = boost::make_shared<GaussianFactorGraph>();
BOOST_FOREACH(Key idx, candidates) { for(Key idx: candidates) {
bool inside = true; bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors; bool useCachedLinear = params_.cacheLinearizedFactors;
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { for(Key key: nonlinearFactors_[idx]->keys()) {
if(affectedKeysSet.find(key) == affectedKeysSet.end()) { if(affectedKeysSet.find(key) == affectedKeysSet.end()) {
inside = false; inside = false;
break; break;
@ -251,7 +251,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySe
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
GaussianFactorGraph cachedBoundary; GaussianFactorGraph cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) { for(sharedClique orphan: orphans) {
// retrieve the cached factor and add to boundary // retrieve the cached factor and add to boundary
cachedBoundary.push_back(orphan->cachedFactor()); cachedBoundary.push_back(orphan->cachedFactor());
} }
@ -292,10 +292,10 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
if(debug) { if(debug) {
cout << "markedKeys: "; cout << "markedKeys: ";
BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; } for(const Key key: markedKeys) { cout << key << " "; }
cout << endl; cout << endl;
cout << "observedKeys: "; cout << "observedKeys: ";
BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; } for(const Key key: observedKeys) { cout << key << " "; }
cout << endl; cout << endl;
} }
@ -322,7 +322,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
// ordering provides all keys in conditionals, there cannot be others because path to root included // ordering provides all keys in conditionals, there cannot be others because path to root included
gttic(affectedKeys); gttic(affectedKeys);
FastList<Key> affectedKeys; FastList<Key> affectedKeys;
BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet) for(const ConditionalType::shared_ptr& conditional: affectedBayesNet)
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
gttoc(affectedKeys); gttoc(affectedKeys);
@ -349,7 +349,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
{ {
// Only if some variables are unconstrained // Only if some variables are unconstrained
FastMap<Key, int> constraintGroups; FastMap<Key, int> constraintGroups;
BOOST_FOREACH(Key var, observedKeys) for(Key var: observedKeys)
constraintGroups[var] = 1; constraintGroups[var] = 1;
order = Ordering::ColamdConstrained(variableIndex_, constraintGroups); order = Ordering::ColamdConstrained(variableIndex_, constraintGroups);
} }
@ -386,7 +386,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
// Reeliminated keys for detailed results // Reeliminated keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Key key, theta_.keys()) { for(Key key: theta_.keys()) {
result.detail->variableStatus[key].isReeliminated = true; result.detail->variableStatus[key].isReeliminated = true;
} }
} }
@ -406,11 +406,11 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
if(debug) factors.print("Relinearized factors: "); if(debug) factors.print("Relinearized factors: ");
gttoc(relinearizeAffected); 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 // Reeliminated keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Key key, affectedAndNewKeys) { for(Key key: affectedAndNewKeys) {
result.detail->variableStatus[key].isReeliminated = true; result.detail->variableStatus[key].isReeliminated = true;
} }
} }
@ -437,7 +437,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
gttic(orphans); gttic(orphans);
// Add the orphaned subtrees // Add the orphaned subtrees
BOOST_FOREACH(const sharedClique& orphan, orphans) for(const sharedClique& orphan: orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan); factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
gttoc(orphans); gttoc(orphans);
@ -465,7 +465,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
} else { } else {
constraintGroups = FastMap<Key,int>(); constraintGroups = FastMap<Key,int>();
const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0;
BOOST_FOREACH (Key var, observedKeys) for (Key var: observedKeys)
constraintGroups.insert(make_pair(var, group)); constraintGroups.insert(make_pair(var, group));
} }
@ -500,8 +500,8 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
// Root clique variables for detailed results // Root clique variables for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(const sharedNode& root, this->roots()) for(const sharedNode& root: this->roots())
BOOST_FOREACH(Key var, *root->conditional()) for(Key var: *root->conditional())
result.detail->variableStatus[var].inRootClique = true; result.detail->variableStatus[var].inRootClique = true;
} }
@ -554,7 +554,7 @@ ISAM2Result ISAM2::update(
// Remove the removed factors // Remove the removed factors
NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size());
BOOST_FOREACH(size_t index, removeFactorIndices) { for(size_t index: removeFactorIndices) {
removeFactors.push_back(nonlinearFactors_[index]); removeFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index); nonlinearFactors_.remove(index);
if(params_.cacheLinearizedFactors) if(params_.cacheLinearizedFactors)
@ -571,7 +571,7 @@ ISAM2Result ISAM2::update(
// Get keys from removed factors and new factors, and compute unused keys, // 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. // i.e., keys that are empty now and do not appear in the new factors.
KeySet removedAndEmpty; KeySet removedAndEmpty;
BOOST_FOREACH(Key key, removeFactors.keys()) { for(Key key: removeFactors.keys()) {
if(variableIndex_[key].empty()) if(variableIndex_[key].empty())
removedAndEmpty.insert(removedAndEmpty.end(), key); removedAndEmpty.insert(removedAndEmpty.end(), key);
} }
@ -580,7 +580,7 @@ ISAM2Result ISAM2::update(
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
// Get indices for unused keys // Get indices for unused keys
BOOST_FOREACH(Key key, unusedKeys) { for(Key key: unusedKeys) {
unusedIndices.insert(unusedIndices.end(), key); unusedIndices.insert(unusedIndices.end(), key);
} }
} }
@ -591,7 +591,7 @@ ISAM2Result ISAM2::update(
Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_);
// New keys for detailed results // New keys for detailed results
if(params_.enableDetailedResults) { 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); gttoc(add_new_variables);
gttic(evaluate_error_before); gttic(evaluate_error_before);
@ -609,14 +609,14 @@ ISAM2Result ISAM2::update(
} }
// Also mark any provided extra re-eliminate keys // Also mark any provided extra re-eliminate keys
if(extraReelimKeys) { if(extraReelimKeys) {
BOOST_FOREACH(Key key, *extraReelimKeys) { for(Key key: *extraReelimKeys) {
markedKeys.insert(key); markedKeys.insert(key);
} }
} }
// Observed keys for detailed results // Observed keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Key key, markedKeys) { for(Key key: markedKeys) {
result.detail->variableStatus[key].isObserved = true; 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 // is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Key value) instead of the iterator constructor. // vector(size_t count, Key value) instead of the iterator constructor.
FastVector<Key> observedKeys; observedKeys.reserve(markedKeys.size()); FastVector<Key> 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 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 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 if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging
// Remove from relinKeys any keys whose linearization points are fixed // Remove from relinKeys any keys whose linearization points are fixed
BOOST_FOREACH(Key key, fixedVariables_) { for(Key key: fixedVariables_) {
relinKeys.erase(key); relinKeys.erase(key);
} }
if(noRelinKeys) { if(noRelinKeys) {
BOOST_FOREACH(Key key, *noRelinKeys) { for(Key key: *noRelinKeys) {
relinKeys.erase(key); relinKeys.erase(key);
} }
} }
// Above relin threshold keys for detailed results // Above relin threshold keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Key key, relinKeys) { for(Key key: relinKeys) {
result.detail->variableStatus[key].isAboveRelinThreshold = true; result.detail->variableStatus[key].isAboveRelinThreshold = true;
result.detail->variableStatus[key].isRelinearized = true; } } result.detail->variableStatus[key].isRelinearized = true; } }
// Add the variables being relinearized to the marked keys // Add the variables being relinearized to the marked keys
KeySet markedRelinMask; KeySet markedRelinMask;
BOOST_FOREACH(const Key key, relinKeys) for(const Key key: relinKeys)
markedRelinMask.insert(key); markedRelinMask.insert(key);
markedKeys.insert(relinKeys.begin(), relinKeys.end()); markedKeys.insert(relinKeys.begin(), relinKeys.end());
gttoc(gather_relinearize_keys); gttoc(gather_relinearize_keys);
@ -667,16 +667,16 @@ ISAM2Result ISAM2::update(
gttic(fluid_find_all); gttic(fluid_find_all);
// 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
if (!relinKeys.empty()) { 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 // add other cliques that have the marked ones in the separator
Impl::FindAll(root, markedKeys, markedRelinMask); Impl::FindAll(root, markedKeys, markedRelinMask);
// Relin involved keys for detailed results // Relin involved keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
KeySet involvedRelinKeys; KeySet involvedRelinKeys;
BOOST_FOREACH(const sharedClique& root, roots_) for(const sharedClique& root: roots_)
Impl::FindAll(root, involvedRelinKeys, markedRelinMask); Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
BOOST_FOREACH(Key key, involvedRelinKeys) { for(Key key: involvedRelinKeys) {
if(!result.detail->variableStatus[key].isAboveRelinThreshold) { if(!result.detail->variableStatus[key].isAboveRelinThreshold) {
result.detail->variableStatus[key].isRelinearizeInvolved = true; result.detail->variableStatus[key].isRelinearizeInvolved = true;
result.detail->variableStatus[key].isRelinearized = true; } } result.detail->variableStatus[key].isRelinearized = true; } }
@ -771,7 +771,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
KeySet leafKeysRemoved; KeySet leafKeysRemoved;
// Remove each variable and its subtrees // 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 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 // Traverse up the tree to find the root of the marginalized subtree
@ -789,7 +789,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// See if we should remove the whole clique // See if we should remove the whole clique
bool marginalizeEntireClique = true; bool marginalizeEntireClique = true;
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
if(!leafKeys.exists(frontal)) { if(!leafKeys.exists(frontal)) {
marginalizeEntireClique = false; marginalizeEntireClique = false;
break; } } break; } }
@ -806,10 +806,10 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// Now remove this clique and its subtree - all of its marginal // Now remove this clique and its subtree - all of its marginal
// information has been stored in marginalFactors. // information has been stored in marginalFactors.
const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques 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()); marginalFactors.erase(removedClique->conditional()->front());
leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); 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 // Add to factors to remove
const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
@ -832,9 +832,9 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
GaussianFactorGraph graph; GaussianFactorGraph graph;
KeySet factorsInSubtreeRoot; KeySet factorsInSubtreeRoot;
Cliques subtreesToRemove; Cliques subtreesToRemove;
BOOST_FOREACH(const sharedClique& child, clique->children) { for(const sharedClique& child: clique->children) {
// Remove subtree if child depends on any marginalized keys // 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)) { if(leafKeys.exists(parent)) {
subtreesToRemove.push_back(child); subtreesToRemove.push_back(child);
graph.push_back(child->cachedFactor()); // Add child marginal graph.push_back(child->cachedFactor()); // Add child marginal
@ -843,13 +843,13 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
} }
} }
Cliques childrenRemoved; 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 const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques
childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end());
BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { for(const sharedClique& removedClique: removedCliques) {
marginalFactors.erase(removedClique->conditional()->front()); marginalFactors.erase(removedClique->conditional()->front());
leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); 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 // Add to factors to remove
const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
@ -867,16 +867,16 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// but do not involve frontal variables of any of its children. // but do not involve frontal variables of any of its children.
// TODO: reuse cached linear factors // TODO: reuse cached linear factors
KeySet factorsFromMarginalizedInClique_step1; KeySet factorsFromMarginalizedInClique_step1;
BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
if(leafKeys.exists(frontal)) if(leafKeys.exists(frontal))
factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
// Remove any factors in subtrees that we're removing at this step // Remove any factors in subtrees that we're removing at this step
BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { for(const sharedClique& removedChild: childrenRemoved) {
BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { for(Key indexInClique: removedChild->conditional()->frontals()) {
BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) { for(Key factorInvolving: variableIndex_[indexInClique]) {
factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } }
// Create factor graph from factor indices // 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_)); } graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
// Reeliminate the linear graph to get the marginal and discard the conditional // Reeliminate the linear graph to get the marginal and discard the conditional
@ -908,7 +908,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
clique->conditional()->nrFrontals() -= nToRemove; clique->conditional()->nrFrontals() -= nToRemove;
// Add to factors to remove factors involved in frontals of current clique // 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]; const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end());
@ -925,8 +925,8 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// Gather factors to add - the new marginal factors // Gather factors to add - the new marginal factors
GaussianFactorGraph factorsToAdd; GaussianFactorGraph factorsToAdd;
typedef pair<Key, vector<GaussianFactor::shared_ptr> > Key_Factors; typedef pair<Key, vector<GaussianFactor::shared_ptr> > Key_Factors;
BOOST_FOREACH(const Key_Factors& key_factors, marginalFactors) { for(const Key_Factors& key_factors: marginalFactors) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, key_factors.second) { for(const GaussianFactor::shared_ptr& factor: key_factors.second) {
if(factor) { if(factor) {
factorsToAdd.push_back(factor); factorsToAdd.push_back(factor);
if(marginalFactorsIndices) if(marginalFactorsIndices)
@ -935,7 +935,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
factor)); factor));
if(params_.cacheLinearizedFactors) if(params_.cacheLinearizedFactors)
linearFactors_.push_back(factor); linearFactors_.push_back(factor);
BOOST_FOREACH(Key factorKey, *factor) { for(Key factorKey: *factor) {
fixedVariables_.insert(factorKey); } fixedVariables_.insert(factorKey); }
} }
} }
@ -944,7 +944,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
// Remove the factors to remove that have been summarized in the newly-added marginal factors // Remove the factors to remove that have been summarized in the newly-added marginal factors
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t i, factorIndicesToRemove) { for(size_t i: factorIndicesToRemove) {
removedFactors.push_back(nonlinearFactors_[i]); removedFactors.push_back(nonlinearFactors_[i]);
nonlinearFactors_.remove(i); nonlinearFactors_.remove(i);
if(params_.cacheLinearizedFactors) if(params_.cacheLinearizedFactors)
@ -1065,7 +1065,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
// Recursively add contributions from children // Recursively add contributions from children
typedef boost::shared_ptr<ISAM2Clique> sharedClique; typedef boost::shared_ptr<ISAM2Clique> sharedClique;
BOOST_FOREACH(const sharedClique& child, root->children) { for(const sharedClique& child: root->children) {
gradientAtZeroTreeAdder(child, g); gradientAtZeroTreeAdder(child, g);
} }
} }
@ -1077,7 +1077,7 @@ VectorValues ISAM2::gradientAtZero() const
VectorValues g; VectorValues g;
// Sum up contributions for each clique // Sum up contributions for each clique
BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) for(const ISAM2::sharedClique& root: this->roots())
gradientAtZeroTreeAdder(root, g); gradientAtZeroTreeAdder(root, g);
return g; return g;

2
gtsam/nonlinear/ISAM2.h
View File

@ -200,7 +200,7 @@ struct GTSAM_EXPORT ISAM2Params {
else else
{ {
std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; std::cout << "relinearizeThreshold: " << "{mapped}" << "\n";
BOOST_FOREACH(const ISAM2ThresholdMapValue& value, boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) { for(const ISAM2ThresholdMapValue& value: boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n";
} }
} }

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -157,7 +157,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
// Only retrieve diagonal vector when reuse_diagonal = false // Only retrieve diagonal vector when reuse_diagonal = false
if (params_.diagonalDamping && state_.reuseDiagonal == false) { if (params_.diagonalDamping && state_.reuseDiagonal == false) {
state_.hessianDiagonal = linear.hessianDiagonal(); 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++) { for (int aa = 0; aa < v.size(); aa++) {
v(aa) = std::min(std::max(v(aa), params_.minDiagonal), v(aa) = std::min(std::max(v(aa), params_.minDiagonal),
params_.maxDiagonal); params_.maxDiagonal);
@ -172,7 +172,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
GaussianFactorGraph &damped = (*dampedPtr); GaussianFactorGraph &damped = (*dampedPtr);
damped.reserve(damped.size() + state_.values.size()); damped.reserve(damped.size() + state_.values.size());
if (params_.diagonalDamping) { 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) // Fill in the diagonal of A with diag(hessian)
try { try {
Matrix A = Eigen::DiagonalMatrix<double, Eigen::Dynamic>( Matrix A = Eigen::DiagonalMatrix<double, Eigen::Dynamic>(
@ -192,7 +192,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
// initialize noise model cache to a reasonable default size // initialize noise model cache to a reasonable default size
NoiseCacheVector noises(6); 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(); size_t dim = key_value.value.dim();
if (dim > noises.size()) if (dim > noises.size())

9
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -11,7 +11,6 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>
namespace gtsam { namespace gtsam {
@ -19,7 +18,7 @@ namespace gtsam {
void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) { void LinearContainerFactor::initializeLinearizationPoint(const Values& linearizationPoint) {
if (!linearizationPoint.empty()) { if (!linearizationPoint.empty()) {
linearizationPoint_ = Values(); linearizationPoint_ = Values();
BOOST_FOREACH(const gtsam::Key& key, this->keys()) { for(const gtsam::Key& key: this->keys()) {
linearizationPoint_->insert(key, linearizationPoint.at(key)); linearizationPoint_->insert(key, linearizationPoint.at(key));
} }
} else { } else {
@ -82,7 +81,7 @@ double LinearContainerFactor::error(const Values& c) const {
// Extract subset of values for comparison // Extract subset of values for comparison
Values csub; Values csub;
BOOST_FOREACH(const gtsam::Key& key, keys()) for(const gtsam::Key& key: keys())
csub.insert(key, c.at(key)); csub.insert(key, c.at(key));
// create dummy ordering for evaluation // create dummy ordering for evaluation
@ -111,7 +110,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
// Extract subset of values // Extract subset of values
Values subsetC; Values subsetC;
BOOST_FOREACH(const gtsam::Key& key, this->keys()) for(const gtsam::Key& key: this->keys())
subsetC.insert(key, c.at(key)); subsetC.insert(key, c.at(key));
// Determine delta between linearization points using new ordering // Determine delta between linearization points using new ordering
@ -170,7 +169,7 @@ NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
const GaussianFactorGraph& linear_graph, const Values& linearizationPoint) const GaussianFactorGraph& linear_graph, const Values& linearizationPoint)
{ {
NonlinearFactorGraph result; NonlinearFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph) for(const GaussianFactor::shared_ptr& f: linear_graph)
if (f) if (f)
result.push_back(NonlinearFactorGraph::sharedFactor( result.push_back(NonlinearFactorGraph::sharedFactor(
new LinearContainerFactor(f, linearizationPoint))); new LinearContainerFactor(f, linearizationPoint)));

4
gtsam/nonlinear/Marginals.cpp
View File

@ -127,7 +127,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
// Get dimensions from factor graph // Get dimensions from factor graph
std::vector<size_t> dims; std::vector<size_t> dims;
dims.reserve(variablesSorted.size()); dims.reserve(variablesSorted.size());
BOOST_FOREACH(Key key, variablesSorted) { for(Key key: variablesSorted) {
dims.push_back(values_.at(key).dim()); 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 { void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
cout << s << "Joint marginal on keys "; cout << s << "Joint marginal on keys ";
bool first = true; bool first = true;
BOOST_FOREACH(Key key, keys_) { for(Key key: keys_) {
if(!first) if(!first)
cout << ", "; cout << ", ";
else else

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
void NonlinearFactor::print(const std::string& s, void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
std::cout << s << " keys = { "; std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, keys()) { for(Key key: keys()) {
std::cout << keyFormatter(key) << " "; std::cout << keyFormatter(key) << " ";
} }
std::cout << "}" << std::endl; std::cout << "}" << std::endl;

25
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -31,7 +31,6 @@
# include <tbb/parallel_for.h> # include <tbb/parallel_for.h>
#endif #endif
#include <boost/foreach.hpp>
#include <cmath> #include <cmath>
#include <limits> #include <limits>
@ -133,7 +132,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
// Find bounds // Find bounds
double minX = numeric_limits<double>::infinity(), maxX = -numeric_limits<double>::infinity(); double minX = numeric_limits<double>::infinity(), maxX = -numeric_limits<double>::infinity();
double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity(); double minY = numeric_limits<double>::infinity(), maxY = -numeric_limits<double>::infinity();
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
if(values.exists(key)) { if(values.exists(key)) {
boost::optional<Point2> xy = getXY(values.at(key), formatting); boost::optional<Point2> xy = getXY(values.at(key), formatting);
if(xy) { if(xy) {
@ -150,7 +149,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
} }
// Create nodes for each variable in the graph // 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 // Label the node with the label from the KeyFormatter
stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; stm << " var" << key << "[label=\"" << keyFormatter(key) << "\"";
if(values.exists(key)) { if(values.exists(key)) {
@ -165,7 +164,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
if (formatting.mergeSimilarFactors) { if (formatting.mergeSimilarFactors) {
// Remove duplicate factors // Remove duplicate factors
std::set<vector<Key> > structure; std::set<vector<Key> > structure;
BOOST_FOREACH(const sharedFactor& factor, *this){ for(const sharedFactor& factor: *this){
if(factor) { if(factor) {
vector<Key> factorKeys = factor->keys(); vector<Key> factorKeys = factor->keys();
std::sort(factorKeys.begin(), factorKeys.end()); std::sort(factorKeys.begin(), factorKeys.end());
@ -175,7 +174,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
// Create factors and variable connections // Create factors and variable connections
size_t i = 0; size_t i = 0;
BOOST_FOREACH(const vector<Key>& factorKeys, structure){ for(const vector<Key>& factorKeys: structure){
// Make each factor a dot // Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point"; stm << " factor" << i << "[label=\"\", shape=point";
{ {
@ -187,7 +186,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
stm << "];\n"; stm << "];\n";
// Make factor-variable connections // Make factor-variable connections
BOOST_FOREACH(Key key, factorKeys) { for(Key key: factorKeys) {
stm << " var" << key << "--" << "factor" << i << ";\n"; stm << " var" << key << "--" << "factor" << i << ";\n";
} }
@ -214,7 +213,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
// Make factor-variable connections // Make factor-variable connections
if(formatting.connectKeysToFactor && factor) { if(formatting.connectKeysToFactor && factor) {
BOOST_FOREACH(Key key, *factor) { for(Key key: *factor) {
stm << " var" << key << "--" << "factor" << i << ";\n"; stm << " var" << key << "--" << "factor" << i << ";\n";
} }
} }
@ -224,7 +223,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
if(factor) { if(factor) {
Key k; Key k;
bool firstTime = true; bool firstTime = true;
BOOST_FOREACH(Key key, *this->at(i)) { for(Key key: *this->at(i)) {
if(firstTime) { if(firstTime) {
k = key; k = key;
firstTime = false; firstTime = false;
@ -246,7 +245,7 @@ double NonlinearFactorGraph::error(const Values& values) const {
gttic(NonlinearFactorGraph_error); gttic(NonlinearFactorGraph_error);
double total_error = 0.; double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities // 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) if(factor)
total_error += factor->error(values); total_error += factor->error(values);
} }
@ -272,7 +271,7 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const
SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared<SymbolicFactorGraph>(); SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared<SymbolicFactorGraph>();
symbolic->reserve(this->size()); symbolic->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, *this) { for(const sharedFactor& factor: *this) {
if(factor) if(factor)
*symbolic += SymbolicFactor(*factor); *symbolic += SymbolicFactor(*factor);
else else
@ -330,7 +329,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
linearFG->reserve(this->size()); linearFG->reserve(this->size());
// linearize all factors // linearize all factors
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { for(const sharedFactor& factor: this->factors_) {
if(factor) { if(factor) {
(*linearFG) += factor->linearize(linearizationPoint); (*linearFG) += factor->linearize(linearizationPoint);
} else } else
@ -345,7 +344,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph NonlinearFactorGraph::clone() const { NonlinearFactorGraph NonlinearFactorGraph::clone() const {
NonlinearFactorGraph result; NonlinearFactorGraph result;
BOOST_FOREACH(const sharedFactor& f, *this) { for(const sharedFactor& f: *this) {
if (f) if (f)
result.push_back(f->clone()); result.push_back(f->clone());
else else
@ -357,7 +356,7 @@ NonlinearFactorGraph NonlinearFactorGraph::clone() const {
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const { NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
NonlinearFactorGraph result; NonlinearFactorGraph result;
BOOST_FOREACH(const sharedFactor& f, *this) { for(const sharedFactor& f: *this) {
if (f) if (f)
result.push_back(f->rekey(rekey_mapping)); result.push_back(f->rekey(rekey_mapping));
else else

6
gtsam/nonlinear/Values-inl.h
View File

@ -26,8 +26,6 @@
#include <utility> #include <utility>
#include <boost/foreach.hpp>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
@ -220,7 +218,7 @@ namespace gtsam {
/** Constructor from a Filtered view copies out all values */ /** Constructor from a Filtered view copies out all values */
template<class ValueType> template<class ValueType>
Values::Values(const Values::Filtered<ValueType>& view) { Values::Values(const Values::Filtered<ValueType>& view) {
BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) { for(const typename Filtered<ValueType>::KeyValuePair& key_value: view) {
Key key = key_value.key; Key key = key_value.key;
insert(key, static_cast<const ValueType&>(key_value.value)); insert(key, static_cast<const ValueType&>(key_value.value));
} }
@ -229,7 +227,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class ValueType> template<class ValueType>
Values::Values(const Values::ConstFiltered<ValueType>& view) { Values::Values(const Values::ConstFiltered<ValueType>& view) {
BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) { for(const typename ConstFiltered<ValueType>::KeyValuePair& key_value: view) {
Key key = key_value.key; Key key = key_value.key;
insert(key, static_cast<const ValueType&>(key_value.value)); insert(key, static_cast<const ValueType&>(key_value.value));
} }

5
gtsam/nonlinear/Values.cpp
View File

@ -25,7 +25,6 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#ifdef __GNUC__ #ifdef __GNUC__
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable" #pragma GCC diagnostic ignored "-Wunused-variable"
@ -194,7 +193,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
size_t Values::dim() const { size_t Values::dim() const {
size_t result = 0; size_t result = 0;
BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { for(const ConstKeyValuePair& key_value: *this) {
result += key_value.value.dim(); result += key_value.value.dim();
} }
return result; return result;
@ -203,7 +202,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues Values::zeroVectors() const { VectorValues Values::zeroVectors() const {
VectorValues result; 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())); result.insert(key_value.key, Vector::Zero(key_value.value.dim()));
return result; return result;
} }

2
gtsam/nonlinear/factorTesting.h
View File

@ -49,7 +49,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
// Loop over all variables // Loop over all variables
const double one_over_2delta = 1.0 / (2.0 * delta); const double one_over_2delta = 1.0 / (2.0 * delta);
VectorValues dX = values.zeroVectors(); VectorValues dX = values.zeroVectors();
BOOST_FOREACH(Key key, factor) { for(Key key: factor) {
// Compute central differences using the values struct. // Compute central differences using the values struct.
const size_t cols = dX.dim(key); const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols); Matrix J = Matrix::Zero(rows, cols);

6
gtsam/nonlinear/tests/testValues.cpp
View File

@ -366,7 +366,7 @@ TEST(Values, filter) {
int i = 0; int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2)); Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
EXPECT_LONGS_EQUAL(2, (long)filtered.size()); 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) { if(i == 0) {
LONGS_EQUAL(2, (long)key_value.key); LONGS_EQUAL(2, (long)key_value.key);
try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
@ -401,7 +401,7 @@ TEST(Values, filter) {
i = 0; i = 0;
Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>(); Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();
EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size()); EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size());
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, pose_filtered) { for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: pose_filtered) {
if(i == 0) { if(i == 0) {
EXPECT_LONGS_EQUAL(1, (long)key_value.key); EXPECT_LONGS_EQUAL(1, (long)key_value.key);
EXPECT(assert_equal(pose1, key_value.value)); EXPECT(assert_equal(pose1, key_value.value));
@ -437,7 +437,7 @@ TEST(Values, Symbol_filter) {
values.insert(Symbol('y', 3), pose3); values.insert(Symbol('y', 3), pose3);
int i = 0; int i = 0;
BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { for(const Values::Filtered<Value>::KeyValuePair& key_value: values.filter(Symbol::ChrTest('y'))) {
if(i == 0) { if(i == 0) {
LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>())); EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));

24
gtsam/slam/InitializePose3.cpp
View File

@ -43,7 +43,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
GaussianFactorGraph linearGraph; GaussianFactorGraph linearGraph;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) { for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
Matrix3 Rij; Matrix3 Rij;
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
@ -75,7 +75,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3; Values validRot3;
BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { for(const VectorValues::value_type& it: relaxedRot3) {
Key key = it.first; Key key = it.first;
if (key != keyAnchor) { if (key != keyAnchor) {
const Vector& rotVector = it.second; const Vector& rotVector = it.second;
@ -108,7 +108,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
gttic(InitializePose3_buildPose3graph); gttic(InitializePose3_buildPose3graph);
NonlinearFactorGraph pose3Graph; NonlinearFactorGraph pose3Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) { for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
// recast to a between on Pose3 // recast to a between on Pose3
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
@ -150,7 +150,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// this works on the inverse rotations, according to Tron&Vidal,2011 // this works on the inverse rotations, according to Tron&Vidal,2011
Values inverseRot; Values inverseRot;
inverseRot.insert(keyAnchor, Rot3()); inverseRot.insert(keyAnchor, Rot3());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) { for(const Values::ConstKeyValuePair& key_value: givenGuess) {
Key key = key_value.key; Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key); const Pose3& pose = givenGuess.at<Pose3>(key);
inverseRot.insert(key, pose.rotation().inverse()); inverseRot.insert(key, pose.rotation().inverse());
@ -165,7 +165,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// calculate max node degree & allocate gradient // calculate max node degree & allocate gradient
size_t maxNodeDeg = 0; size_t maxNodeDeg = 0;
VectorValues grad; VectorValues grad;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { for(const Values::ConstKeyValuePair& key_value: inverseRot) {
Key key = key_value.key; Key key = key_value.key;
grad.insert(key,Vector3::Zero()); grad.insert(key,Vector3::Zero());
size_t currNodeDeg = (adjEdgesMap.at(key)).size(); 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 //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
// <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
maxGrad = 0; maxGrad = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { for(const Values::ConstKeyValuePair& key_value: inverseRot) {
Key key = key_value.key; Key key = key_value.key;
//std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl;
Vector gradKey = Vector3::Zero(); Vector gradKey = Vector3::Zero();
// collect the gradient for each edge incident on key // 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 Rij = factorId2RotMap.at(factorId);
Rot3 Ri = inverseRot.at<Rot3>(key); Rot3 Ri = inverseRot.at<Rot3>(key);
if( key == (pose3Graph.at(factorId))->keys()[0] ){ if( key == (pose3Graph.at(factorId))->keys()[0] ){
@ -236,7 +236,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// Return correct rotations // Return correct rotations
const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
Values estimateRot; Values estimateRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { for(const Values::ConstKeyValuePair& key_value: inverseRot) {
Key key = key_value.key; Key key = key_value.key;
if (key != keyAnchor) { if (key != keyAnchor) {
const Rot3& R = inverseRot.at<Rot3>(key); const Rot3& R = inverseRot.at<Rot3>(key);
@ -252,7 +252,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
/* ************************************************************************* */ /* ************************************************************************* */
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
size_t factorId = 0; size_t factorId = 0;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose3Graph) { for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){ if (pose3Between){
@ -321,7 +321,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure // put into Values structure
Values initialPose; Values initialPose;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ for(const Values::ConstKeyValuePair& key_value: initialRot){
Key key = key_value.key; Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key); const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
@ -346,7 +346,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure // put into Values structure
Values estimate; Values estimate;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) { for(const Values::ConstKeyValuePair& key_value: GNresult) {
Key key = key_value.key; Key key = key_value.key;
if (key != keyAnchor) { if (key != keyAnchor) {
const Pose3& pose = GNresult.at<Pose3>(key); const Pose3& pose = GNresult.at<Pose3>(key);
@ -391,7 +391,7 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
// Compute the full poses (1 GN iteration on full poses) // Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, orientations); return computePoses(pose3Graph, orientations);
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { // for(const Values::ConstKeyValuePair& key_value: orientations) {
// Key key = key_value.key; // Key key = key_value.key;
// if (key != keyAnchor) { // if (key != keyAnchor) {
// const Point3& pos = givenGuess.at<Pose3>(key).translation(); // const Point3& pos = givenGuess.at<Pose3>(key).translation();

2
gtsam/slam/JacobianFactorQ.h
View File

@ -44,7 +44,7 @@ public:
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(keys.size()); QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys) for(const Key& key: keys)
QF.push_back(KeyMatrix(key, zeroMatrix)); QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model); JacobianFactor::fillTerms(QF, zeroVector, model);
} }

4
gtsam/slam/JacobianFactorSVD.h
View File

@ -45,7 +45,7 @@ public:
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(keys.size()); QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys) for(const Key& key: keys)
QF.push_back(KeyMatrix(key, zeroMatrix)); QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model); JacobianFactor::fillTerms(QF, zeroVector, model);
} }
@ -67,7 +67,7 @@ public:
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
// PLAIN NULL SPACE TRICK // PLAIN NULL SPACE TRICK
// Matrix Q = Enull * Enull.transpose(); // 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)); // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b); // JacobianFactor factor(QF, Q * b);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;

2
gtsam/slam/SmartFactorBase.h
View File

@ -161,7 +161,7 @@ public:
/// Collect all cameras: important that in key order /// Collect all cameras: important that in key order
virtual Cameras cameras(const Values& values) const { virtual Cameras cameras(const Values& values) const {
Cameras cameras; Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_) for(const Key& k: this->keys_)
cameras.push_back(values.at<CAMERA>(k)); cameras.push_back(values.at<CAMERA>(k));
return cameras; return cameras;
} }

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -290,9 +290,9 @@ public:
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian // failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs) for(Matrix& m: Gs)
m = Matrix::Zero(Base::Dim, Base::Dim); m = Matrix::Zero(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs) for(Vector& v: gs)
v = Vector::Zero(Base::Dim); v = Vector::Zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0); Gs, gs, 0.0);

2
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -123,7 +123,7 @@ public:
*/ */
typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras; typename Base::Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_) { for(const Key& k: this->keys_) {
Pose3 pose = values.at<Pose3>(k); Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_) if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_)); pose = pose.compose(*(Base::body_P_sensor_));

21
gtsam/slam/dataset.cpp
View File

@ -37,7 +37,6 @@
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#include <boost/filesystem/operations.hpp> #include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp> #include <boost/filesystem/path.hpp>
#include <boost/foreach.hpp>
#include <cmath> #include <cmath>
#include <fstream> #include <fstream>
@ -68,8 +67,8 @@ string findExampleDataFile(const string& name) {
namesToSearch.push_back(name + ".out"); namesToSearch.push_back(name + ".out");
// Find first name that exists // Find first name that exists
BOOST_FOREACH(const fs::path& root, rootsToSearch) { for(const fs::path& root: rootsToSearch) {
BOOST_FOREACH(const fs::path& name, namesToSearch) { for(const fs::path& name: namesToSearch) {
if (fs::is_regular_file(root / name)) if (fs::is_regular_file(root / name))
return (root / name).string(); return (root / name).string();
} }
@ -366,7 +365,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
// save poses // save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { for(const Values::ConstKeyValuePair& key_value: config) {
const Pose2& pose = key_value.value.cast<Pose2>(); const Pose2& pose = key_value.value.cast<Pose2>();
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< " " << pose.theta() << endl; << " " << pose.theta() << endl;
@ -375,7 +374,7 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
// save edges // save edges
Matrix R = model->R(); Matrix R = model->R();
Matrix RR = trans(R) * R; //prod(trans(R),R); Matrix RR = trans(R) * R; //prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) { for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor) if (!factor)
@ -413,13 +412,13 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
// save 2D & 3D poses // save 2D & 3D poses
Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>(); Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, viewPose2) { for(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value: viewPose2) {
stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " " stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
<< key_value.value.y() << " " << key_value.value.theta() << endl; << key_value.value.y() << " " << key_value.value.theta() << endl;
} }
Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>(); Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, viewPose3) { for(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value: viewPose3) {
Point3 p = key_value.value.translation(); Point3 p = key_value.value.translation();
Rot3 R = key_value.value.rotation(); Rot3 R = key_value.value.rotation();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z() 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) // save edges (2D or 3D)
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) { for(boost::shared_ptr<NonlinearFactor> factor_: graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (factor){ if (factor){
@ -898,7 +897,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
Values initialCamerasEstimate(const SfM_data& db) { Values initialCamerasEstimate(const SfM_data& db) {
Values initial; Values initial;
size_t i = 0; // NO POINTS: j = 0; 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); initial.insert(i++, camera);
return initial; return initial;
} }
@ -906,9 +905,9 @@ Values initialCamerasEstimate(const SfM_data& db) {
Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initialCamerasAndPointsEstimate(const SfM_data& db) {
Values initial; Values initial;
size_t i = 0, j = 0; 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); 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); initial.insert(P(j++), track.p);
return initial; return initial;
} }

18
gtsam/slam/lago.cpp
View File

@ -82,7 +82,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0)); thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
// for all nodes in the tree // 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 // compute the orientation wrt root
Key nodeKey = it.first; Key nodeKey = it.first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
@ -101,7 +101,7 @@ void getSymbolicGraph(
// Get keys for which you want the orientation // Get keys for which you want the orientation
size_t id = 0; size_t id = 0;
// Loop over the factors // Loop over the factors
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) { for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
if (factor->keys().size() == 2) { if (factor->keys().size() == 2) {
Key key1 = factor->keys()[0]; Key key1 = factor->keys()[0];
Key key2 = factor->keys()[1]; Key key2 = factor->keys()[1];
@ -167,14 +167,14 @@ GaussianFactorGraph buildLinearOrientationGraph(
noiseModel::Diagonal::shared_ptr model_deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta;
// put original measurements in the spanning tree // put original measurements in the spanning tree
BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { for(const size_t& factorId: spanningTreeIds) {
const FastVector<Key>& keys = g[factorId]->keys(); const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
} }
// put regularized measurements in the chordsIds // put regularized measurements in the chordsIds
BOOST_FOREACH(const size_t& factorId, chordsIds) { for(const size_t& factorId: chordsIds) {
const FastVector<Key>& keys = g[factorId]->keys(); const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
@ -199,7 +199,7 @@ static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
gttic(lago_buildPose2graph); gttic(lago_buildPose2graph);
NonlinearFactorGraph pose2Graph; NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) { for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
// recast to a between on Pose2 // recast to a between on Pose2
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between = boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
@ -226,7 +226,7 @@ static PredecessorMap<Key> findOdometricPath(
Key minKey = keyAnchor; // this initialization does not matter Key minKey = keyAnchor; // this initialization does not matter
bool minUnassigned = true; bool minUnassigned = true;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) { for(const boost::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
Key key2 = std::max(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; GaussianFactorGraph linearPose2graph;
// We include the linear version of each between factor // We include the linear version of each between factor
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) { for(const boost::shared_ptr<NonlinearFactor>& factor: pose2graph) {
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between = boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
@ -346,7 +346,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
// put into Values structure // put into Values structure
Values initialGuessLago; Values initialGuessLago;
BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { for(const VectorValues::value_type& it: posesLago) {
Key key = it.first; Key key = it.first;
if (key != keyAnchor) { if (key != keyAnchor) {
const Vector& poseVector = it.second; const Vector& poseVector = it.second;
@ -383,7 +383,7 @@ Values initialize(const NonlinearFactorGraph& graph,
VectorValues orientations = initializeOrientations(graph); VectorValues orientations = initializeOrientations(graph);
// for all nodes in the tree // 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; Key key = it.first;
if (key != keyAnchor) { if (key != keyAnchor) {
const Pose2& pose = initialGuess.at<Pose2>(key); const Pose2& pose = initialGuess.at<Pose2>(key);

2
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -465,7 +465,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) {
} }
// Now loop over all these noise models // Now loop over all these noise models
BOOST_FOREACH(SharedNoiseModel model, models) { for(SharedNoiseModel model: models) {
Projection factor(measurement, model, X(1), L(1)); Projection factor(measurement, model, X(1), L(1));
// Test linearize // Test linearize

4
gtsam/slam/tests/testLago.cpp
View File

@ -285,7 +285,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
Values::shared_ptr expected; Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile); 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; Key k = key_val.key;
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5)); EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
} }
@ -311,7 +311,7 @@ TEST( Lago, largeGraphNoisy ) {
Values::shared_ptr expected; Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile); 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; Key k = key_val.key;
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2)); EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
} }

4
gtsam/symbolic/SymbolicBayesNet.cpp
View File

@ -20,6 +20,8 @@
#include <gtsam/symbolic/SymbolicConditional.h> #include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <boost/foreach.hpp>
#include <fstream> #include <fstream>
namespace gtsam { namespace gtsam {
@ -43,7 +45,7 @@ namespace gtsam {
SymbolicConditional::Frontals frontals = conditional->frontals(); SymbolicConditional::Frontals frontals = conditional->frontals();
Key me = frontals.front(); Key me = frontals.front();
SymbolicConditional::Parents parents = conditional->parents(); SymbolicConditional::Parents parents = conditional->parents();
BOOST_FOREACH(Key p, parents) for(Key p: parents)
of << p << "->" << me << std::endl; of << p << "->" << me << std::endl;
} }

5
gtsam/symbolic/SymbolicFactor-inst.h
View File

@ -24,7 +24,6 @@
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <utility> #include <utility>
@ -43,12 +42,12 @@ namespace gtsam
// Gather all keys // Gather all keys
KeySet allKeys; KeySet allKeys;
BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) { for(const boost::shared_ptr<FACTOR>& factor: factors) {
allKeys.insert(factor->begin(), factor->end()); allKeys.insert(factor->begin(), factor->end());
} }
// Check keys // Check keys
BOOST_FOREACH(Key key, keys) { for(Key key: keys) {
if(allKeys.find(key) == allKeys.end()) if(allKeys.find(key) == allKeys.end())
throw std::runtime_error("Requested to eliminate a key that is not in the factors"); throw std::runtime_error("Requested to eliminate a key that is not in the factors");
} }

8
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -213,7 +213,7 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayes
if (subtree) { if (subtree) {
cliques.push_back(subtree); cliques.push_back(subtree);
// Recursive call over all child cliques // Recursive call over all child cliques
BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children) { for(SymbolicBayesTree::sharedClique& childClique: subtree->children) {
getAllCliques(childClique,cliques); getAllCliques(childClique,cliques);
} }
} }
@ -241,7 +241,7 @@ TEST( BayesTree, shortcutCheck )
SymbolicBayesTree::Cliques allCliques; SymbolicBayesTree::Cliques allCliques;
getAllCliques(rootClique,allCliques); getAllCliques(rootClique,allCliques);
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { for(SymbolicBayesTree::sharedClique& clique: allCliques) {
//clique->print("Clique#"); //clique->print("Clique#");
SymbolicBayesNet bn = clique->shortcut(rootClique); SymbolicBayesNet bn = clique->shortcut(rootClique);
//bn.print("Shortcut:\n"); //bn.print("Shortcut:\n");
@ -250,13 +250,13 @@ TEST( BayesTree, shortcutCheck )
// Check if all the cached shortcuts are cleared // Check if all the cached shortcuts are cleared
rootClique->deleteCachedShortcuts(); rootClique->deleteCachedShortcuts();
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { for(SymbolicBayesTree::sharedClique& clique: allCliques) {
bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); bool notCleared = clique->cachedSeparatorMarginal().is_initialized();
CHECK( notCleared == false); CHECK( notCleared == false);
} }
EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());
// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { // for(SymbolicBayesTree::sharedClique& clique: allCliques) {
// clique->print("Clique#"); // clique->print("Clique#");
// if(clique->cachedShortcut()){ // if(clique->cachedShortcut()){
// bn = clique->cachedShortcut().get(); // bn = clique->cachedShortcut().get();