diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp new file mode 100644 index 000000000..49bf23024 --- /dev/null +++ b/timing/timeSFMBAL.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeSFMBAL.cpp + * @brief time structure from motion with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//#define TERNARY + +int main(int argc, char *argv[]) { + typedef GeneralSFMFactor, Point3> sfmFactor; + using symbol_shorthand::P; + + string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(argc>1 ? argv[1] : defaultFilename, db); + if (!success) throw runtime_error("Could not access file!"); + + SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + NonlinearFactorGraph graph; + + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) + graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + } + + Values initial = initialCamerasAndPointsEstimate(db); + + LevenbergMarquardtOptimizer lm(graph, initial); + + Values actual = lm.optimize(); + tictoc_print_(); + + return 0; +}