Replaced BOOSE_FOREACH with for in timing folder. Tested the changed code locally: successful.

release/4.3a0
Yao Chen 2016-05-20 21:22:30 -04:00
parent 20c586c398
commit b68746beae
11 changed files with 22 additions and 26 deletions

View File

@ -40,7 +40,7 @@ public:
void multiplyHessian(double alpha, const VectorValues& x, void multiplyHessian(double alpha, const VectorValues& x,
VectorValues& y) const { VectorValues& y) const {
BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) { for(const KeyMatrix2D& Fi: this->Fblocks_) {
static const Vector empty; static const Vector empty;
Key key = Fi.first; Key key = Fi.first;
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty); std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);

View File

@ -57,7 +57,7 @@ int main(int argc, char *argv[]) {
// Compute marginals // Compute marginals
Marginals marginals(graph, optimizer.values()); Marginals marginals(graph, optimizer.values());
int i=0; int i=0;
BOOST_FOREACH(Key key, initial.keys()) { for(Key key: initial.keys()) {
gttic_(marginalInformation); gttic_(marginalInformation);
Matrix info = marginals.marginalInformation(key); Matrix info = marginals.marginalInformation(key);
gttoc_(marginalInformation); gttoc_(marginalInformation);

View File

@ -82,7 +82,7 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
// // create an internal ordering to render Ab // // create an internal ordering to render Ab
// Ordering render; // Ordering render;
// render += key; // render += key;
// BOOST_FOREACH(const Symbol& k, joint_factor->keys()) // for(const Symbol& k: joint_factor->keys())
// if (k != key) render += k; // if (k != key) render += k;
// //
// Matrix Ab = joint_factor->matrix_augmented(render,false); // Matrix Ab = joint_factor->matrix_augmented(render,false);

View File

@ -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. // the factor graph already includes a factor for the prior/equality constraint.
// double dof = graph.size() - config.size(); // double dof = graph.size() - config.size();
int graph_dim = 0; 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 += nlf->dim(); graph_dim += nlf->dim();
} }
double dof = graph_dim - config.dim(); // kaess: changed to dim double dof = graph_dim - config.dim(); // kaess: changed to dim
@ -246,7 +246,7 @@ int main(int argc, char *argv[]) {
break; break;
} }
tictoc_print_(); tictoc_print_();
BOOST_FOREACH(Key key, values.keys()) { for(Key key: values.keys()) {
gttic_(marginalInformation); gttic_(marginalInformation);
Matrix info = marginals.marginalInformation(key); Matrix info = marginals.marginalInformation(key);
gttoc_(marginalInformation); gttoc_(marginalInformation);

View File

@ -45,7 +45,7 @@ int main(int argc, char *argv[]) {
Sampler sampler(42u); Sampler sampler(42u);
Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>(); Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses) for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: poses)
initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0

View File

@ -20,7 +20,6 @@
#include <boost/timer.hpp> #include <boost/timer.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
#include <boost/foreach.hpp>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
@ -173,29 +172,29 @@ int main(int argc, char* argv[]) {
// Do a few initial assignments to let any cache effects stabilize // Do a few initial assignments to let any cache effects stabilize
for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj())); for_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
for(size_t rep=0; rep<1000; ++rep) 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_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
for(size_t rep=0; rep<1000; ++rep) 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(); basicTime = tim.elapsed();
cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; 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_each(ijs.begin(), ijs.end(), _1 = make_pair(rni(),rnj()));
for(size_t rep=0; rep<1000; ++rep) 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(); fullTime = tim.elapsed();
cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; 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_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%top.rows(),rnj()));
for(size_t rep=0; rep<1000; ++rep) 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(); topTime = tim.elapsed();
cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; 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_each(ijs.begin(), ijs.end(), _1 = make_pair(rni()%block.rows(),rnj()%block.cols()));
for(size_t rep=0; rep<1000; ++rep) 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(); blockTime = tim.elapsed();
cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl; cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl;

View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <boost/foreach.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -38,7 +37,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { 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; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j)));
@ -47,9 +46,9 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
BOOST_FOREACH (const SfM_Camera& camera, db.cameras) for (const SfM_Camera& camera: db.cameras)
initial.insert(C(i++), camera); 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); initial.insert(P(j++), track.p);
return optimize(db, graph, initial); return optimize(db, graph, initial);

View File

@ -22,7 +22,6 @@
#include <gtsam/nonlinear/AdaptAutoDiff.h> #include <gtsam/nonlinear/AdaptAutoDiff.h>
#include <gtsam/3rdparty/ceres/example.h> #include <gtsam/3rdparty/ceres/example.h>
#include <boost/foreach.hpp>
#include <stddef.h> #include <stddef.h>
#include <stdexcept> #include <stdexcept>
#include <string> #include <string>
@ -46,7 +45,7 @@ int main(int argc, char* argv[]) {
// Build graph // Build graph
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { 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; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
Expression<Vector9> camera_(C(i)); Expression<Vector9> camera_(C(i));
@ -59,14 +58,14 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { for (const SfM_Camera& camera: db.cameras) {
// readBAL converts to GTSAM format, so we need to convert back ! // readBAL converts to GTSAM format, so we need to convert back !
Pose3 openGLpose = gtsam2openGL(camera.pose()); Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9; Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration().vector(); v9 << Pose3::Logmap(openGLpose), camera.calibration().vector();
initial.insert(C(i++), v9); 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(); Vector3 v3 = track.p.vector();
initial.insert(P(j++), v3); initial.insert(P(j++), v3);
} }

View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <boost/foreach.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -35,7 +34,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor // Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) { 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; size_t i = m.first;
Point2 z = m.second; Point2 z = m.second;
Pose3_ camTnav_(C(i)); Pose3_ camTnav_(C(i));
@ -50,12 +49,12 @@ int main(int argc, char* argv[]) {
Values initial; Values initial;
size_t i = 0, j = 0; size_t i = 0, j = 0;
BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { for (const SfM_Camera& camera: db.cameras) {
initial.insert(C(i), camera.pose().inverse()); // inverse !!! initial.insert(C(i), camera.pose().inverse()); // inverse !!!
initial.insert(K(i), camera.calibration()); initial.insert(K(i), camera.calibration());
i += 1; i += 1;
} }
BOOST_FOREACH (const SfM_Track& track, db.tracks) for (const SfM_Track& track: db.tracks)
initial.insert(P(j++), track.p); initial.insert(P(j++), track.p);
bool separateCalibration = true; bool separateCalibration = true;

View File

@ -150,7 +150,7 @@ int main(void) {
ms += m; ms += m;
//for (size_t m=10;m<=100;m+=10) ms += m; //for (size_t m=10;m<=100;m+=10) ms += m;
// loop over number of images // loop over number of images
BOOST_FOREACH(size_t m,ms) for(size_t m: ms)
timeAll<PinholePose<Cal3Bundler> >(m, NUM_ITERATIONS); timeAll<PinholePose<Cal3Bundler> >(m, NUM_ITERATIONS);
} }

View File

@ -93,7 +93,7 @@ int main(int argc, char *argv[]) {
// Write times // Write times
ofstream timesFile("times.txt"); ofstream timesFile("times.txt");
BOOST_FOREACH(double t, times) { for(double t: times) {
timesFile << t << "\n"; } timesFile << t << "\n"; }
return 0; return 0;