Merge pull request #1082 from borglab/fix/sfm_reading

Cleaned up loading SfmData

examples/SFMExampleExpressions_bal.cpp

@@ -28,7 +28,8 @@
 // Header order is close to far
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
+#include <gtsam/sfm/SfmData.h>  // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>
 #include <vector>
 
 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;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") %
               mydata.numberTracks() % mydata.numberCameras();
 

examples/SFMExample_bal.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMExample.cpp
+ * @file    SFMExample_bal.cpp
  * @brief   Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file
  * @author  Frank Dellaert
  */
@@ -20,7 +20,8 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/sfm/SfmData.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>
 #include <vector>
 
 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;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
 
   // Create a factor graph

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -22,7 +22,8 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
+#include <gtsam/sfm/SfmData.h>  // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>
 
 #include <gtsam/base/timing.h>
 
@@ -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;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") %
               mydata.numberTracks() % mydata.numberCameras();
 

gtsam/navigation/ImuBias.cpp

@@ -66,8 +66,8 @@ namespace imuBias {
 //    }
 /// ostream operator
 std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
-  os << "acc = " << Point3(bias.accelerometer());
+  os << "acc = " << bias.accelerometer().transpose();
-  os << " gyro = " << Point3(bias.gyroscope());
+  os << " gyro = " << bias.gyroscope().transpose();
   return os;
 }
 

gtsam/sfm/SfmData.cpp

@@ -18,6 +18,7 @@
 
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/sfm/SfmData.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
 
 #include <fstream>
 #include <iostream>
@@ -28,7 +29,6 @@ using std::cout;
 using std::endl;
 
 using gtsam::symbol_shorthand::P;
-using gtsam::symbol_shorthand::X;
 
 /* ************************************************************************** */
 void SfmData::print(const std::string &s) const {
@@ -99,14 +99,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;
+    throw std::runtime_error(
-    return false;
+        "Error in FromBundlerFile: can not find the file!!");
   }
 
+  SfmData sfmData;
+
   // Ignore the first line
   char aux[500];
   is.getline(aux, 500);
@@ -133,9 +135,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
+      throw std::runtime_error(
-           << endl;
+          "Error in FromBundlerFile: zero rotation matrix");
-      return false;
     }
 
     // Get the translation vector
@@ -144,11 +145,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 +179,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 sfmData;
-  return true;
 }
 
 /* ************************************************************************** */
-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;
+    throw std::runtime_error("Error in FromBalFile: can not find the file!!");
-    return false;
   }
 
+  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 +227,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 +235,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 sfmData;
-  return true;
 }
 
 /* ************************************************************************** */
-SfmData readBal(const std::string &filename) {
+bool writeBAL(const std::string &filename, const SfmData &data) {
-  SfmData data;
-  readBAL(filename, data);
-  return data;
-}
-
-/* ************************************************************************** */
-bool writeBAL(const std::string &filename, SfmData &data) {
   // Open the output file
   std::ofstream os;
   os.open(filename.c_str());
@@ -329,9 +322,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<Cal3Bundler>;
   SfmData dataValues = data;
 
@@ -339,7 +355,7 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
   size_t nrPoses = values.count<Pose3>();
   if (nrPoses == dataValues.cameras.size()) {  // we only estimated camera poses
     for (size_t i = 0; i < dataValues.cameras.size(); i++) {  // for each camera
-      Pose3 pose = values.at<Pose3>(X(i));
+      Pose3 pose = values.at<Pose3>(i);
       Cal3Bundler K = dataValues.cameras[i].calibration();
       Camera camera(pose, K);
       dataValues.cameras[i] = camera;
@@ -387,6 +403,41 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
   return writeBAL(filename, dataValues);
 }
 
+/* ************************************************************************** */
+NonlinearFactorGraph SfmData::generalSfmFactors(
+    const SharedNoiseModel &model) const {
+  using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
+  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<ProjectionFactor>(uv, model, i, P(j));
+    }
+    j += 1;
+  }
+
+  return factors;
+}
+
+/* ************************************************************************** */
+NonlinearFactorGraph SfmData::sfmFactorGraph(
+    const SharedNoiseModel &model, boost::optional<size_t> fixedCamera,
+    boost::optional<size_t> fixedPoint) const {
+  using ProjectionFactor = GeneralSFMFactor<SfmCamera, Point3>;
+  NonlinearFactorGraph graph = generalSfmFactors(model);
+  using noiseModel::Constrained;
+  if (fixedCamera) {
+    graph.addPrior(*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;
@@ -399,7 +450,7 @@ Values initialCamerasEstimate(const SfmData &db) {
 Values initialCamerasAndPointsEstimate(const SfmData &db) {
   Values initial;
   size_t i = 0, j = 0;
-  for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera);
+  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
   for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
   return initial;
 }

gtsam/sfm/SfmData.h

@@ -20,6 +20,7 @@
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmTrack.h>
 
@@ -35,11 +36,31 @@ typedef PinholeCamera<Cal3Bundler> SfmCamera;
  * @brief SfmData stores a bunch of SfmTracks
  * @addtogroup sfm
  */
-struct SfmData {
+struct GTSAM_EXPORT SfmData {
   std::vector<SfmCamera> cameras;  ///< Set of cameras
 
   std::vector<SfmTrack> 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,31 @@ struct SfmData {
   /// The camera pose at frame index `idx`
   SfmCamera camera(size_t idx) const { return cameras[idx]; }
 
+  /**
+   * @brief Create projection factors using keys 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.
+   *
+   * Note: pose keys are simply integer indices, points use Symbol('p', j).
+   *
+   * @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<size_t> fixedCamera = 0,
+      boost::optional<size_t> fixedPoint = 0) const;
+
   /// @}
   /// @name Testable
   /// @{
@@ -101,23 +147,12 @@ struct SfmData {
 template <>
 struct traits<SfmData> : public Testable<SfmData> {};
 
-/**
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
- * @brief This function parses a bundler output file and stores the data into a
+GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
- * SfmData structure
+                                               SfmData& data);
- * @param filename The name of the bundler file
+GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
- * @param data SfM structure where the data is stored
+                                           SfmData& data);
- * @return true if the parsing was successful, false otherwise
+#endif
- */
-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);
 
 /**
  * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and
@@ -134,7 +169,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
@@ -144,12 +179,12 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfmData& data);
  * @param data SfM structure where the data is stored
  * @param values structure where the graph values are stored (values can be
  * either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be
- * Point3 for the 3D points). Note that the current version assumes that the
+ * Point3 for the 3D points). Note: assumes that the keys are "i" for pose i
- * keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
+ * and "Symbol::('p',j)" for landmark j.
  * @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
@@ -179,7 +214,10 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
 GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
 
 /**
- * @brief This function creates initial values for cameras from db
+ * @brief This function creates initial values for cameras from db.
+ *
+ * No symbol is used, so camera keys are simply integer indices.
+ *
  * @param SfmData
  * @return Values
  */
@@ -187,6 +225,9 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
 
 /**
  * @brief This function creates initial values for cameras and points from db
+ *
+ * Note: Pose keys are simply integer indices, points use Symbol('p', j).
+ *
  * @param SfmData
  * @return Values
  */

gtsam/sfm/SfmTrack.h

@@ -38,7 +38,7 @@ typedef std::pair<size_t, size_t> 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
 

gtsam/sfm/sfm.i

@@ -31,12 +31,23 @@ class SfmTrack {
 #include <gtsam/sfm/SfmData.h>
 class SfmData {
   SfmData();
-  size_t numberCameras() const;
+  static gtsam::SfmData FromBundlerFile(string filename);
-  size_t numberTracks() const;
+  static gtsam::SfmData FromBalFile(string filename);
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+
-  gtsam::SfmTrack track(size_t idx) const;
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
+  size_t numberTracks() const;
+  size_t numberCameras() const;
+  gtsam::SfmTrack track(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+
+  gtsam::NonlinearFactorGraph generalSfmFactors(
+      const gtsam::SharedNoiseModel& model =
+          gtsam::noiseModel::Isotropic::Sigma(2, 1.0)) const;
+  gtsam::NonlinearFactorGraph sfmFactorGraph(
+      const gtsam::SharedNoiseModel& model =
+          gtsam::noiseModel::Isotropic::Sigma(2, 1.0),
+      size_t fixedCamera = 0, size_t fixedPoint = 0) const;
 
   // enabling serialization functionality
   void serialize() const;

gtsam/sfm/tests/testSfmData.cpp

@@ -18,13 +18,13 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/sfm/SfmData.h>
 
 using namespace std;
 using namespace gtsam;
 
 using gtsam::symbol_shorthand::P;
-using gtsam::symbol_shorthand::X;
 
 namespace gtsam {
 GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
@@ -33,41 +33,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;
+  SfmData sfmData = SfmData::FromBundlerFile(filename);
-  CHECK(readBundler(filename, mydata));
 
   // Check number of things
-  EXPECT_LONGS_EQUAL(5, mydata.numberCameras());
+  EXPECT_LONGS_EQUAL(5, sfmData.numberCameras());
-  EXPECT_LONGS_EQUAL(544, mydata.numberTracks());
+  EXPECT_LONGS_EQUAL(544, sfmData.numberTracks());
-  const SfmTrack& track0 = mydata.tracks[0];
+  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;
+  SfmData sfmData = SfmData::FromBalFile(filename);
-  CHECK(readBAL(filename, mydata));
 
   // Check number of things
-  EXPECT_LONGS_EQUAL(3, mydata.numberCameras());
+  EXPECT_LONGS_EQUAL(3, sfmData.numberCameras());
-  EXPECT_LONGS_EQUAL(7, mydata.numberTracks());
+  EXPECT_LONGS_EQUAL(7, sfmData.numberTracks());
-  const SfmTrack& track0 = mydata.tracks[0];
+  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 +114,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;
+  SfmData readData = SfmData::FromBalFile(filenameToRead);
-  readBAL(filenameToRead, readData);
 
   // Write readData to file filenameToWrite
   const string filenameToWrite = createRewrittenFileName(filenameToRead);
   CHECK(writeBAL(filenameToWrite, readData));
 
   // Read what we wrote
-  SfmData writtenData;
+  SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
-  CHECK(readBAL(filenameToWrite, writtenData));
 
   // Check that what we read is the same as what we wrote
   EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
@@ -154,31 +160,28 @@ 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;
+  SfmData readData = SfmData::FromBalFile(filenameToRead);
-  readBAL(filenameToRead, readData);
 
   Pose3 poseChange =
       Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
 
-  Values value;
+  Values values;
   for (size_t i = 0; i < readData.numberCameras(); i++) {  // for each camera
     Pose3 pose = poseChange.compose(readData.cameras[i].pose());
-    value.insert(X(i), pose);
+    values.insert(i, pose);
   }
   for (size_t j = 0; j < readData.numberTracks(); j++) {  // for each point
     Point3 point = poseChange.transformFrom(readData.tracks[j].p);
-    value.insert(P(j), point);
+    values.insert(P(j), point);
   }
 
   // Write values and readData to a file
   const string filenameToWrite = createRewrittenFileName(filenameToRead);
-  writeBALfromValues(filenameToWrite, readData, value);
+  writeBALfromValues(filenameToWrite, readData, values);
 
   // Read the file we wrote
-  SfmData writtenData;
+  SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
-  readBAL(filenameToWrite, writtenData);
 
   // Check that the reprojection errors are the same and the poses are correct
   // Check number of things
@@ -195,11 +198,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
   EXPECT(assert_equal(expected, actual, 12));
 
   Pose3 expectedPose = camera0.pose();
-  Pose3 actualPose = value.at<Pose3>(X(0));
+  Pose3 actualPose = values.at<Pose3>(0);
   EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
 
   Point3 expectedPoint = track0.p;
-  Point3 actualPoint = value.at<Point3>(P(0));
+  Point3 actualPoint = values.at<Point3>(P(0));
   EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
 }
 

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -15,6 +15,7 @@
 #include <gtsam/nonlinear/expressionTesting.h>
 #include <gtsam/nonlinear/factorTesting.h>
 #include <gtsam/slam/EssentialMatrixFactor.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 
 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;
+SfmData data = SfmData::FromBalFile(filename);
-bool readOK = readBAL(filename, data);
 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;
+SfmData data = SfmData::FromBalFile(filename);
-bool readOK = readBAL(filename, data);
 Rot3 aRb = data.cameras[1].pose().rotation();
 Point3 aTb = data.cameras[1].pose().translation();
 EssentialMatrix trueE(aRb, Unit3(aTb));

gtsam/slam/tests/testSerializationDataset.cpp

@@ -16,6 +16,7 @@
  * @date Jan 1, 2021
  */
 
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 
 #include <gtsam/base/serializationTestHelpers.h>
@@ -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;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  CHECK(readBAL(filename, mydata));
 
   // 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;
+  SfmData mydata = SfmData::FromBalFile(filename);
-  CHECK(readBAL(filename, mydata));
 
   SfmTrack track = mydata.track(0);
 

python/gtsam/examples/SFMExample_bal.py

@@ -15,9 +15,9 @@ import logging
 import sys
 
 import gtsam
-from gtsam import (GeneralSFMFactorCal3Bundler,
+from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
                    PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
-from gtsam.symbol_shorthand import C, P  # type: ignore
+from gtsam.symbol_shorthand import P  # type: ignore
 from gtsam.utils import plot  # type: ignore
 from matplotlib import pyplot as plt
 
@@ -26,12 +26,11 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO)
 DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
 
 
-def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
+def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
     """Plot the SFM results."""
     plot_vals = gtsam.Values()
-    for cam_idx in range(scene_data.numberCameras()):
+    for i in range(scene_data.numberCameras()):
-        plot_vals.insert(C(cam_idx),
+        plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
-                         result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
     for j in range(scene_data.numberTracks()):
         plot_vals.insert(P(j), result.atPoint3(P(j)))
 
@@ -46,7 +45,7 @@ def run(args: argparse.Namespace) -> None:
     input_file = args.input_file
 
     # Load the SfM data from file
-    scene_data = gtsam.readBal(input_file)
+    scene_data = SfmData.FromBalFile(input_file)
     logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
                  scene_data.numberCameras())
 
@@ -64,12 +63,12 @@ def run(args: argparse.Namespace) -> None:
             # i represents the camera index, and uv is the 2d measurement
             i, uv = track.measurement(m_idx)
             # note use of shorthand symbols C and P
-            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
+            graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
 
     # Add a prior on pose x1. This indirectly specifies where the origin is.
     graph.push_back(
         PriorFactorPinholeCameraCal3Bundler(
-            C(0), scene_data.camera(0),
+            0, scene_data.camera(0),
             gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
     # Also add a prior on the position of the first landmark to fix the scale
     graph.push_back(
@@ -82,9 +81,9 @@ def run(args: argparse.Namespace) -> None:
 
     i = 0
     # add each PinholeCameraCal3Bundler
-    for cam_idx in range(scene_data.numberCameras()):
+    for i in range(scene_data.numberCameras()):
-        camera = scene_data.camera(cam_idx)
+        camera = scene_data.camera(i)
-        initial.insert(C(i), camera)
+        initial.insert(i, camera)
         i += 1
 
     # add each SfmTrack

python/gtsam/examples/SimpleRotation.py

@@ -31,7 +31,7 @@ def main():
     - A measurement model with the correct dimensionality for the factor
     """
     prior = gtsam.Rot2.fromAngle(np.deg2rad(30))
-    prior.print_('goal angle')
+    prior.print('goal angle')
     model = gtsam.noiseModel.Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
     key = X(1)
     factor = gtsam.PriorFactorRot2(key, prior, model)
@@ -48,7 +48,7 @@ def main():
     """
     graph = gtsam.NonlinearFactorGraph()
     graph.push_back(factor)
-    graph.print_('full graph')
+    graph.print('full graph')
 
     """
     Step 3: Create an initial estimate
@@ -65,7 +65,7 @@ def main():
     """
     initial = gtsam.Values()
     initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20)))
-    initial.print_('initial estimate')
+    initial.print('initial estimate')
 
     """
     Step 4: Optimize
@@ -77,7 +77,7 @@ def main():
     with the final state of the optimization.
     """
     result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
-    result.print_('final result')
+    result.print('final result')
 
 
 if __name__ == '__main__':

python/gtsam/examples/VisualISAMExample.py

@@ -10,8 +10,6 @@ A visualSLAM example for the structure-from-motion problem on a simulated datase
 This version uses iSAM to solve the problem incrementally
 """
 
-from __future__ import print_function
-
 import numpy as np
 import gtsam
 from gtsam.examples import SFMdata
@@ -94,7 +92,7 @@ def main():
             current_estimate = isam.estimate()
             print('*' * 50)
             print('Frame {}:'.format(i))
-            current_estimate.print_('Current estimate: ')
+            current_estimate.print('Current estimate: ')
 
             # Clear the factor graph and values for the next iteration
             graph.resize(0)

python/gtsam/tests/test_SfmData.py

@@ -14,9 +14,9 @@ from __future__ import print_function
 
 import unittest
 
-import numpy as np
-
 import gtsam
+import numpy as np
+from gtsam import Point2, Point3, SfmData, SfmTrack
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -25,55 +25,97 @@ class TestSfmData(GtsamTestCase):
 
     def setUp(self):
         """Initialize SfmData and SfmTrack"""
-        self.data = gtsam.SfmData()
+        self.data = SfmData()
         # initialize SfmTrack with 3D point
-        self.tracks = gtsam.SfmTrack()
+        self.tracks = SfmTrack()
 
     def test_tracks(self):
         """Test functions in SfmTrack"""
         # measurement is of format (camera_idx, imgPoint)
         # create arbitrary camera indices for two cameras
-        i1, i2 = 4,5
+        i1, i2 = 4, 5
+
         # create arbitrary image measurements for cameras i1 and i2
-        uv_i1 = gtsam.Point2(12.6, 82)
+        uv_i1 = Point2(12.6, 82)
+
         # translating point uv_i1 along X-axis
-        uv_i2 = gtsam.Point2(24.88, 82)
+        uv_i2 = Point2(24.88, 82)
+
         # add measurements to the track
         self.tracks.addMeasurement(i1, uv_i1)
         self.tracks.addMeasurement(i2, uv_i2)
+
         # Number of measurements in the track is 2
         self.assertEqual(self.tracks.numberMeasurements(), 2)
+
         # camera_idx in the first measurement of the track corresponds to i1
         cam_idx, img_measurement = self.tracks.measurement(0)
         self.assertEqual(cam_idx, i1)
         np.testing.assert_array_almost_equal(
-            gtsam.Point3(0.,0.,0.), 
+            Point3(0., 0., 0.),
             self.tracks.point3()
         )
 
-
     def test_data(self):
         """Test functions in SfmData"""
         # Create new track with 3 measurements
-        i1, i2, i3 = 3,5,6
+        i1, i2, i3 = 3, 5, 6
-        uv_i1 = gtsam.Point2(21.23, 45.64)
+        uv_i1 = Point2(21.23, 45.64)
+
         # translating along X-axis
-        uv_i2 = gtsam.Point2(45.7, 45.64)
+        uv_i2 = Point2(45.7, 45.64)
-        uv_i3 = gtsam.Point2(68.35, 45.64)
+        uv_i3 = Point2(68.35, 45.64)
+
         # add measurements and arbitrary point to the track
         measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
-        pt = gtsam.Point3(1.0, 6.0, 2.0)
+        pt = Point3(1.0, 6.0, 2.0)
-        track2 = gtsam.SfmTrack(pt)
+        track2 = SfmTrack(pt)
         track2.addMeasurement(i1, uv_i1)
         track2.addMeasurement(i2, uv_i2)
         track2.addMeasurement(i3, uv_i3)
         self.data.addTrack(self.tracks)
         self.data.addTrack(track2)
+
         # Number of tracks in SfmData is 2
         self.assertEqual(self.data.numberTracks(), 2)
+
         # camera idx of first measurement of second track corresponds to i1
         cam_idx, img_measurement = self.data.track(1).measurement(0)
         self.assertEqual(cam_idx, i1)
 
+    def test_Balbianello(self):
+        """ Check that we can successfully read a bundler file and create a 
+            factor graph from it
+        """
+        # The structure where we will save the SfM data
+        filename = gtsam.findExampleDataFile("Balbianello.out")
+        sfm_data = SfmData.FromBundlerFile(filename)
+
+        # Check number of things
+        self.assertEqual(5, sfm_data.numberCameras())
+        self.assertEqual(544, sfm_data.numberTracks())
+        track0 = sfm_data.track(0)
+        self.assertEqual(3, track0.numberMeasurements())
+
+        # Check projection of a given point
+        self.assertEqual(0, track0.measurement(0)[0])
+        camera0 = sfm_data.camera(0)
+        expected = camera0.project(track0.point3())
+        actual = track0.measurement(0)[1]
+        self.gtsamAssertEquals(expected, actual, 1)
+
+        # We share *one* noiseModel between all projection factors
+        model = gtsam.noiseModel.Isotropic.Sigma(
+            2, 1.0)  # one pixel in u and v
+
+        # Convert to NonlinearFactorGraph
+        graph = sfm_data.sfmFactorGraph(model)
+        self.assertEqual(1419, graph.size())  # regression
+
+        # Get initial estimate
+        values = gtsam.initialCamerasAndPointsEstimate(sfm_data)
+        self.assertEqual(549, values.size())  # regression
+
+
 if __name__ == '__main__':
     unittest.main()

python/gtsam/utils/visual_data_generator.py

@@ -1,12 +1,12 @@
 from __future__ import print_function
-from typing import Tuple
 
 import math
-import numpy as np
 from math import pi
+from typing import Tuple
 
 import gtsam
-from gtsam import Point3, Pose3, PinholeCameraCal3_S2, Cal3_S2
+import numpy as np
+from gtsam import Cal3_S2, PinholeCameraCal3_S2, Point3, Pose3
 
 
 class Options:
@@ -36,7 +36,7 @@ class GroundTruth:
         self.cameras = [Pose3()] * nrCameras
         self.points = [Point3(0, 0, 0)] * nrPoints
 
-    def print_(self, s="") -> None:
+    def print(self, s: str = "") -> None:
         print(s)
         print("K = ", self.K)
         print("Cameras: ", len(self.cameras))
@@ -88,7 +88,8 @@ def generate_data(options) -> Tuple[Data, GroundTruth]:
         r = 10
         for j in range(len(truth.points)):
             theta = j * 2 * pi / nrPoints
-            truth.points[j] = Point3(r * math.cos(theta), r * math.sin(theta), 0)
+            truth.points[j] = Point3(
+                r * math.cos(theta), r * math.sin(theta), 0)
     else:  # 3D landmarks as vertices of a cube
         truth.points = [
             Point3(10, 10, 10), Point3(-10, 10, 10),

tests/testGeneralSFMFactorB.cpp

@@ -15,6 +15,7 @@
  * @brief test general SFM class, with nonlinear optimization and BAL files
  */
 
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/geometry/Point3.h>
@@ -42,9 +43,7 @@ using symbol_shorthand::P;
 /* ************************************************************************* */
 TEST(PinholeCamera, BAL) {
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  SfmData db;
+  SfmData db = SfmData::FromBalFile(filename);
-  bool success = readBAL(filename, db);
-  if (!success) throw runtime_error("Could not access file!");
 
   SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
   NonlinearFactorGraph graph;

tests/testTranslationRecovery.cpp

@@ -18,6 +18,7 @@
 
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/sfm/TranslationRecovery.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/slam/dataset.h>
 
 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;
+  SfmData db = SfmData::FromBalFile(filename);
-  bool success = readBAL(filename, db);
-  if (!success) throw runtime_error("Could not access file!");
 
   // Get camera poses, as Values
   size_t j = 0;

timing/timeSFMBAL.h

@@ -16,7 +16,7 @@
  * @date    July 5, 2015
  */
 
-#include <gtsam/slam/dataset.h>
+#include <gtsam/sfm/SfmData.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
@@ -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);
+  return SfmData::FromBalFile(filename);
-  if (!success) throw runtime_error("Could not access file!");
-  return db;
 }
 
 // Create ordering and optimize

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();