From 7ec056f5cc022f1694b0da4f9e1bde00d4fd77dd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 5 Jul 2015 10:26:11 -0700 Subject: [PATCH] Split script into two scripts, isolated common code --- timing/timeSFMBAL.cpp | 144 ++++------------------------------ timing/timeSFMBAL.h | 84 ++++++++++++++++++++ timing/timeSFMBALautodiff.cpp | 92 ++++++++++++++++++++++ 3 files changed, 190 insertions(+), 130 deletions(-) create mode 100644 timing/timeSFMBAL.h create mode 100644 timing/timeSFMBALautodiff.cpp diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index fce535d14..26c3d3955 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -11,162 +11,46 @@ /** * @file timeSFMBAL.cpp - * @brief time structure from motion with BAL file + * @brief time structure from motion with BAL file, conventional GeneralSFMFactor * @author Frank Dellaert * @date June 6, 2015 */ -#include +#include "timeSFMBAL.h" + +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include -#include using namespace std; using namespace gtsam; -//#define USE_GTSAM_FACTOR -#ifdef USE_GTSAM_FACTOR -#include -#include -#include typedef PinholeCamera Camera; typedef GeneralSFMFactor SfmFactor; -#else -#include -#include -#include -// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html for conventions -// Special version of Cal3Bundler so that default constructor = 0,0,0 -struct CeresCalibration: public Cal3Bundler { - CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, - double v0 = 0) : - Cal3Bundler(f, k1, k2, u0, v0) { - } - CeresCalibration(const Cal3Bundler& cal) : - Cal3Bundler(cal) { - } - CeresCalibration retract(const Vector& d) const { - return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); - } - Vector3 localCoordinates(const CeresCalibration& T2) const { - return T2.vector() - vector(); - } -}; - -namespace gtsam { -template<> -struct traits : public internal::Manifold { -}; -} - -// With that, camera below behaves like Snavely's 9-dim vector -typedef PinholeCamera Camera; -#endif int main(int argc, char* argv[]) { - using symbol_shorthand::P; + // parse options and read BAL file + SfM_data db = preamble(argc, argv); - // Load BAL file (default is tiny) - //string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); - string defaultFilename = "/home/frank/problem-16-22106-pre.txt"; - SfM_data db; - bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); - if (!success) - throw runtime_error("Could not access file!"); - -#ifndef USE_GTSAM_FACTOR - AdaptAutoDiff snavely; -#endif - - // Build graph - SharedNoiseModel unit2 = noiseModel::Unit::Create(2); + // 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) { size_t i = m.first; Point2 z = m.second; -#ifdef USE_GTSAM_FACTOR - graph.push_back(SfmFactor(z, unit2, i, P(j))); -#else - Expression camera_(i); - Expression point_(P(j)); - // Snavely expects measurements in OpenGL format, with y increasing upwards - graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()), - Expression(snavely, camera_, point_)); -#endif + graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j))); } } Values initial; size_t i = 0, j = 0; - BOOST_FOREACH(const SfM_Camera& camera, db.cameras) { -#ifdef USE_GTSAM_FACTOR - initial.insert((i++), camera); -#else - // readBAL converts to GTSAM format, so we need to convert back ! - Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration()); - Vector9 v9 = Camera().localCoordinates(ceresCamera); - initial.insert((i++), v9); -#endif - } - BOOST_FOREACH(const SfM_Track& track, db.tracks) { -#ifdef USE_GTSAM_FACTOR + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) + initial.insert(C(i++), camera); + BOOST_FOREACH (const SfM_Track& track, db.tracks) initial.insert(P(j++), track.p); -#else - Vector3 v3 = track.p.vector(); - initial.insert(P(j++), v3); -#endif - } - // Check projection of first point in first camera - Point2 expected = db.tracks.front().measurements.front().second; -#ifdef USE_GTSAM_FACTOR - Camera camera = initial.at(0); - Point3 point = initial.at(P(0)); - Point2 actual = camera.project(point); -#else - Vector9 camera = initial.at(0); - Vector3 point = initial.at(P(0)); - Point2 z = snavely(camera, point); - // Again: fix y to increase upwards - Point2 actual(z.x(), -z.y()); -#endif - assert_equal(expected,actual,10); - - // Create Schur-complement ordering -#ifdef CCOLAMD - vector pointKeys; - for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j)); - Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true); -#else - Ordering ordering; - for (size_t j = 0; j < db.number_tracks(); j++) - ordering.push_back(P(j)); - for (size_t i = 0; i < db.number_cameras(); i++) - ordering.push_back(i); -#endif - - // Optimize - // Set parameters to be similar to ceres - LevenbergMarquardtParams params; - LevenbergMarquardtParams::SetCeresDefaults(¶ms); - params.setOrdering(ordering); - params.setVerbosityLM("SUMMARY"); - LevenbergMarquardtOptimizer lm(graph, initial, params); - Values result = lm.optimize(); - - tictoc_finishedIteration_(); - tictoc_print_(); - - return 0; + return optimize(db, graph, initial); } diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h new file mode 100644 index 000000000..e0f9d1458 --- /dev/null +++ b/timing/timeSFMBAL.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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.h + * @brief Common code for timeSFMBAL scripts + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::K; +using symbol_shorthand::P; + +static bool gUseSchur = true; +static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); + +// parse options and read BAL file +SfM_data preamble(int argc, char* argv[]) { + // primitive argument parsing: + if (argc > 2) { + if (string(argv[1]) == "--colamd") + gUseSchur = false; + else + throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]"); + } + + // Load BAL file + SfM_data db; + string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre"); + bool success = readBAL(argc > 1 ? argv[argc] : defaultFilename, db); + if (!success) throw runtime_error("Could not access file!"); + return db; +} + +// Create ordering and optimize +int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, + const Values& initial) { + using symbol_shorthand::P; + + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetCeresDefaults(¶ms); + params.setVerbosityLM("SUMMARY"); + + if (gUseSchur) { + // Create Schur-complement ordering + Ordering ordering; + for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j)); + for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(C(i)); + params.setOrdering(ordering); + } + + // Optimize + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +} diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp new file mode 100644 index 000000000..2674c5fdc --- /dev/null +++ b/timing/timeSFMBALautodiff.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALautodiff.cpp + * @brief time structure from motion with BAL file, Ceres autodiff version + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include "timeSFMBAL.h" +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html +// Special version of Cal3Bundler so that default constructor = 0,0,0 +// This is only used in localCoordinates below +struct CeresCalibration : public Cal3Bundler { + CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, + double v0 = 0) + : Cal3Bundler(f, k1, k2, u0, v0) {} + CeresCalibration(const Cal3Bundler& cal) : Cal3Bundler(cal) {} + CeresCalibration retract(const Vector& d) const { + return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + } + Vector3 localCoordinates(const CeresCalibration& T2) const { + return T2.vector() - vector(); + } +}; + +namespace gtsam { +template <> +struct traits : public internal::Manifold { +}; +} + +// With that, camera below behaves like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + AdaptAutoDiff snavely; + + // Build graph + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + Expression camera_(C(i)); + Expression point_(P(j)); + // Expects measurements in OpenGL format, with y increasing upwards + graph.addExpressionFactor(gNoiseModel, Vector2(z.x(), -z.y()), + Expression(snavely, camera_, point_)); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + // readBAL converts to GTSAM format, so we need to convert back ! + Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration()); + Vector9 v9 = Camera().localCoordinates(ceresCamera); + initial.insert(C(i++), v9); + } + BOOST_FOREACH (const SfM_Track& track, db.tracks) { + Vector3 v3 = track.p.vector(); + initial.insert(P(j++), v3); + } + + return optimize(db, graph, initial); +}