diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index fac2cf03d..e81c76bd6 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -36,6 +36,10 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_to, p); } +inline Point3_ transform_from(const Pose3_& x, const Point3_& p) { + return Point3_(x, &Pose3::transform_from, p); +} + // Projection typedef Expression Cal3_S2_; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index fce535d14..6308a1d61 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 SFM 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..e9449af7b --- /dev/null +++ b/timing/timeSFMBAL.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * 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 (strcmp(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 - 1] : 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, bool separateCalibration = false) { + 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)); + if (separateCalibration) ordering.push_back(K(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..4545abc21 --- /dev/null +++ b/timing/timeSFMBALautodiff.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFM 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 +// as to why so much gymnastics is needed to massage the initial estimates and +// measurements: basically, Snavely does not use computer vision conventions +// but OpenGL conventions :-( + +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 ! + 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) { + Vector3 v3 = track.p.vector(); + initial.insert(P(j++), v3); + } + + return optimize(db, graph, initial); +} diff --git a/timing/timeSFMBALcamTnav.cpp b/timing/timeSFMBALcamTnav.cpp new file mode 100644 index 000000000..32fae4ac2 --- /dev/null +++ b/timing/timeSFMBALcamTnav.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALcamTnav.cpp + * @brief time SFM with BAL file, expressions with camTnav pose + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, 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) { + size_t i = m.first; + Point2 z = m.second; + Pose3_ camTnav_(C(i)); + Cal3Bundler_ calibration_(K(i)); + Point3_ nav_point_(P(j)); + graph.addExpressionFactor( + gNoiseModel, z, + uncalibrate(calibration_, // now using transform_from !!!: + project(transform_from(camTnav_, nav_point_)))); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (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) + initial.insert(P(j++), track.p); + + bool separateCalibration = true; + return optimize(db, graph, initial, separateCalibration); +} diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp new file mode 100644 index 000000000..e370a5a67 --- /dev/null +++ b/timing/timeSFMBALnavTcam.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALnavTcam.cpp + * @brief time SFM with BAL file, expressions with navTcam pose + * @author Frank Dellaert + * @date July 5, 2015 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, 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) { + size_t i = m.first; + Point2 z = m.second; + Pose3_ navTcam_(C(i)); + Cal3Bundler_ calibration_(K(i)); + Point3_ nav_point_(P(j)); + graph.addExpressionFactor( + gNoiseModel, z, + uncalibrate(calibration_, + project(transform_to(navTcam_, nav_point_)))); + } + } + + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH (const SfM_Camera& camera, db.cameras) { + initial.insert(C(i), camera.pose()); + initial.insert(K(i), camera.calibration()); + i += 1; + } + BOOST_FOREACH (const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); + + bool separateCalibration = true; + return optimize(db, graph, initial, separateCalibration); +}