diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 9fa9e3468..0e610d8d6 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -21,10 +21,6 @@ #include #include #include -#include -#include -#include -#include #include @@ -325,34 +321,6 @@ TEST( PinholeCamera, range3) { EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } -/* ************************************************************************* */ -typedef GeneralSFMFactor sfmFactor; -using symbol_shorthand::P; - -/* ************************************************************************* */ -TEST( PinholeCamera, BAL) { - string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfM_data db; - bool success = readBAL(filename, 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(); - double actualError = graph.error(actual); - EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp new file mode 100644 index 000000000..d43e7e31a --- /dev/null +++ b/tests/testGeneralSFMFactorB.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGeneralSFMFactorB.cpp + * @author Frank Dellaert + * @brief test general SFM class, with nonlinear optimization and BAL files + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef GeneralSFMFactor, Point3> sfmFactor; +using symbol_shorthand::P; + +/* ************************************************************************* */ +TEST(PinholeCamera, BAL) { + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data db; + bool success = readBAL(filename, 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(); + double actualError = graph.error(actual); + EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-7); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */