From 9dfe52d0b6109ed3c88e593d23e59d231bb564e1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 31 Jan 2022 12:29:13 -0500 Subject: [PATCH] Made reading static named constructors --- examples/SFMExampleExpressions_bal.cpp | 6 +- examples/SFMExample_bal.cpp | 6 +- examples/SFMExample_bal_COLAMD_METIS.cpp | 6 +- gtsam/sfm/SfmData.cpp | 111 +++++++++++++----- gtsam/sfm/SfmData.h | 73 ++++++++---- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/sfm.i | 3 + gtsam/sfm/tests/testSfmData.cpp | 52 ++++---- .../slam/tests/testEssentialMatrixFactor.cpp | 9 +- gtsam/slam/tests/testSerializationDataset.cpp | 7 +- tests/testGeneralSFMFactorB.cpp | 5 +- tests/testTranslationRecovery.cpp | 5 +- timing/timeSFMBAL.h | 6 +- timing/timeSFMBALautodiff.cpp | 2 +- 14 files changed, 189 insertions(+), 104 deletions(-) diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index 4fd6c1ada..3a02e6cab 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -28,7 +28,8 @@ // Header order is close to far #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include using namespace std; @@ -46,8 +47,7 @@ int main(int argc, char* argv[]) { if (argc > 1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 9d6e38a15..3ebfee6ba 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -20,7 +20,8 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include using namespace std; @@ -41,8 +42,7 @@ int main (int argc, char* argv[]) { if (argc>1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); // Create a factor graph diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index b59510c9f..4d04dd16e 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -22,7 +22,8 @@ #include #include #include -#include // for loading BAL datasets ! +#include // for loading BAL datasets ! +#include #include @@ -45,8 +46,7 @@ int main(int argc, char* argv[]) { if (argc > 1) filename = string(argv[1]); // Load the SfM data from file - SfmData mydata; - readBAL(filename, mydata); + SfmData mydata = SfmData::FromBalFile(filename); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 2f5396fcd..5c76b076a 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -99,14 +100,16 @@ Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) { } /* ************************************************************************** */ -bool readBundler(const std::string &filename, SfmData &data) { +SfmData SfmData::FromBundlerFile(const std::string &filename) { // Load the data file std::ifstream is(filename.c_str(), std::ifstream::in); if (!is) { - cout << "Error in readBundler: can not find the file!!" << endl; - return false; + throw std::runtime_error( + "Error in FromBundlerFile: can not find the file!!"); } + SfmData sfmData; + // Ignore the first line char aux[500]; is.getline(aux, 500); @@ -133,9 +136,8 @@ bool readBundler(const std::string &filename, SfmData &data) { // Check for all-zero R, in which case quit if (r11 == 0 && r12 == 0 && r13 == 0) { - cout << "Error in readBundler: zero rotation matrix for pose " << i - << endl; - return false; + throw std::runtime_error( + "Error in FromBundlerFile: zero rotation matrix"); } // Get the translation vector @@ -144,11 +146,11 @@ bool readBundler(const std::string &filename, SfmData &data) { Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.emplace_back(pose, K); + sfmData.cameras.emplace_back(pose, K); } // Get the information for the 3D points - data.tracks.reserve(nrPoints); + sfmData.tracks.reserve(nrPoints); for (size_t j = 0; j < nrPoints; j++) { SfmTrack track; @@ -178,34 +180,34 @@ bool readBundler(const std::string &filename, SfmData &data) { track.siftIndices.emplace_back(cam_idx, point_idx); } - data.tracks.push_back(track); + sfmData.tracks.push_back(track); } - is.close(); - return true; + return sfmData; } /* ************************************************************************** */ -bool readBAL(const std::string &filename, SfmData &data) { +SfmData SfmData::FromBalFile(const std::string &filename) { // Load the data file std::ifstream is(filename.c_str(), std::ifstream::in); if (!is) { - cout << "Error in readBAL: can not find the file!!" << endl; - return false; + throw std::runtime_error("Error in FromBalFile: can not find the file!!"); } + SfmData sfmData; + // Get the number of camera poses and 3D points size_t nrPoses, nrPoints, nrObservations; is >> nrPoses >> nrPoints >> nrObservations; - data.tracks.resize(nrPoints); + sfmData.tracks.resize(nrPoints); // Get the information for the observations for (size_t k = 0; k < nrObservations; k++) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); + sfmData.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -226,7 +228,7 @@ bool readBAL(const std::string &filename, SfmData &data) { is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.emplace_back(pose, K); + sfmData.cameras.emplace_back(pose, K); } // Get the information for the 3D points @@ -234,26 +236,18 @@ bool readBAL(const std::string &filename, SfmData &data) { // Get the 3D position float x, y, z; is >> x >> y >> z; - SfmTrack &track = data.tracks[j]; + SfmTrack &track = sfmData.tracks[j]; track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; track.b = 0.4f; } - is.close(); - return true; + return sfmData; } /* ************************************************************************** */ -SfmData readBal(const std::string &filename) { - SfmData data; - readBAL(filename, data); - return data; -} - -/* ************************************************************************** */ -bool writeBAL(const std::string &filename, SfmData &data) { +bool writeBAL(const std::string &filename, const SfmData &data) { // Open the output file std::ofstream os; os.open(filename.c_str()); @@ -329,9 +323,32 @@ bool writeBAL(const std::string &filename, SfmData &data) { return true; } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +bool readBundler(const std::string &filename, SfmData &data) { + try { + data = SfmData::FromBundlerFile(filename); + return true; + } catch (const std::exception & /* e */) { + return false; + } +} +bool readBAL(const std::string &filename, SfmData &data) { + try { + data = SfmData::FromBalFile(filename); + return true; + } catch (const std::exception & /* e */) { + return false; + } +} +#endif + +SfmData readBal(const std::string &filename) { + return SfmData::FromBalFile(filename); +} + /* ************************************************************************** */ bool writeBALfromValues(const std::string &filename, const SfmData &data, - Values &values) { + const Values &values) { using Camera = PinholeCamera; SfmData dataValues = data; @@ -387,6 +404,42 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data, return writeBAL(filename, dataValues); } +/* ************************************************************************** */ +NonlinearFactorGraph SfmData::generalSfmFactors( + const SharedNoiseModel &model) const { + using ProjectionFactor = GeneralSFMFactor; + NonlinearFactorGraph factors; + + size_t j = 0; + for (const SfmTrack &track : tracks) { + for (const SfmMeasurement &m : track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + factors.emplace_shared( + uv, model, X(i), P(j)); // note use of shorthand symbols X and P + } + j += 1; + } + + return factors; +} + +/* ************************************************************************** */ +NonlinearFactorGraph SfmData::sfmFactorGraph( + const SharedNoiseModel &model, boost::optional fixedCamera, + boost::optional fixedPoint) const { + using ProjectionFactor = GeneralSFMFactor; + NonlinearFactorGraph graph = generalSfmFactors(model); + using noiseModel::Constrained; + if (fixedCamera) { + graph.addPrior(X(*fixedCamera), cameras[0], Constrained::All(9)); + } + if (fixedPoint) { + graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3)); + } + return graph; +} + /* ************************************************************************** */ Values initialCamerasEstimate(const SfmData &db) { Values initial; diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index e2a6985f2..def20807a 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -35,11 +36,31 @@ typedef PinholeCamera SfmCamera; * @brief SfmData stores a bunch of SfmTracks * @addtogroup sfm */ -struct SfmData { +struct GTSAM_EXPORT SfmData { std::vector cameras; ///< Set of cameras std::vector tracks; ///< Sparse set of points + /// @name Create from file + /// @{ + + /** + * @brief Parses a bundler output file and return result as SfmData instance. + * @param filename The name of the bundler file + * @param data SfM structure where the data is stored + * @return true if the parsing was successful, false otherwise + */ + static SfmData FromBundlerFile(const std::string& filename); + + /** + * @brief Parse a "Bundle Adjustment in the Large" (BAL) file and return + * result as SfmData instance. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ + static SfmData FromBalFile(const std::string& filename); + + /// @} /// @name Standard Interface /// @{ @@ -61,6 +82,29 @@ struct SfmData { /// The camera pose at frame index `idx` SfmCamera camera(size_t idx) const { return cameras[idx]; } + /** + * @brief Create projection factors using keys X(i) and P(j) + * + * @param model a noise model for projection errors + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph generalSfmFactors( + const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, + 1.0)) const; + + /** + * @brief Create factor graph with projection factors and gauge fix. + * + * @param model a noise model for projection errors + * @param fixedCamera which camera to fix, if any (use boost::none if none) + * @param fixedPoint which point to fix, if any (use boost::none if none) + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph sfmFactorGraph( + const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0), + boost::optional fixedCamera = 0, + boost::optional fixedPoint = 0) const; + /// @} /// @name Testable /// @{ @@ -101,23 +145,12 @@ struct SfmData { template <> struct traits : public Testable {}; -/** - * @brief This function parses a bundler output file and stores the data into a - * SfmData structure - * @param filename The name of the bundler file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData& data); - -/** - * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and - * stores the data into a SfmData structure - * @param filename The name of the BAL file - * @param data SfM structure where the data is stored - * @return true if the parsing was successful, false otherwise - */ -GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData& data); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename, + SfmData& data); +GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename, + SfmData& data); +#endif /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and @@ -134,7 +167,7 @@ GTSAM_EXPORT SfmData readBal(const std::string& filename); * @param data SfM structure where the data is stored * @return true if the parsing was successful, false otherwise */ -GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data); +GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data); /** * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file @@ -149,7 +182,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data); * @return true if the parsing was successful, false otherwise */ GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, - const SfmData& data, Values& values); + const SfmData& data, const Values& values); /** * @brief This function converts an openGL camera pose to an GTSAM camera pose diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index ee9128d04..37731b32a 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -38,7 +38,7 @@ typedef std::pair SiftIndex; * @brief An SfmTrack stores SfM measurements grouped in a track * @addtogroup sfm */ -struct SfmTrack { +struct GTSAM_EXPORT SfmTrack { Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 14f183d16..ea229f51f 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -31,6 +31,9 @@ class SfmTrack { #include class SfmData { SfmData(); + gtsam::SfmData FromBundlerFile(string filename); + gtsam::SfmData FromBalFile(string filename); + size_t numberCameras() const; size_t numberTracks() const; gtsam::PinholeCamera camera(size_t idx) const; diff --git a/gtsam/sfm/tests/testSfmData.cpp b/gtsam/sfm/tests/testSfmData.cpp index bc1c8b645..a48e3e7f3 100644 --- a/gtsam/sfm/tests/testSfmData.cpp +++ b/gtsam/sfm/tests/testSfmData.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -33,41 +34,50 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); /* ************************************************************************* */ TEST(dataSet, Balbianello) { - ///< The structure where we will save the SfM data + // The structure where we will save the SfM data const string filename = findExampleDataFile("Balbianello"); - SfmData mydata; - CHECK(readBundler(filename, mydata)); + SfmData sfmData = SfmData::FromBundlerFile(filename); // Check number of things - EXPECT_LONGS_EQUAL(5, mydata.numberCameras()); - EXPECT_LONGS_EQUAL(544, mydata.numberTracks()); - const SfmTrack& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(5, sfmData.numberCameras()); + EXPECT_LONGS_EQUAL(544, sfmData.numberTracks()); + const SfmTrack& track0 = sfmData.tracks[0]; EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; + const SfmCamera& camera0 = sfmData.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected, actual, 1)); + + // We share *one* noiseModel between all projection factors + auto model = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Convert to NonlinearFactorGraph + NonlinearFactorGraph graph = sfmData.sfmFactorGraph(model); + EXPECT_LONGS_EQUAL(1419, graph.size()); // regression + + // Get initial estimate + Values values = initialCamerasAndPointsEstimate(sfmData); + EXPECT_LONGS_EQUAL(549, values.size()); // regression } /* ************************************************************************* */ TEST(dataSet, readBAL_Dubrovnik) { - ///< The structure where we will save the SfM data + // The structure where we will save the SfM data const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData sfmData = SfmData::FromBalFile(filename); // Check number of things - EXPECT_LONGS_EQUAL(3, mydata.numberCameras()); - EXPECT_LONGS_EQUAL(7, mydata.numberTracks()); - const SfmTrack& track0 = mydata.tracks[0]; + EXPECT_LONGS_EQUAL(3, sfmData.numberCameras()); + EXPECT_LONGS_EQUAL(7, sfmData.numberTracks()); + const SfmTrack& track0 = sfmData.tracks[0]; EXPECT_LONGS_EQUAL(3, track0.numberMeasurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0, track0.measurements[0].first); - const SfmCamera& camera0 = mydata.cameras[0]; + const SfmCamera& camera0 = sfmData.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected, actual, 12)); @@ -105,18 +115,15 @@ TEST(dataSet, gtsam2openGL) { /* ************************************************************************* */ TEST(dataSet, writeBAL_Dubrovnik) { - ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); + SfmData readData = SfmData::FromBalFile(filenameToRead); // Write readData to file filenameToWrite const string filenameToWrite = createRewrittenFileName(filenameToRead); CHECK(writeBAL(filenameToWrite, readData)); // Read what we wrote - SfmData writtenData; - CHECK(readBAL(filenameToWrite, writtenData)); + SfmData writtenData = SfmData::FromBalFile(filenameToWrite); // Check that what we read is the same as what we wrote EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras()); @@ -154,10 +161,8 @@ TEST(dataSet, writeBAL_Dubrovnik) { /* ************************************************************************* */ TEST(dataSet, writeBALfromValues_Dubrovnik) { - ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData readData; - readBAL(filenameToRead, readData); + SfmData readData = SfmData::FromBalFile(filenameToRead); Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3)); @@ -177,8 +182,7 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) { writeBALfromValues(filenameToWrite, readData, value); // Read the file we wrote - SfmData writtenData; - readBAL(filenameToWrite, writtenData); + SfmData writtenData = SfmData::FromBalFile(filenameToWrite); // Check that the reprojection errors are the same and the poses are correct // Check number of things diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 1a5c64c8c..ef22bad2a 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include using namespace std::placeholders; @@ -34,8 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); namespace example1 { const string filename = findExampleDataFile("18pointExample1.txt"); -SfmData data; -bool readOK = readBAL(filename, data); +SfmData data = SfmData::FromBalFile(filename); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); // TODO: maybe default value not good; assert with 0th @@ -53,8 +53,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } //************************************************************************* TEST(EssentialMatrixFactor, testData) { - CHECK(readOK); - // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; @@ -538,8 +536,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { namespace example2 { const string filename = findExampleDataFile("5pointExample2.txt"); -SfmData data; -bool readOK = readBAL(filename, data); +SfmData data = SfmData::FromBalFile(filename); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); diff --git a/gtsam/slam/tests/testSerializationDataset.cpp b/gtsam/slam/tests/testSerializationDataset.cpp index 6ef82f07f..dcac3d47e 100644 --- a/gtsam/slam/tests/testSerializationDataset.cpp +++ b/gtsam/slam/tests/testSerializationDataset.cpp @@ -16,6 +16,7 @@ * @date Jan 1, 2021 */ +#include #include #include @@ -29,8 +30,7 @@ using namespace gtsam::serializationTestHelpers; TEST(dataSet, sfmDataSerialization) { // Test the serialization of SfmData const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData mydata = SfmData::FromBalFile(filename); // round-trip equality check on serialization and subsequent deserialization EXPECT(equalsObj(mydata)); @@ -42,8 +42,7 @@ TEST(dataSet, sfmDataSerialization) { TEST(dataSet, sfmTrackSerialization) { // Test the serialization of SfmTrack const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData mydata; - CHECK(readBAL(filename, mydata)); + SfmData mydata = SfmData::FromBalFile(filename); SfmTrack track = mydata.track(0); diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 0bf609adc..fa27e1370 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -15,6 +15,7 @@ * @brief test general SFM class, with nonlinear optimization and BAL files */ +#include #include #include #include @@ -42,9 +43,7 @@ using symbol_shorthand::P; /* ************************************************************************* */ TEST(PinholeCamera, BAL) { string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData db; - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); + SfmData db = SfmData::FromBalFile(filename); SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; diff --git a/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp index 2915a375e..4fe643047 100644 --- a/tests/testTranslationRecovery.cpp +++ b/tests/testTranslationRecovery.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -42,9 +43,7 @@ Unit3 GetDirectionFromPoses(const Values& poses, // sets up an optimization problem for the three unknown translations. TEST(TranslationRecovery, BAL) { const string filename = findExampleDataFile("dubrovnik-3-7-pre"); - SfmData db; - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); + SfmData db = SfmData::FromBalFile(filename); // Get camera poses, as Values size_t j = 0; diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 347500cd2..79d7432c8 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -16,7 +16,7 @@ * @date July 5, 2015 */ -#include +#include #include #include #include @@ -54,9 +54,7 @@ SfmData preamble(int argc, char* argv[]) { filename = argv[argc - 1]; else filename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(filename, db); - if (!success) throw runtime_error("Could not access file!"); - return db; + return SfmData::FromBalFile(filename); } // Create ordering and optimize diff --git a/timing/timeSFMBALautodiff.cpp b/timing/timeSFMBALautodiff.cpp index 083d36729..1a7e35929 100644 --- a/timing/timeSFMBALautodiff.cpp +++ b/timing/timeSFMBALautodiff.cpp @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) { Values initial; size_t i = 0, j = 0; for (const SfmCamera& camera: db.cameras) { - // readBAL converts to GTSAM format, so we need to convert back ! + // SfmData::FromBalFile converts to GTSAM format, so we need to convert back ! Pose3 openGLpose = gtsam2openGL(camera.pose()); Vector9 v9; v9 << Pose3::Logmap(openGLpose), camera.calibration();