diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h index 08e9d8f4b..5e2664d26 100644 --- a/timing/DummyFactor.h +++ b/timing/DummyFactor.h @@ -40,7 +40,7 @@ public: void multiplyHessian(double alpha, const VectorValues& x, VectorValues& y) const { - BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) { + for(const KeyMatrix2D& Fi: this->Fblocks_) { static const Vector empty; Key key = Fi.first; std::pair it = y.tryInsert(key, empty); diff --git a/timing/timeBatch.cpp b/timing/timeBatch.cpp index a1e5f359b..4ed1a4555 100644 --- a/timing/timeBatch.cpp +++ b/timing/timeBatch.cpp @@ -57,7 +57,7 @@ int main(int argc, char *argv[]) { // Compute marginals Marginals marginals(graph, optimizer.values()); int i=0; - BOOST_FOREACH(Key key, initial.keys()) { + for(Key key: initial.keys()) { gttic_(marginalInformation); Matrix info = marginals.marginalInformation(key); gttoc_(marginalInformation); diff --git a/timing/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp index a3157729e..b49d2f82b 100644 --- a/timing/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -82,7 +82,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) { // // create an internal ordering to render Ab // Ordering render; // render += key; -// BOOST_FOREACH(const Symbol& k, joint_factor->keys()) +// for(const Symbol& k: joint_factor->keys()) // if (k != key) render += k; // // Matrix Ab = joint_factor->matrix_augmented(render,false); diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index b5dd37516..cac98b09b 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -60,7 +60,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& nlf, graph) { + for(const boost::shared_ptr& nlf: graph) { graph_dim += nlf->dim(); } double dof = graph_dim - config.dim(); // kaess: changed to dim @@ -246,7 +246,7 @@ int main(int argc, char *argv[]) { break; } tictoc_print_(); - BOOST_FOREACH(Key key, values.keys()) { + for(Key key: values.keys()) { gttic_(marginalInformation); Matrix info = marginals.marginalInformation(key); gttoc_(marginalInformation); diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 3a4da89b5..d0b6b263c 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -45,7 +45,7 @@ int main(int argc, char *argv[]) { Sampler sampler(42u); Values::ConstFiltered poses = solution->filter(); SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) + for(const Values::ConstFiltered::KeyValuePair& it: poses) initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); // Add prior on the pose having index (key) = 0 diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index a2d11a756..a2010891b 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include @@ -173,29 +172,29 @@ int main(int argc, char* argv[]) { // Do a few initial assignments to let any cache effects stabilize for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { mat(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = rng(); } basicTime = tim.elapsed(); cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { full(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { full(ij.first, ij.second) = rng(); } fullTime = tim.elapsed(); cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { top(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { top(ij.first, ij.second) = rng(); } topTime = tim.elapsed(); cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols())); for(size_t rep=0; rep<1000; ++rep) - BOOST_FOREACH(const ij_t& ij, ijs) { block(ij.first, ij.second) = rng(); } + for(const ij_t& ij: ijs) { block(ij.first, ij.second) = rng(); } blockTime = tim.elapsed(); cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 6308a1d61..c1e695b3a 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -23,7 +23,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -38,7 +37,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); @@ -47,9 +46,9 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) + for (const SfM_Camera& camera: db.cameras) initial.insert(C(i++), camera); - BOOST_FOREACH (const SfM_Track& track, db.tracks) + for (const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); return optimize(db, graph, initial); diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 4545abc21..867953257 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include #include @@ -46,7 +45,7 @@ int main(int argc, char* argv[]) { // Build graph NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Expression camera_(C(i)); @@ -59,14 +58,14 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + for (const SfM_Camera& camera: db.cameras) { // readBAL converts to GTSAM format, so we need to convert back ! Pose3 openGLpose = gtsam2openGL(camera.pose()); Vector9 v9; v9 << Pose3::Logmap(openGLpose), camera.calibration().vector(); initial.insert(C(i++), v9); } - BOOST_FOREACH (const SfM_Track& track, db.tracks) { + for (const SfM_Track& track: db.tracks) { Vector3 v3 = track.p.vector(); initial.insert(P(j++), v3); } diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp index 32fae4ac2..33680e813 100644 --- a/timing/timeSFMBALcamTnav.cpp +++ b/timing/timeSFMBALcamTnav.cpp @@ -23,7 +23,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -35,7 +34,7 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + for (const SfM_Measurement& m: db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Pose3_ camTnav_(C(i)); @@ -50,12 +49,12 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; - BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + for (const SfM_Camera& camera: db.cameras) { initial.insert(C(i), camera.pose().inverse()); // inverse !!! initial.insert(K(i), camera.calibration()); i += 1; } - BOOST_FOREACH (const SfM_Track& track, db.tracks) + for (const SfM_Track& track: db.tracks) initial.insert(P(j++), track.p); bool separateCalibration = true; diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 9be55f831..e64c34340 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -150,7 +150,7 @@ int main(void) { ms += m; //for (size_t m=10;m<=100;m+=10) ms += m; // loop over number of images - BOOST_FOREACH(size_t m,ms) + for(size_t m: ms) timeAll >(m, NUM_ITERATIONS); } diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index c4196f414..7a4413ac7 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -93,7 +93,7 @@ int main(int argc, char *argv[]) { // Write times ofstream timesFile("times.txt"); - BOOST_FOREACH(double t, times) { + for(double t: times) { timesFile << t << "\n"; } return 0;