Replaced BOOSE_FOREACH with for in timing folder. Tested the changed code locally: successful.
parent
20c586c398
commit
b68746beae
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue