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.h b/timing/timeSFMBAL.h index e0f9d1458..e9449af7b 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -41,7 +41,7 @@ static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); SfM_data preamble(int argc, char* argv[]) { // primitive argument parsing: if (argc > 2) { - if (string(argv[1]) == "--colamd") + if (strcmp(argv[1], "--colamd")) gUseSchur = false; else throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]"); @@ -50,14 +50,14 @@ SfM_data preamble(int argc, char* argv[]) { // Load BAL file SfM_data db; string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(argc > 1 ? argv[argc] : defaultFilename, db); + 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) { + const Values& initial, bool separateCalibration = false) { using symbol_shorthand::P; // Set parameters to be similar to ceres @@ -69,7 +69,10 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, // 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)); + 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); } 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); +}