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

Merge remote-tracking branch 'origin/develop' into feature/restoreOldImuFactor

release/4.3a0
dellaert 2016-06-05 10:41:02 -07:00
parent 9f84b46e3f 6f24ac8de9
commit 45d993dd0a
205 changed files with 868 additions and 999 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cmake/GtsamBuildTypes.cmake
View File

@ -6,7 +6,8 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE} CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE)
endif()
# Add option for using build type postfixes to allow installing multiple build modes

3
examples/CreateSFMExampleData.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
@ -41,7 +40,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
data.cameras.push_back(SfM_Camera(pose1, K));
data.cameras.push_back(SfM_Camera(pose2, K));
BOOST_FOREACH(const Point3& p, P) {
for(const Point3& p: P) {
// Create the track
SfM_Track track;

4
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
@ -66,7 +66,7 @@ int main(const int argc, const char *argv[]) {
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){

2
examples/Pose3SLAMExample_g2o.cpp
View File

@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

2
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -45,7 +45,7 @@ int main(const int argc, const char *argv[]) {
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

5
examples/RangeISAMExample_plaza2.cpp
View File

@ -43,7 +43,6 @@
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <fstream>
#include <iostream>
@ -151,7 +150,7 @@ int main (int argc, char** argv) {
// Loop over odometry
gttic_(iSAM);
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
for(const TimedOdometry& timedOdometry: odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
@ -196,7 +195,7 @@ int main (int argc, char** argv) {
}
i += 1;
//--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
} // end for
gttoc_(iSAM);
// Print timings

8
examples/SFMExampleExpressions_bal.cpp
View File

@ -78,10 +78,10 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
for(const SfM_Track& track: mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
@ -98,9 +98,9 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0;
j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
for(const SfM_Camera& camera: mydata.cameras)
initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
for(const SfM_Track& track: mydata.tracks)
initial.insert(P(j++), track.p);
/* Optimize the graph and print results */

8
examples/SFMExample_bal.cpp
View File

@ -55,8 +55,8 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
@ -72,8 +72,8 @@ int main (int argc, char* argv[]) {
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;

8
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -60,8 +60,8 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
@ -77,8 +77,8 @@ int main (int argc, char* argv[]) {
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/

13
examples/SolverComparer.cpp
View File

@ -50,6 +50,7 @@
#include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
@ -80,7 +81,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c
// the factor graph already includes a factor for the prior/equality constraint.
// double dof = graph.size() - config.size();
int graph_dim = 0;
BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
graph_dim += (int)nlf->dim();
}
double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
@ -421,9 +422,9 @@ void runIncremental()
//try {
// Marginals marginals(graph, values);
// int i=0;
// BOOST_REVERSE_FOREACH(Key key1, values.keys()) {
// for (Key key1: boost::adaptors::reverse(values.keys())) {
// int j=0;
// BOOST_REVERSE_FOREACH(Key key2, values.keys()) {
// for (Key key12: boost::adaptors::reverse(values.keys())) {
// if(i != j) {
// gttic_(jointMarginalInformation);
// std::vector<Key> keys(2);
@ -442,7 +443,7 @@ void runIncremental()
// break;
// }
// tictoc_print_();
// BOOST_FOREACH(Key key, values.keys()) {
// for(Key key: values.keys()) {
// gttic_(marginalInformation);
// Matrix info = marginals.marginalInformation(key);
// gttoc_(marginalInformation);
@ -535,7 +536,7 @@ void runCompare()
vector<Key> commonKeys;
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
double maxDiff = 0.0;
BOOST_FOREACH(Key j, commonKeys)
for(Key j: commonKeys)
maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl;
}
@ -549,7 +550,7 @@ void runPerturb()
// Perturb values
VectorValues noise;
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
for(const Values::KeyValuePair& key_val: initial)
{
Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i)

13
examples/TimeTBB.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Matrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <map>
using namespace std;
@ -76,7 +75,7 @@ map<int, double> testWithoutMemoryAllocation()
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
@ -129,7 +128,7 @@ map<int, double> testWithMemoryAllocation()
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
@ -150,7 +149,7 @@ int main(int argc, char* argv[])
const vector<int> numThreads = list_of(1)(4)(8);
Results results;
BOOST_FOREACH(size_t n, numThreads)
for(size_t n: numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init((int)n);
@ -160,19 +159,19 @@ int main(int argc, char* argv[])
}
cout << "Summary of results:" << endl;
BOOST_FOREACH(const Results::value_type& threads_result, results)
for(const Results::value_type& threads_result: results)
{
const int threads = threads_result.first;
const ResultWithThreads& result = threads_result.second;
if(threads != 1)
{
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation)
for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
}
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation)
for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;

9
gtsam/base/DSFVector.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/base/DSFVector.h>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <algorithm>
using namespace std;
@ -79,7 +78,7 @@ DSFVector::DSFVector(const boost::shared_ptr<V>& v_in,
/* ************************************************************************* */
bool DSFVector::isSingleton(const size_t& label) const {
bool result = false;
BOOST_FOREACH(size_t key,keys_) {
for(size_t key: keys_) {
if (find(key) == label) {
if (!result) // find the first occurrence
result = true;
@ -93,7 +92,7 @@ bool DSFVector::isSingleton(const size_t& label) const {
/* ************************************************************************* */
std::set<size_t> DSFVector::set(const size_t& label) const {
std::set < size_t > set;
BOOST_FOREACH(size_t key,keys_)
for(size_t key: keys_)
if (find(key) == label)
set.insert(key);
return set;
@ -102,7 +101,7 @@ std::set<size_t> DSFVector::set(const size_t& label) const {
/* ************************************************************************* */
std::map<size_t, std::set<size_t> > DSFVector::sets() const {
std::map<size_t, std::set<size_t> > sets;
BOOST_FOREACH(size_t key,keys_)
for(size_t key: keys_)
sets[find(key)].insert(key);
return sets;
}
@ -110,7 +109,7 @@ std::map<size_t, std::set<size_t> > DSFVector::sets() const {
/* ************************************************************************* */
std::map<size_t, std::vector<size_t> > DSFVector::arrays() const {
std::map<size_t, std::vector<size_t> > arrays;
BOOST_FOREACH(size_t key,keys_)
for(size_t key: keys_)
arrays[find(key)].push_back(key);
return arrays;
}

13
gtsam/base/Matrix.cpp
View File

@ -23,7 +23,6 @@
#include <Eigen/SVD>
#include <Eigen/LU>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tokenizer.hpp>
@ -190,7 +189,7 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
// Copy coefficients to matrix
destinationMatrix.resize(height, width);
int row = 0;
BOOST_FOREACH(const vector<double>& rowVec, coeffs) {
for(const vector<double>& rowVec: coeffs) {
destinationMatrix.row(row) = Eigen::Map<const Eigen::RowVectorXd>(&rowVec[0], width);
++ row;
}
@ -419,7 +418,7 @@ Matrix stack(size_t nrMatrices, ...)
Matrix stack(const std::vector<Matrix>& blocks) {
if (blocks.size() == 1) return blocks.at(0);
DenseIndex nrows = 0, ncols = blocks.at(0).cols();
BOOST_FOREACH(const Matrix& mat, blocks) {
for(const Matrix& mat: blocks) {
nrows += mat.rows();
if (ncols != mat.cols())
throw invalid_argument("Matrix::stack(): column size mismatch!");
@ -427,7 +426,7 @@ Matrix stack(const std::vector<Matrix>& blocks) {
Matrix result(nrows, ncols);
DenseIndex cur_row = 0;
BOOST_FOREACH(const Matrix& mat, blocks) {
for(const Matrix& mat: blocks) {
result.middleRows(cur_row, mat.rows()) = mat;
cur_row += mat.rows();
}
@ -441,7 +440,7 @@ Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
size_t dimA1 = m;
size_t dimA2 = n*matrices.size();
if (!m && !n) {
BOOST_FOREACH(const Matrix* M, matrices) {
for(const Matrix* M: matrices) {
dimA1 = M->rows(); // TODO: should check if all the same !
dimA2 += M->cols();
}
@ -450,7 +449,7 @@ Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
// stl::copy version
Matrix A(dimA1, dimA2);
size_t hindex = 0;
BOOST_FOREACH(const Matrix* M, matrices) {
for(const Matrix* M: matrices) {
size_t row_len = M->cols();
A.block(0, hindex, dimA1, row_len) = *M;
hindex += row_len;
@ -611,7 +610,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
boost::tokenizer<boost::char_separator<char> > tok(matrixStr, boost::char_separator<char>("\n"));
DenseIndex row = 0;
BOOST_FOREACH(const std::string& line, tok)
for(const std::string& line: tok)
{
assert(row < effectiveRows);
if(row > 0)

31
gtsam/base/TestableAssertions.h
View File

@ -20,7 +20,6 @@
#include <gtsam/base/Testable.h>
#include <gtsam/global_includes.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <map>
#include <iostream>
@ -91,7 +90,7 @@ bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual,
match = false;
if(match) {
size_t i = 0;
BOOST_FOREACH(const V& a, expected) {
for(const V& a: expected) {
if (!assert_equal(a, actual[i++], tol)) {
match = false;
break;
@ -100,9 +99,9 @@ bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual,
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const V& a, expected) { std::cout << a << " "; }
for(const V& a: expected) { std::cout << a << " "; }
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const V& a, actual) { std::cout << a << " "; }
for(const V& a: actual) { std::cout << a << " "; }
std::cout << std::endl;
return false;
}
@ -133,12 +132,12 @@ bool assert_container_equal(const std::map<V1,V2>& expected, const std::map<V1,V
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const typename Map::value_type& a, expected) {
for(const typename Map::value_type& a: expected) {
a.first.print("key");
a.second.print(" value");
}
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const typename Map::value_type& a, actual) {
for(const typename Map::value_type& a: actual) {
a.first.print("key");
a.second.print(" value");
}
@ -171,12 +170,12 @@ bool assert_container_equal(const std::map<size_t,V2>& expected, const std::map<
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const typename Map::value_type& a, expected) {
for(const typename Map::value_type& a: expected) {
std::cout << "Key: " << a.first << std::endl;
a.second.print(" value");
}
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const typename Map::value_type& a, actual) {
for(const typename Map::value_type& a: actual) {
std::cout << "Key: " << a.first << std::endl;
a.second.print(" value");
}
@ -210,12 +209,12 @@ bool assert_container_equal(const std::vector<std::pair<V1,V2> >& expected,
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const typename VectorPair::value_type& a, expected) {
for(const typename VectorPair::value_type& a: expected) {
a.first.print( " first ");
a.second.print(" second");
}
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const typename VectorPair::value_type& a, actual) {
for(const typename VectorPair::value_type& a: actual) {
a.first.print( " first ");
a.second.print(" second");
}
@ -247,9 +246,9 @@ bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const typename V::value_type& a, expected) { a.print(" "); }
for(const typename V::value_type& a: expected) { a.print(" "); }
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const typename V::value_type& a, actual) { a.print(" "); }
for(const typename V::value_type& a: actual) { a.print(" "); }
std::cout << std::endl;
return false;
}
@ -279,12 +278,12 @@ bool assert_container_equality(const std::map<size_t,V2>& expected, const std::m
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const typename Map::value_type& a, expected) {
for(const typename Map::value_type& a: expected) {
std::cout << "Key: " << a.first << std::endl;
std::cout << "Value: " << a.second << std::endl;
}
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const typename Map::value_type& a, actual) {
for(const typename Map::value_type& a: actual) {
std::cout << "Key: " << a.first << std::endl;
std::cout << "Value: " << a.second << std::endl;
}
@ -316,9 +315,9 @@ bool assert_container_equality(const V& expected, const V& actual) {
}
if(!match) {
std::cout << "expected: " << std::endl;
BOOST_FOREACH(const typename V::value_type& a, expected) { std::cout << a << " "; }
for(const typename V::value_type& a: expected) { std::cout << a << " "; }
std::cout << "\nactual: " << std::endl;
BOOST_FOREACH(const typename V::value_type& a, actual) { std::cout << a << " "; }
for(const typename V::value_type& a: actual) { std::cout << a << " "; }
std::cout << std::endl;
return false;
}

5
gtsam/base/Vector.cpp
View File

@ -17,7 +17,6 @@
*/
#include <gtsam/base/Vector.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <stdexcept>
#include <cstdarg>
@ -264,12 +263,12 @@ weightedPseudoinverse(const Vector& a, const Vector& weights) {
Vector concatVectors(const std::list<Vector>& vs)
{
size_t dim = 0;
BOOST_FOREACH(Vector v, vs)
for(Vector v: vs)
dim += v.size();
Vector A(dim);
size_t index = 0;
BOOST_FOREACH(Vector v, vs) {
for(Vector v: vs) {
for(int d = 0; d < v.size(); d++)
A(d+index) = v(d);
index += v.size();

5
gtsam/base/tests/testDSFVector.cpp
View File

@ -20,7 +20,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
@ -70,7 +69,7 @@ TEST(DSFBase, mergePairwiseMatches) {
// Merge matches
DSFBase dsf(7); // We allow for keys 0..6
BOOST_FOREACH(const Match& m, matches)
for(const Match& m: matches)
dsf.merge(m.first,m.second);
// Each point is now associated with a set, represented by one of its members
@ -206,7 +205,7 @@ TEST(DSFVector, mergePairwiseMatches) {
// Merge matches
DSFVector dsf(keys);
BOOST_FOREACH(const Match& m, matches)
for(const Match& m: matches)
dsf.merge(m.first,m.second);
// Check that we have two connected components, 1,2,3 and 4,5,6

1
gtsam/base/tests/testGroup.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Testable.h>
#include <Eigen/Core>
#include <iostream>
#include <boost/foreach.hpp>
namespace gtsam {

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

@ -21,7 +21,6 @@
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <iostream>
#include <sstream>
@ -907,10 +906,6 @@ TEST(Matrix, weighted_elimination )
Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2).finished();
Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
Vector r;
double di, sigma;
size_t i;
// perform elimination
Matrix A1 = A;
Vector b1 = b;
@ -918,8 +913,11 @@ TEST(Matrix, weighted_elimination )
weighted_eliminate(A1, b1, sigmas);
// unpack and verify
i = 0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution){
size_t i = 0;
for (const auto& tuple : solution) {
Vector r;
double di, sigma;
boost::tie(r, di, sigma) = tuple;
EXPECT(assert_equal(r, expectedR.row(i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma

11
gtsam/base/timing.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/base/timing.h>
#include <boost/algorithm/string/replace.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <cmath>
@ -66,7 +65,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) :
size_t TimingOutline::time() const {
size_t time = 0;
bool hasChildren = false;
BOOST_FOREACH(const ChildMap::value_type& child, children_) {
for(const ChildMap::value_type& child: children_) {
time += child.second->time();
hasChildren = true;
}
@ -86,11 +85,11 @@ void TimingOutline::print(const std::string& outline) const {
// Order children
typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildOrder;
ChildOrder childOrder;
BOOST_FOREACH(const ChildMap::value_type& child, children_) {
for(const ChildMap::value_type& child: children_) {
childOrder[child.second->myOrder_] = child.second;
}
// Print children
BOOST_FOREACH(const ChildOrder::value_type order_child, childOrder) {
for(const ChildOrder::value_type order_child: childOrder) {
std::string childOutline(outline);
childOutline += "| ";
order_child.second->print(childOutline);
@ -130,7 +129,7 @@ void TimingOutline::print2(const std::string& outline,
std::cout << std::endl;
}
BOOST_FOREACH(const ChildMap::value_type& child, children_) {
for(const ChildMap::value_type& child: children_) {
std::string childOutline(outline);
if (n_ == 0) {
child.second->print2(childOutline, childTotal);
@ -210,7 +209,7 @@ void TimingOutline::finishedIteration() {
if (tMin_ == 0 || tIt_ < tMin_)
tMin_ = tIt_;
tIt_ = 0;
BOOST_FOREACH(ChildMap::value_type& child, children_) {
for(ChildMap::value_type& child: children_) {
child.second->finishedIteration();
}
}

5
gtsam/base/treeTraversal-inst.h
View File

@ -29,7 +29,6 @@
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
namespace gtsam {
@ -91,7 +90,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
// Add roots to stack (insert such that they are visited and processed in order
{
typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& root, forest.roots())
for(const sharedNode& root: forest.roots())
stack.insert(insertLocation, TraversalNode(root, rootData));
}
@ -112,7 +111,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
node.dataPointer = dataList.insert(dataList.end(),
visitorPre(node.treeNode, node.parentData));
typename Stack::iterator insertLocation = stack.begin();
BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
for(const sharedNode& child: node.treeNode->children)
stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
node.expanded = true;
}

7
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -20,7 +20,6 @@
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#ifdef GTSAM_USE_TBB
# include <tbb/tbb.h>
@ -101,7 +100,7 @@ namespace gtsam {
tbb::task* firstChild = 0;
tbb::task_list childTasks;
BOOST_FOREACH(const boost::shared_ptr<NODE>& child, treeNode->children)
for(const boost::shared_ptr<NODE>& child: treeNode->children)
{
// Process child in a subtask. Important: Run visitorPre before calling
// allocate_child so that if visitorPre throws an exception, we will not have
@ -143,7 +142,7 @@ namespace gtsam {
void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
{
BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
for(const boost::shared_ptr<NODE>& child: node->children)
{
DATA childData = visitorPre(child, myData);
processNodeRecursively(child, childData);
@ -174,7 +173,7 @@ namespace gtsam {
typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
// Create data and tasks for our children
tbb::task_list tasks;
BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots)
for(const boost::shared_ptr<NODE>& root: roots)
{
boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
tasks.push_back(*new(allocate_child())

2
gtsam/base/treeTraversal/statistics.h
View File

@ -63,7 +63,7 @@ namespace gtsam {
{
int largestProblemSize = 0;
int secondLargestProblemSize = 0;
BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
for(const boost::shared_ptr<NODE>& child: node->children)
{
if (child->problemSize() > largestProblemSize)
{

5
gtsam/discrete/Assignment.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <boost/foreach.hpp>
#include <iostream>
#include <vector>
#include <map>
@ -36,7 +35,7 @@ namespace gtsam {
public:
void print(const std::string& s = "Assignment: ") const {
std::cout << s << ": ";
BOOST_FOREACH(const typename Assignment::value_type& keyValue, *this)
for(const typename Assignment::value_type& keyValue: *this)
std::cout << "(" << keyValue.first << ", " << keyValue.second << ")";
std::cout << std::endl;
}
@ -65,7 +64,7 @@ namespace gtsam {
std::vector<Assignment<L> > allPossValues;
Assignment<L> values;
typedef std::pair<L, size_t> DiscreteKey;
BOOST_FOREACH(const DiscreteKey& key, keys)
for(const DiscreteKey& key: keys)
values[key.first] = 0; //Initialize from 0
while (1) {
allPossValues.push_back(values);

11
gtsam/discrete/DecisionTree-inl.h
View File

@ -24,7 +24,6 @@
#include <boost/format.hpp>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/vector.hpp>
using boost::assign::operator+=;
@ -310,7 +309,7 @@ namespace gtsam {
label_(label), allSame_(true) {
branches_.reserve(f.branches_.size()); // reserve space
BOOST_FOREACH (const NodePtr& branch, f.branches_)
for (const NodePtr& branch: f.branches_)
push_back(branch->apply(op));
}
@ -332,7 +331,7 @@ namespace gtsam {
// If second argument of binary op is Leaf node, recurse on branches
NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
BOOST_FOREACH(NodePtr branch, branches_)
for(NodePtr branch: branches_)
h->push_back(fL.apply_f_op_g(*branch, op));
return Unique(h);
}
@ -347,7 +346,7 @@ namespace gtsam {
template<typename OP>
NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const {
boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
BOOST_FOREACH(const NodePtr& branch, branches_)
for(const NodePtr& branch: branches_)
h->push_back(branch->apply_f_op_g(gL, op));
return Unique(h);
}
@ -359,7 +358,7 @@ namespace gtsam {
// second case, not label of interest, just recurse
boost::shared_ptr<Choice> r(new Choice(label_, branches_.size()));
BOOST_FOREACH(const NodePtr& branch, branches_)
for(const NodePtr& branch: branches_)
r->push_back(branch->choose(label, index));
return Unique(r);
}
@ -593,7 +592,7 @@ namespace gtsam {
// put together via Shannon expansion otherwise not sorted.
std::vector<LY> functions;
BOOST_FOREACH(const MXNodePtr& branch, choice->branches()) {
for(const MXNodePtr& branch: choice->branches()) {
LY converted(convert<M, X>(branch, map, op));
functions += converted;
}

7
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/base/FastSet.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
using namespace std;
@ -66,11 +65,11 @@ namespace gtsam {
ADT::Binary op) const {
map<Key,size_t> cs; // new cardinalities
// make unique key-cardinality map
BOOST_FOREACH(Key j, keys()) cs[j] = cardinality(j);
BOOST_FOREACH(Key j, f.keys()) cs[j] = f.cardinality(j);
for(Key j: keys()) cs[j] = cardinality(j);
for(Key j: f.keys()) cs[j] = f.cardinality(j);
// Convert map into keys
DiscreteKeys keys;
BOOST_FOREACH(const DiscreteKey& key, cs)
for(const DiscreteKey& key: cs)
keys.push_back(key);
// apply operand
ADT result = ADT::apply(f, op);

1
gtsam/discrete/DecisionTreeFactor.h
View File

@ -22,7 +22,6 @@
#include <gtsam/discrete/Potentials.h>
#include <gtsam/inference/Ordering.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>

8
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -19,7 +19,9 @@
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <boost/make_shared.hpp>
#include <boost/range/adaptor/reversed.hpp>
namespace gtsam {
@ -46,7 +48,7 @@ namespace gtsam {
double DiscreteBayesNet::evaluate(const DiscreteConditional::Values & values) const {
// evaluate all conditionals and multiply
double result = 1.0;
BOOST_FOREACH(DiscreteConditional::shared_ptr conditional, *this)
for(DiscreteConditional::shared_ptr conditional: *this)
result *= (*conditional)(values);
return result;
}
@ -55,7 +57,7 @@ namespace gtsam {
DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const {
// solve each node in turn in topological sort order (parents first)
DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
BOOST_REVERSE_FOREACH (DiscreteConditional::shared_ptr conditional, *this)
for (auto conditional: boost::adaptors::reverse(*this))
conditional->solveInPlace(*result);
return result;
}
@ -64,7 +66,7 @@ namespace gtsam {
DiscreteFactor::sharedValues DiscreteBayesNet::sample() const {
// sample each node in turn in topological sort order (parents first)
DiscreteFactor::sharedValues result(new DiscreteFactor::Values());
BOOST_REVERSE_FOREACH(DiscreteConditional::shared_ptr conditional, *this)
for (auto conditional: boost::adaptors::reverse(*this))
conditional->sampleInPlace(*result);
return result;
}

8
gtsam/discrete/DiscreteConditional.cpp
View File

@ -87,7 +87,7 @@ bool DiscreteConditional::equals(const DiscreteFactor& other,
Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
ADT pFS(*this);
Key j; size_t value;
BOOST_FOREACH(Key key, parents())
for(Key key: parents())
try {
j = (key);
value = parentsValues.at(j);
@ -111,7 +111,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
double maxP = 0;
DiscreteKeys keys;
BOOST_FOREACH(Key idx, frontals()) {
for(Key idx: frontals()) {
DiscreteKey dk(idx, cardinality(idx));
keys & dk;
}
@ -119,7 +119,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
vector<Values> allPosbValues = cartesianProduct(keys);
// Find the MPE
BOOST_FOREACH(Values& frontalVals, allPosbValues) {
for(Values& frontalVals: allPosbValues) {
double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues)
// Update MPE solution if better
if (pValueS > maxP) {
@ -129,7 +129,7 @@ void DiscreteConditional::solveInPlace(Values& values) const {
}
//set values (inPlace) to mpe
BOOST_FOREACH(Key j, frontals()) {
for(Key j: frontals()) {
values[j] = mpe[j];
}
}

1
gtsam/discrete/DiscreteFactor.cpp
View File

@ -18,7 +18,6 @@
*/
#include <gtsam/discrete/DiscreteFactor.h>
#include <boost/foreach.hpp>
using namespace std;

12
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -41,7 +41,7 @@ namespace gtsam {
/* ************************************************************************* */
KeySet DiscreteFactorGraph::keys() const {
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
for(const sharedFactor& factor: *this)
if (factor) keys.insert(factor->begin(), factor->end());
return keys;
}
@ -49,7 +49,7 @@ namespace gtsam {
/* ************************************************************************* */
DecisionTreeFactor DiscreteFactorGraph::product() const {
DecisionTreeFactor result;
BOOST_FOREACH(const sharedFactor& factor, *this)
for(const sharedFactor& factor: *this)
if (factor) result = (*factor) * result;
return result;
}
@ -58,7 +58,7 @@ namespace gtsam {
double DiscreteFactorGraph::operator()(
const DiscreteFactor::Values &values) const {
double product = 1.0;
BOOST_FOREACH( const sharedFactor& factor, factors_ )
for( const sharedFactor& factor: factors_ )
product *= (*factor)(values);
return product;
}
@ -78,7 +78,7 @@ namespace gtsam {
// /* ************************************************************************* */
// void DiscreteFactorGraph::permuteWithInverse(
// const Permutation& inversePermutation) {
// BOOST_FOREACH(const sharedFactor& factor, factors_) {
// for(const sharedFactor& factor: factors_) {
// if(factor)
// factor->permuteWithInverse(inversePermutation);
// }
@ -87,7 +87,7 @@ namespace gtsam {
// /* ************************************************************************* */
// void DiscreteFactorGraph::reduceWithInverse(
// const internal::Reduction& inverseReduction) {
// BOOST_FOREACH(const sharedFactor& factor, factors_) {
// for(const sharedFactor& factor: factors_) {
// if(factor)
// factor->reduceWithInverse(inverseReduction);
// }
@ -107,7 +107,7 @@ namespace gtsam {
// PRODUCT: multiply all factors
gttic(product);
DecisionTreeFactor product;
BOOST_FOREACH(const DiscreteFactor::shared_ptr& factor, factors)
for(const DiscreteFactor::shared_ptr& factor: factors)
product = (*factor) * product;
gttoc(product);

5
gtsam/discrete/DiscreteKey.cpp
View File

@ -18,7 +18,6 @@
#include <iostream>
#include <boost/format.hpp> // for key names
#include <boost/foreach.hpp> // FOREACH
#include "DiscreteKey.h"
namespace gtsam {
@ -34,7 +33,7 @@ namespace gtsam {
vector<Key> DiscreteKeys::indices() const {
vector < Key > js;
BOOST_FOREACH(const DiscreteKey& key, *this)
for(const DiscreteKey& key: *this)
js.push_back(key.first);
return js;
}
@ -42,7 +41,7 @@ namespace gtsam {
map<Key,size_t> DiscreteKeys::cardinalities() const {
map<Key,size_t> cs;
cs.insert(begin(),end());
// BOOST_FOREACH(const DiscreteKey& key, *this)
// for(const DiscreteKey& key: *this)
// cs.insert(key);
return cs;
}

6
gtsam/discrete/Potentials.cpp
View File

@ -54,7 +54,7 @@ namespace gtsam {
void Potentials::print(const string& s,
const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: ";
BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
for(const DiscreteKey& key: cardinalities_)
cout << formatter(key.first) << "=" << formatter(key.second) << " ";
cout << endl;
ADT::print(" ");
@ -68,11 +68,11 @@ namespace gtsam {
// map<Key, Key> ordering;
//
// // Get the original keys from cardinalities_
// BOOST_FOREACH(const DiscreteKey& key, cardinalities_)
// for(const DiscreteKey& key: cardinalities_)
// keys & key;
//
// // Perform Permutation
// BOOST_FOREACH(DiscreteKey& key, keys) {
// for(DiscreteKey& key: keys) {
// ordering[key.first] = remapping[key.first];
// key.first = ordering[key.first];
// }

13
gtsam/discrete/Signature.cpp
View File

@ -17,7 +17,6 @@
*/
#include <sstream>
#include <boost/foreach.hpp>
#include "Signature.h"
@ -125,7 +124,7 @@ namespace gtsam {
DiscreteKeys Signature::discreteKeysParentsFirst() const {
DiscreteKeys keys;
BOOST_FOREACH(const DiscreteKey& key, parents_)
for(const DiscreteKey& key: parents_)
keys.push_back(key);
keys.push_back(key_);
return keys;
@ -134,7 +133,7 @@ namespace gtsam {
vector<Key> Signature::indices() const {
vector<Key> js;
js.push_back(key_.first);
BOOST_FOREACH(const DiscreteKey& key, parents_)
for(const DiscreteKey& key: parents_)
js.push_back(key.first);
return js;
}
@ -142,8 +141,8 @@ namespace gtsam {
vector<double> Signature::cpt() const {
vector<double> cpt;
if (table_) {
BOOST_FOREACH(const Row& row, *table_)
BOOST_FOREACH(const double& x, row)
for(const Row& row: *table_)
for(const double& x: row)
cpt.push_back(x);
}
return cpt;
@ -171,7 +170,7 @@ namespace gtsam {
// qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); // using full grammar
parser::parse_table(spec, table);
if (success) {
BOOST_FOREACH(Row& row, table)
for(Row& row: table)
normalize(row);
table_.reset(table);
}
@ -180,7 +179,7 @@ namespace gtsam {
Signature& Signature::operator=(const Table& t) {
Table table = t;
BOOST_FOREACH(Row& row, table)
for(Row& row: table)
normalize(row);
table_.reset(table);
return *this;

5
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -25,7 +25,6 @@
#define DISABLE_TIMING
#include <boost/timer.hpp>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
@ -66,7 +65,7 @@ void dot(const T&f, const string& filename) {
typename DecisionTree<L, double>::Node::Ptr DecisionTree<L, double>::Choice::apply_fC_op_gL(
Cache& cache, const Leaf& gL, Mul op) const {
Ptr h(new Choice(label(), cardinality()));
BOOST_FOREACH(const NodePtr& branch, branches_)
for(const NodePtr& branch: branches_)
h->push_back(branch->apply_f_op_g(cache, gL, op));
return Unique(cache, h);
}
@ -401,7 +400,7 @@ TEST(ADT, constructor)
DiscreteKey z0(0,5), z1(1,4), z2(2,3), z3(3,2);
vector<double> table(5 * 4 * 3 * 2);
double x = 0;
BOOST_FOREACH(double& t, table)
for(double& t: table)
t = x++;
ADT f3(z0 & z1 & z2 & z3, table);
Assignment<Key> assignment;

4
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -63,7 +63,7 @@
//// double evaluate(const DiscreteConditional::Values & values) {
//// double result = (*(this->conditional_))(values);
//// // evaluate all children and multiply into result
//// BOOST_FOREACH(boost::shared_ptr<Clique> c, children_)
//// for(boost::shared_ptr<Clique> c: children_)
//// result *= c->evaluate(values);
//// return result;
//// }
@ -213,7 +213,7 @@
//
// // calculate all shortcuts to root
// DiscreteBayesTree::Nodes cliques = bayesTree.nodes();
// BOOST_FOREACH(Clique::shared_ptr c, cliques) {
// for(Clique::shared_ptr c: cliques) {
// DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete);
// if (debug) {
// c->printSignature();

4
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -321,7 +321,7 @@ struct Graph2: public std::list<Factor2> {
/** Add a factor graph*/
// void operator +=(const Graph2& graph) {
// BOOST_FOREACH(const Factor2& f, graph)
// for(const Factor2& f: graph)
// push_back(f);
// }
friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);
@ -334,7 +334,7 @@ friend std::ostream& operator <<(std::ostream &os, const Graph2& graph);
// return graph;
//}
std::ostream& operator <<(std::ostream &os, const Graph2& graph) {
BOOST_FOREACH(const Factor2& f, graph)
for(const Factor2& f: graph)
os << f << endl;
return os;
}

1
gtsam/geometry/PinholeSet.h
View File

@ -20,7 +20,6 @@
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/triangulation.h>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
namespace gtsam {

1
gtsam/geometry/Point2.cpp
View File

@ -16,7 +16,6 @@
*/
#include <gtsam/geometry/Point2.h>
#include <boost/foreach.hpp>
#include <cmath>
using namespace std;

6
gtsam/geometry/Pose2.cpp
View File

@ -19,8 +19,6 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/concepts.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <iostream>
#include <iomanip>
@ -314,7 +312,7 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
// calculate centroids
Point2 cp,cq;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
for(const Point2Pair& pair: pairs) {
cp += pair.first;
cq += pair.second;
}
@ -323,7 +321,7 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
// calculate cos and sin
double c=0,s=0;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
for(const Point2Pair& pair: pairs) {
Point2 dq = pair.first - cp;
Point2 dp = pair.second - cq;
c += dp.x() * dq.x() + dp.y() * dq.y();

3
gtsam/geometry/Pose3.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/concepts.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <cmath>
@ -424,7 +423,7 @@ boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs)
boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
vector<Point3Pair> abPointPairs;
BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) {
for (const Point3Pair& baPair: baPointPairs) {
abPointPairs.push_back(make_pair(baPair.second, baPair.first));
}
return Pose3::Align(abPointPairs);

1
gtsam/geometry/tests/testPose2.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/base/lieProxies.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <boost/assign/std/vector.hpp> // for operator +=
#include <cmath>

3
gtsam/geometry/tests/testUnit3.cpp
View File

@ -33,7 +33,6 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/assign/std/vector.hpp>
#include <cmath>
@ -56,7 +55,7 @@ TEST(Unit3, point3) {
ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
/ sqrt(2.0);
Matrix actualH, expectedH;
BOOST_FOREACH(Point3 p,ps) {
for(Point3 p: ps) {
Unit3 s(p);
expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
EXPECT(assert_equal(p, s.point3(actualH), 1e-8));

10
gtsam/geometry/triangulation.h
View File

@ -237,7 +237,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// construct projection matrices from poses & calibration
std::vector<Matrix34> projection_matrices;
CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
BOOST_FOREACH(const Pose3& pose, poses)
for(const Pose3& pose: poses)
projection_matrices.push_back(createP(pose));
// Triangulate linearly
@ -250,7 +250,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {
for(const Pose3& pose: poses) {
const Point3& p_local = pose.transform_to(point);
if (p_local.z() <= 0)
throw(TriangulationCheiralityException());
@ -286,7 +286,7 @@ Point3 triangulatePoint3(
// construct projection matrices from poses & calibration
std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const CAMERA& camera, cameras)
for(const CAMERA& camera: cameras)
projection_matrices.push_back(
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
camera.pose()));
@ -298,7 +298,7 @@ Point3 triangulatePoint3(
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras
BOOST_FOREACH(const CAMERA& camera, cameras) {
for(const CAMERA& camera: cameras) {
const Point3& p_local = camera.pose().transform_to(point);
if (p_local.z() <= 0)
throw(TriangulationCheiralityException());
@ -454,7 +454,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
// Check landmark distance and re-projection errors to avoid outliers
size_t i = 0;
double totalReprojError = 0.0;
BOOST_FOREACH(const CAMERA& camera, cameras) {
for(const CAMERA& camera: cameras) {
const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0
&& distance(pose.translation(), point)

6
gtsam/inference/BayesNet-inst.h
View File

@ -21,7 +21,7 @@
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/BayesNet.h>
#include <boost/foreach.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <fstream>
namespace gtsam {
@ -40,11 +40,11 @@ namespace gtsam {
std::ofstream of(s.c_str());
of << "digraph G{\n";
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
for (auto conditional: boost::adaptors::reverse(*this)) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
BOOST_FOREACH(Key p, parents)
for(Key p: parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}

43
gtsam/inference/BayesTree-inst.h
View File

@ -26,7 +26,6 @@
#include <gtsam/base/timing.h>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp>
#include <fstream>
@ -38,7 +37,7 @@ namespace gtsam {
template<class CLIQUE>
BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
BayesTreeCliqueData data;
BOOST_FOREACH(const sharedClique& root, roots_)
for(const sharedClique& root: roots_)
getCliqueData(data, root);
return data;
}
@ -48,7 +47,7 @@ namespace gtsam {
void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
data.separatorSizes.push_back(clique->conditional()->nrParents());
BOOST_FOREACH(sharedClique c, clique->children) {
for(sharedClique c: clique->children) {
getCliqueData(data, c);
}
}
@ -57,7 +56,7 @@ namespace gtsam {
template<class CLIQUE>
size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
size_t count = 0;
BOOST_FOREACH(const sharedClique& root, roots_)
for(const sharedClique& root: roots_)
count += root->numCachedSeparatorMarginals();
return count;
}
@ -68,7 +67,7 @@ namespace gtsam {
if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
BOOST_FOREACH(const sharedClique& root, roots_)
for(const sharedClique& root: roots_)
saveGraph(of, root, keyFormatter);
of<<"}";
of.close();
@ -84,7 +83,7 @@ namespace gtsam {
std::string parent = out.str();
parent += "[label=\"";
BOOST_FOREACH(Key index, clique->conditional_->frontals()) {
for(Key index: clique->conditional_->frontals()) {
if(!first) parent += ","; first = false;
parent += indexFormatter(index);
}
@ -95,7 +94,7 @@ namespace gtsam {
}
first = true;
BOOST_FOREACH(Key sep, clique->conditional_->parents()) {
for(Key sep: clique->conditional_->parents()) {
if(!first) parent += ","; first = false;
parent += indexFormatter(sep);
}
@ -103,7 +102,7 @@ namespace gtsam {
s << parent;
parentnum = num;
BOOST_FOREACH(sharedClique c, clique->children) {
for(sharedClique c: clique->children) {
num++;
saveGraph(s, c, indexFormatter, parentnum);
}
@ -113,7 +112,7 @@ namespace gtsam {
template<class CLIQUE>
size_t BayesTree<CLIQUE>::size() const {
size_t size = 0;
BOOST_FOREACH(const sharedClique& clique, roots_)
for(const sharedClique& clique: roots_)
size += clique->treeSize();
return size;
}
@ -121,7 +120,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
BOOST_FOREACH(Key j, clique->conditional()->frontals())
for(Key j: clique->conditional()->frontals())
nodes_[j] = clique;
if (parent_clique != NULL) {
clique->parent_ = parent_clique;
@ -189,7 +188,7 @@ namespace gtsam {
this->clear();
boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
BOOST_FOREACH(const sharedClique& root, rootContainer->children) {
for(const sharedClique& root: rootContainer->children) {
root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique
insertRoot(root);
}
@ -234,13 +233,13 @@ namespace gtsam {
template<class CLIQUE>
void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
// Add each frontal variable of this root node
BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) {
for(const Key& j: subtree->conditional()->frontals()) {
bool inserted = nodes_.insert(std::make_pair(j, subtree)).second;
assert(inserted); (void)inserted;
}
// Fill index for each child
typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children) {
for(const sharedClique& child: subtree->children) {
fillNodesIndex(child); }
}
@ -345,7 +344,7 @@ namespace gtsam {
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
FastVector<Key> C1_minus_B; {
KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) {
for(const Key j: *B->conditional()) {
C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
}
@ -357,7 +356,7 @@ namespace gtsam {
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
FastVector<Key> C2_minus_B; {
KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) {
for(const Key j: *B->conditional()) {
C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
}
@ -403,7 +402,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
void BayesTree<CLIQUE>::deleteCachedShortcuts() {
BOOST_FOREACH(const sharedClique& root, roots_) {
for(const sharedClique& root: roots_) {
root->deleteCachedShortcuts();
}
}
@ -424,10 +423,10 @@ namespace gtsam {
}
// orphan my children
BOOST_FOREACH(sharedClique child, clique->children)
for(sharedClique child: clique->children)
child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
for(Key j: clique->conditional()->frontals()) {
nodes_.unsafe_erase(j);
}
}
@ -462,7 +461,7 @@ namespace gtsam {
void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans)
{
// process each key of the new factor
BOOST_FOREACH(const Key& j, keys)
for(const Key& j: keys)
{
// get the clique
// TODO: Nodes will be searched again in removeClique
@ -475,7 +474,7 @@ namespace gtsam {
// Delete cachedShortcuts for each orphan subtree
//TODO: Consider Improving
BOOST_FOREACH(sharedClique& orphan, orphans)
for(sharedClique& orphan: orphans)
orphan->deleteCachedShortcuts();
}
@ -499,14 +498,14 @@ namespace gtsam {
for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
{
// Add children
BOOST_FOREACH(const sharedClique& child, (*clique)->children) {
for(const sharedClique& child: (*clique)->children) {
cliques.push_back(child); }
// Delete cached shortcuts
(*clique)->deleteCachedShortcutsNonRecursive();
// Remove this node from the nodes index
BOOST_FOREACH(Key j, (*clique)->conditional()->frontals()) {
for(Key j: (*clique)->conditional()->frontals()) {
nodes_.unsafe_erase(j); }
// Erase the parent and children pointers

5
gtsam/inference/BayesTree.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/inference/BayesTree.h>
#include <boost/foreach.hpp>
#include <iostream>
namespace gtsam {
@ -41,7 +40,7 @@ BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
double sum = 0.0;
size_t max = 0;
BOOST_FOREACH(size_t s, conditionalSizes) {
for(size_t s: conditionalSizes) {
sum += (double)s;
if(s > max) max = s;
}
@ -50,7 +49,7 @@ BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
sum = 0.0;
max = 1;
BOOST_FOREACH(size_t s, separatorSizes) {
for(size_t s: separatorSizes) {
sum += (double)s;
if(s > max) max = s;
}

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

@ -18,7 +18,6 @@
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
namespace gtsam {
@ -83,7 +82,7 @@ namespace gtsam {
template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children)
for(const derived_ptr& child: children)
size += child->treeSize();
return size;
}
@ -96,7 +95,7 @@ namespace gtsam {
return 0;
size_t subtree_count = 1;
BOOST_FOREACH(const derived_ptr& child, children)
for(const derived_ptr& child: children)
subtree_count += child->numCachedSeparatorMarginals();
return subtree_count;
@ -204,7 +203,7 @@ namespace gtsam {
// root are also generated. So, if this clique's cached shortcut is set,
// recursively call over all child cliques. Otherwise, it is unnecessary.
if (cachedSeparatorMarginal_) {
BOOST_FOREACH(derived_ptr& child, children) {
for(derived_ptr& child: children) {
child->deleteCachedShortcuts();
}

11
gtsam/inference/ClusterTree-inst.h
View File

@ -13,7 +13,6 @@
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
namespace gtsam {
@ -87,7 +86,7 @@ struct EliminationData {
gatheredFactors += myData.childFactors;
// Check for Bayes tree orphan subtrees, and add them to our children
BOOST_FOREACH(const sharedFactor& f, node->factors) {
for(const sharedFactor& f: node->factors) {
if (const BayesTreeOrphanWrapper<BTNode>* asSubtree =
dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) {
myData.bayesTreeNode->children.push_back(asSubtree->clique);
@ -107,7 +106,7 @@ struct EliminationData {
// Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
// putting orphan subtrees in the index - they'll already be in the index of the ISAM2
// object they're added to.
BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals())
for(const Key& j: myData.bayesTreeNode->conditional()->frontals())
nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
// Store remaining factor in parent's gathered factors
@ -138,7 +137,7 @@ void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
size_t nrNewChildren = 0;
// Loop over children
size_t i = 0;
BOOST_FOREACH(const sharedNode& child, children) {
for(const sharedNode& child: children) {
if (merge[i]) {
nrKeys += child->orderedFrontalKeys.size();
nrFactors += child->factors.size();
@ -155,7 +154,7 @@ void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
typename Node::Children newChildren;
newChildren.reserve(nrNewChildren);
i = 0;
BOOST_FOREACH(const sharedNode& child, children) {
for(const sharedNode& child: children) {
if (merge[i]) {
// Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
orderedFrontalKeys.insert(orderedFrontalKeys.end(),
@ -228,7 +227,7 @@ std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > ClusterTree<
remaining->reserve(
remainingFactors_.size() + rootsContainer.childFactors.size());
remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) {
for(const sharedFactor& factor: rootsContainer.childFactors) {
if (factor)
remaining->push_back(factor);
}

5
gtsam/inference/Conditional-inst.h
View File

@ -18,7 +18,6 @@
// \callgraph
#pragma once
#include <boost/foreach.hpp>
#include <iostream>
#include <gtsam/inference/Conditional.h>
@ -29,11 +28,11 @@ namespace gtsam {
template<class FACTOR, class DERIVEDFACTOR>
void Conditional<FACTOR,DERIVEDFACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals())
for(Key key: frontals())
std::cout << " " << formatter(key);
if (nrParents() > 0)
std::cout << " |";
BOOST_FOREACH(Key parent, parents())
for(Key parent: parents())
std::cout << " " << formatter(parent);
std::cout << ")" << std::endl;
}

21
gtsam/inference/EliminationTree-inst.h
View File

@ -17,7 +17,6 @@
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/bind.hpp>
#include <stack>
@ -66,7 +65,7 @@ namespace gtsam {
const std::string& str, const KeyFormatter& keyFormatter) const
{
std::cout << str << "(" << keyFormatter(key) << ")\n";
BOOST_FOREACH(const sharedFactor& factor, factors) {
for(const sharedFactor& factor: factors) {
if(factor)
factor->print(str);
else
@ -107,7 +106,7 @@ namespace gtsam {
// for row i \in Struct[A*j] do
node->children.reserve(factors.size());
node->factors.reserve(factors.size());
BOOST_FOREACH(const size_t i, factors) {
for(const size_t i: factors) {
// If we already hit a variable in this factor, make the subtree containing the previous
// variable in this factor a child of the current node. This means that the variables
// eliminated earlier in the factor depend on the later variables in the factor. If we
@ -222,15 +221,15 @@ namespace gtsam {
// Add roots in sorted order
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& root, this->roots_) { keys.insert(std::make_pair(root->key, root)); }
for(const sharedNode& root: this->roots_) { keys.insert(std::make_pair(root->key, root)); }
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
}
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& root, expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
for(const sharedNode& root: expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
}
// Traverse, adding children in sorted order
@ -262,15 +261,15 @@ namespace gtsam {
// Add children in sorted order
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& node, node1->children) { keys.insert(std::make_pair(node->key, node)); }
for(const sharedNode& node: node1->children) { keys.insert(std::make_pair(node->key, node)); }
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack1.push(key_node.second); }
for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
}
{
FastMap<Key,sharedNode> keys;
BOOST_FOREACH(const sharedNode& node, node2->children) { keys.insert(std::make_pair(node->key, node)); }
for(const sharedNode& node: node2->children) { keys.insert(std::make_pair(node->key, node)); }
typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
BOOST_FOREACH(const Key_Node& key_node, keys) { stack2.push(key_node.second); }
for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
}
}

5
gtsam/inference/Factor.cpp
View File

@ -19,7 +19,6 @@
// \callgraph
#include <boost/foreach.hpp>
#include <iostream>
#include <gtsam/inference/Factor.h>
@ -35,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */
void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " ";
BOOST_FOREACH(Key key, keys_) std::cout << " " << formatter(key);
for(Key key: keys_) std::cout << " " << formatter(key);
std::cout << std::endl;
}
@ -44,4 +43,4 @@ namespace gtsam {
return keys_ == other.keys_;
}
}
}

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

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

2
gtsam/inference/ISAM-inst.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
factors += newFactors;
// Add the orphaned subtrees
BOOST_FOREACH(const sharedClique& orphan, orphans)
for(const sharedClique& orphan: orphans)
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
// eliminate into a Bayes net

2
gtsam/inference/JunctionTree-inst.h
View File

@ -105,7 +105,7 @@ struct ConstructorTraversalData {
// decide which children to merge, as index into children
std::vector<bool> merge(nrChildren, false);
size_t myNrFrontals = 1, i = 0;
BOOST_FOREACH(const sharedNode& child, node->children) {
for(const sharedNode& child: node->children) {
// Check if we should merge the i^th child
if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
// Increment number of frontal variables

3
gtsam/inference/Key.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/inference/Key.h>
#include <gtsam/inference/LabeledSymbol.h>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
@ -63,7 +62,7 @@ static void Print(const CONTAINER& keys, const string& s,
if (keys.empty())
cout << "(none)" << endl;
else {
BOOST_FOREACH(const Key& key, keys)
for(const Key& key: keys)
cout << keyFormatter(key) << " ";
cout << endl;
}

7
gtsam/inference/MetisIndex-inl.h
View File

@ -17,7 +17,6 @@
#pragma once
#include <boost/foreach.hpp>
#include <map>
#include <vector>
@ -42,7 +41,7 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
// key to integer mapping. This is referenced during the adjaceny step
for (size_t i = 0; i < factors.size(); i++) {
if (factors[i]) {
BOOST_FOREACH(const Key& key, *factors[i]) {
for(const Key& key: *factors[i]) {
keySet.insert(keySet.end(), key); // Keep a track of all unique keys
if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) {
intKeyBMap_.insert(bm_type::value_type(key, keyCounter));
@ -55,8 +54,8 @@ void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
// Create an adjacency mapping that stores the set of all adjacent keys for every key
for (size_t i = 0; i < factors.size(); i++) {
if (factors[i]) {
BOOST_FOREACH(const Key& k1, *factors[i])
BOOST_FOREACH(const Key& k2, *factors[i])
for(const Key& k1: *factors[i])
for(const Key& k2: *factors[i])
if (k1 != k2) {
// Store both in Key and int32_t format
int i = intKeyBMap_.left.at(k1);

5
gtsam/inference/VariableIndex-inl.h
View File

@ -19,7 +19,6 @@
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
namespace gtsam {
@ -34,7 +33,7 @@ void VariableIndex::augment(const FG& factors,
if (factors[i]) {
const size_t globalI =
newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
BOOST_FOREACH(const Key key, *factors[i]) {
for(const Key key: *factors[i]) {
index_[key].push_back(globalI);
++nEntries_;
}
@ -67,7 +66,7 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
throw std::invalid_argument(
"Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if (factors[i]) {
BOOST_FOREACH(Key j, *factors[i]) {
for(Key j: *factors[i]) {
Factors& factorEntries = internalAt(j);
Factors::iterator entry = std::find(factorEntries.begin(),
factorEntries.end(), *factorIndex);

8
gtsam/inference/VariableIndex.cpp
View File

@ -33,9 +33,9 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str;
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
for(KeyMap::value_type key_factors: index_) {
cout << "var " << keyFormatter(key_factors.first) << ":";
BOOST_FOREACH(const size_t factor, key_factors.second)
for(const size_t factor: key_factors.second)
cout << " " << factor;
cout << "\n";
}
@ -46,9 +46,9 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
void VariableIndex::outputMetisFormat(ostream& os) const {
os << size() << " " << nFactors() << "\n";
// run over variables, which will be hyper-edges.
BOOST_FOREACH(KeyMap::value_type key_factors, index_) {
for(KeyMap::value_type key_factors: index_) {
// every variable is a hyper-edge covering its factors
BOOST_FOREACH(const size_t factor, key_factors.second)
for(const size_t factor: key_factors.second)
os << (factor+1) << " "; // base 1
os << "\n";
}

5
gtsam/inference/VariableSlots.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/inference/VariableSlots.h>
#include <iostream>
#include <boost/foreach.hpp>
using namespace std;
@ -33,12 +32,12 @@ void VariableSlots::print(const std::string& str) const {
else {
cout << str << "\n";
cout << "Var:\t";
BOOST_FOREACH(const value_type& slot, *this) { cout << slot.first << "\t"; }
for(const value_type& slot: *this) { cout << slot.first << "\t"; }
cout << "\n";
for(size_t i=0; i<this->begin()->second.size(); ++i) {
cout << " \t";
BOOST_FOREACH(const value_type& slot, *this) {
for(const value_type& slot: *this) {
if(slot.second[i] == Empty)
cout << "x" << "\t";
else

5
gtsam/inference/VariableSlots.h
View File

@ -24,7 +24,6 @@
#include <gtsam/base/timing.h>
#include <gtsam/base/Testable.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <iostream>
@ -99,10 +98,10 @@ VariableSlots::VariableSlots(const FG& factorGraph)
// removed factors. The slot number is the max integer value if the
// factor does not involve that variable.
size_t jointFactorPos = 0;
BOOST_FOREACH(const typename FG::sharedFactor& factor, factorGraph) {
for(const typename FG::sharedFactor& factor: factorGraph) {
assert(factor);
size_t factorVarSlot = 0;
BOOST_FOREACH(const Key involvedVariable, *factor) {
for(const Key involvedVariable: *factor) {
// Set the slot in this factor for this variable. If the
// variable was not already discovered, create an array for it
// that we'll fill with the slot indices for each factor that

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

@ -18,7 +18,6 @@
#pragma once
#include <stdexcept>
#include <boost/foreach.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
@ -32,8 +31,6 @@
#include <gtsam/inference/graph.h>
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
@ -123,9 +120,10 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
G g;
std::map<KEY, V> key2vertex;
V v1, v2, root;
KEY child, parent;
bool foundRoot = false;
FOREACH_PAIR(child, parent, p_map) {
for(auto child_parent: p_map) {
KEY child, parent;
std::tie(child,parent) = child_parent;
if (key2vertex.find(child) == key2vertex.end()) {
v1 = add_vertex(child, g);
key2vertex.insert(std::make_pair(child, v1));
@ -193,7 +191,7 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
// attach the relative poses to the edges
PoseEdge edge12, edge21;
bool found1, found2;
BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) {
for(typename G::sharedFactor nl_factor: graph) {
if (nl_factor->keys().size() > 2)
throw std::invalid_argument("composePoses: only support factors with at most two keys");
@ -243,7 +241,7 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// convert edge to string pairs
PredecessorMap<KEY> tree;
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 parent = boost::get(boost::vertex_name, g, vi);
tree.insert(key, parent);
@ -258,7 +256,7 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
typedef typename G::sharedFactor F ;
BOOST_FOREACH(const F& factor, g)
for(const F& factor: g)
{
if (factor->keys().size() > 2)
throw(std::invalid_argument("split: only support factors with at most two keys"));

15
gtsam/linear/Errors.cpp
View File

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

19
gtsam/linear/GaussianBayesNet.cpp
View File

@ -20,14 +20,11 @@
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/range/adaptor/reversed.hpp>
using namespace std;
using namespace gtsam;
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
#define REVERSE_FOREACH_PAIR( KEY, VAL, COL) BOOST_REVERSE_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
// Instantiate base class
@ -52,7 +49,7 @@ namespace gtsam {
VectorValues soln(solutionForMissing); // possibly empty
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
/** solve each node in turn in topological sort order (parents first)*/
BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
for (auto cg: boost::adaptors::reverse(*this)) {
// i^th part of R*x=y, x=inv(R)*y
// (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
soln.insert(cg->solve(soln));
@ -88,7 +85,7 @@ namespace gtsam {
VectorValues result;
// TODO this looks pretty sketchy. result is passed as the parents argument
// as it's filled up by solving the gaussian conditionals.
BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) {
for (auto cg: boost::adaptors::reverse(*this)) {
result.insert(cg->solveOtherRHS(result, rhs));
}
return result;
@ -107,7 +104,7 @@ namespace gtsam {
// we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L
BOOST_FOREACH(const sharedConditional& cg, *this)
for(const sharedConditional& cg: *this)
cg->solveTransposeInPlace(gy);
return gy;
@ -146,15 +143,15 @@ namespace gtsam {
KeySet keys = factorGraph.keys();
// add frontal keys in order
Ordering ordering;
BOOST_FOREACH (const sharedConditional& cg, *this)
for (const sharedConditional& cg: *this)
if (cg) {
BOOST_FOREACH (Key key, cg->frontals()) {
for (Key key: cg->frontals()) {
ordering.push_back(key);
keys.erase(key);
}
}
// add remaining keys in case Bayes net is incomplete
BOOST_FOREACH (Key key, keys)
for (Key key: keys)
ordering.push_back(key);
// return matrix and RHS
return factorGraph.jacobian(ordering);
@ -182,7 +179,7 @@ namespace gtsam {
double GaussianBayesNet::logDeterminant() const
{
double logDet = 0.0;
BOOST_FOREACH(const sharedConditional& cg, *this) {
for(const sharedConditional& cg: *this) {
if(cg->get_model()) {
Vector diag = cg->get_R().diagonal();
cg->get_model()->whitenInPlace(diag);

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

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

42
gtsam/linear/GaussianFactorGraph.cpp
View File

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

2
gtsam/linear/GaussianFactorGraph.h
View File

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

5
gtsam/linear/HessianFactor.cpp
View File

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

5
gtsam/linear/IterativeSolver.cpp
View File

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

11
gtsam/linear/JacobianFactor.cpp
View File

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

9
gtsam/linear/NoiseModel.cpp
View File

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

3
gtsam/linear/PCGSolver.cpp
View File

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

2
gtsam/linear/Preconditioner.cpp
View File

@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build(
/* getting the block diagonals over the factors */
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);
/* 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/RegularJacobianFactor.h>
#include <boost/foreach.hpp>
#include <vector>
namespace gtsam {
@ -114,7 +113,7 @@ public:
double* yvalues) const {
// Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size());
BOOST_FOREACH(VectorD & yi, y_)
for(VectorD & yi: y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive
@ -147,7 +146,7 @@ public:
// Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size());
BOOST_FOREACH(VectorD & yi, y_)
for(VectorD & yi: y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive

1
gtsam/linear/RegularJacobianFactor.h
View File

@ -20,7 +20,6 @@
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
namespace gtsam {

5
gtsam/linear/Scatter.cpp
View File

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

38
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -32,7 +32,7 @@
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/foreach.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/shared_ptr.hpp>
#include <algorithm>
@ -59,7 +59,7 @@ namespace gtsam {
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
for(const GaussianFactor::shared_ptr &gf: gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) {
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 */
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 ) {
touched[id] = true ;
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) {
edges_.reserve(indices.size());
BOOST_FOREACH ( const size_t &idx, indices ) {
for ( const size_t &idx: indices ) {
edges_.push_back(SubgraphEdge(idx, 1.0));
}
}
@ -144,7 +144,7 @@ Subgraph::Subgraph(const std::vector<size_t> &indices) {
/****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size());
BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) {
for ( const SubgraphEdge &edge: edges_ ) {
eid.push_back(edge.index_);
}
return eid;
@ -180,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) {
for ( const SubgraphEdge &e: subgraph.edges() ) {
os << e << ", " ;
}
return os;
@ -286,7 +286,7 @@ std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 1 ) {
result.push_back(idx);
}
@ -299,7 +299,7 @@ std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
for ( const GaussianFactor::shared_ptr &gf: gfg ) {
if ( gf->size() == 2 ) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ( (k1-k0) == 1 || (k0-k1) == 1 )
@ -332,9 +332,9 @@ std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
/* traversal */
while ( !q.empty() ) {
const size_t head = q.front(); q.pop();
BOOST_FOREACH ( const size_t id, variableIndex[head] ) {
for ( const size_t id: variableIndex[head] ) {
const GaussianFactor &gf = *gfg[id];
BOOST_FOREACH ( const size_t key, gf.keys() ) {
for ( const size_t key: gf.keys() ) {
if ( flags.count(key) == 0 ) {
q.push(key);
flags.insert(key);
@ -360,7 +360,7 @@ std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con
DSFVector D(n) ;
size_t count = 0 ; double sum = 0.0 ;
BOOST_FOREACH (const size_t id, idx) {
for (const size_t id: idx) {
const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue;
const size_t u = ordering.find(gf.keys()[0])->second,
@ -399,7 +399,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg
}
/* down weight the tree edges to zero */
BOOST_FOREACH ( const size_t id, tree ) {
for ( const size_t id: tree ) {
w[id] = 0.0;
}
@ -419,7 +419,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg
const size_t m = gfg.size() ;
Weights weight; weight.reserve(m);
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) {
for(const GaussianFactor::shared_ptr &gf: gfg ) {
switch ( parameters_.skeletonWeight_ ) {
case SubgraphBuilderParameters::EQUAL:
weight.push_back(1.0);
@ -569,7 +569,7 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
BOOST_REVERSE_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
for (auto cg: boost::adaptors::reverse(*Rc1_)) {
/* collect a subvector of x that consists of the parents of cg (S) */
const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
@ -589,7 +589,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
BOOST_FOREACH(const boost::shared_ptr<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 solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().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 */
size_t d = 0;
BOOST_FOREACH ( const Key &key, keys ) {
for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.push_back(make_pair(entry.colstart(), entry.dim()));
d += entry.dim();
@ -643,7 +643,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
/* use the cache to fill the result */
Vector result = Vector::Zero(d, 1);
size_t idx = 0;
BOOST_FOREACH ( const Cache::value_type &p, cache ) {
for ( const Cache::value_type &p: cache ) {
result.segment(idx, p.second) = src.segment(p.first, p.second) ;
idx += p.second;
}
@ -655,7 +655,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
/* use the cache */
size_t idx = 0;
BOOST_FOREACH ( const Key &key, keys ) {
for ( const Key &key: keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
idx += entry.dim();
@ -668,7 +668,7 @@ buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, co
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
result->reserve(subgraph.size());
BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) {
for ( const SubgraphEdge &e: subgraph ) {
const size_t idx = e.index();
if ( clone ) result->push_back(gfg[idx]->clone());
else result->push_back(gfg[idx]);

2
gtsam/linear/SubgraphSolver.cpp
View File

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

31
gtsam/linear/VectorValues.cpp
View File

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

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

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

1
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -30,7 +30,6 @@ using namespace boost::assign;
#include <sstream>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
using namespace std;
using namespace gtsam;

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

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

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

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

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

105
gtsam/nonlinear/ISAM2.cpp
View File

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

2
gtsam/nonlinear/ISAM2.h
View File

@ -200,7 +200,7 @@ struct GTSAM_EXPORT ISAM2Params {
else
{
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";
}
}

6
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

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

9
gtsam/nonlinear/LinearContainerFactor.cpp
View File

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

4
gtsam/nonlinear/Marginals.cpp
View File

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

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

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

25
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

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

3
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -16,12 +16,9 @@
*/
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <boost/foreach.hpp>
#include <iostream>
using namespace std;

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

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

4
gtsam/nonlinear/factorTesting.h
View File

@ -22,8 +22,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/foreach.hpp>
namespace gtsam {
/**
@ -49,7 +47,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
// Loop over all variables
const double one_over_2delta = 1.0 / (2.0 * delta);
VectorValues dX = values.zeroVectors();
BOOST_FOREACH(Key key, factor) {
for(Key key: factor) {
// Compute central differences using the values struct.
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);

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

@ -366,7 +366,7 @@ TEST(Values, filter) {
int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
EXPECT_LONGS_EQUAL(2, (long)filtered.size());
BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) {
for(const Values::Filtered<>::KeyValuePair& key_value: filtered) {
if(i == 0) {
LONGS_EQUAL(2, (long)key_value.key);
try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
@ -401,7 +401,7 @@ TEST(Values, filter) {
i = 0;
Values::ConstFiltered<Pose3> pose_filtered = values.filter<Pose3>();
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) {
EXPECT_LONGS_EQUAL(1, (long)key_value.key);
EXPECT(assert_equal(pose1, key_value.value));
@ -437,7 +437,7 @@ TEST(Values, Symbol_filter) {
values.insert(Symbol('y', 3), pose3);
int i = 0;
BOOST_FOREACH(const Values::Filtered<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) {
LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
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;
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;
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;
Values validRot3;
BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
for(const VectorValues::value_type& it: relaxedRot3) {
Key key = it.first;
if (key != keyAnchor) {
const Vector& rotVector = it.second;
@ -108,7 +108,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
gttic(InitializePose3_buildPose3graph);
NonlinearFactorGraph pose3Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
// recast to a between on Pose3
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
Values inverseRot;
inverseRot.insert(keyAnchor, Rot3());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) {
for(const Values::ConstKeyValuePair& key_value: givenGuess) {
Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key);
inverseRot.insert(key, pose.rotation().inverse());
@ -165,7 +165,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// calculate max node degree & allocate gradient
size_t maxNodeDeg = 0;
VectorValues grad;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
for(const Values::ConstKeyValuePair& key_value: inverseRot) {
Key key = key_value.key;
grad.insert(key,Vector3::Zero());
size_t currNodeDeg = (adjEdgesMap.at(key)).size();
@ -191,12 +191,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
//std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
// <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
maxGrad = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
for(const Values::ConstKeyValuePair& key_value: inverseRot) {
Key key = key_value.key;
//std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl;
Vector gradKey = Vector3::Zero();
// collect the gradient for each edge incident on key
BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){
for(const size_t& factorId: adjEdgesMap.at(key)){
Rot3 Rij = factorId2RotMap.at(factorId);
Rot3 Ri = inverseRot.at<Rot3>(key);
if( key == (pose3Graph.at(factorId))->keys()[0] ){
@ -236,7 +236,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// Return correct rotations
const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
Values estimateRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
for(const Values::ConstKeyValuePair& key_value: inverseRot) {
Key key = key_value.key;
if (key != keyAnchor) {
const Rot3& R = inverseRot.at<Rot3>(key);
@ -252,7 +252,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
/* ************************************************************************* */
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
size_t factorId = 0;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose3Graph) {
for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
@ -321,7 +321,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure
Values initialPose;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
for(const Values::ConstKeyValuePair& key_value: initialRot){
Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
@ -346,7 +346,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure
Values estimate;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) {
for(const Values::ConstKeyValuePair& key_value: GNresult) {
Key key = key_value.key;
if (key != keyAnchor) {
const Pose3& pose = GNresult.at<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)
return computePoses(pose3Graph, orientations);
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
// for(const Values::ConstKeyValuePair& key_value: orientations) {
// Key key = key_value.key;
// if (key != keyAnchor) {
// const Point3& pos = givenGuess.at<Pose3>(key).translation();

2
gtsam/slam/JacobianFactorQ.h
View File

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

4
gtsam/slam/JacobianFactorSVD.h
View File

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

1
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -10,7 +10,6 @@
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <iosfwd>
namespace gtsam {

Some files were not shown because too many files have changed in this diff Show More