Moved loading code

gtsam/sfm/SfmData.cpp

@@ -16,15 +16,27 @@
  * @brief Data structure for dealing with Structure from Motion data
  */
 
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/sfm/SfmData.h>
 
+#include <fstream>
+#include <iostream>
+
 namespace gtsam {
 
+using std::cout;
+using std::endl;
+
+using gtsam::symbol_shorthand::P;
+using gtsam::symbol_shorthand::X;
+
+/* ************************************************************************** */
 void SfmData::print(const std::string &s) const {
   std::cout << "Number of cameras = " << nrCameras() << std::endl;
   std::cout << "Number of tracks = " << nrTracks() << std::endl;
 }
 
+/* ************************************************************************** */
 bool SfmData::equals(const SfmData &sfmData, double tol) const {
   // check number of cameras and tracks
   if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) {
@@ -48,4 +60,349 @@ bool SfmData::equals(const SfmData& sfmData, double tol) const {
   return true;
 }
 
+/* ************************************************************************* */
+Rot3 openGLFixedRotation() {  // this is due to different convention for
+                              // cameras in gtsam and openGL
+  /* R = [ 1   0   0
+   *       0  -1   0
+   *       0   0  -1]
+   */
+  Matrix3 R_mat = Matrix3::Zero(3, 3);
+  R_mat(0, 0) = 1.0;
+  R_mat(1, 1) = -1.0;
+  R_mat(2, 2) = -1.0;
+  return Rot3(R_mat);
+}
+
+/* ************************************************************************* */
+Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
+  Rot3 R90 = openGLFixedRotation();
+  Rot3 wRc = (R.inverse()).compose(R90);
+
+  // Our camera-to-world translation wTc = -R'*t
+  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
+}
+
+/* ************************************************************************* */
+Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
+  Rot3 R90 = openGLFixedRotation();
+  Rot3 cRw_openGL = R90.compose(R.inverse());
+  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
+  return Pose3(cRw_openGL, t_openGL);
+}
+
+/* ************************************************************************* */
+Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
+  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
+                      PoseGTSAM.z());
+}
+
+/* ************************************************************************** */
+bool readBundler(const std::string &filename, SfmData &data) {
+  // 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;
+  }
+
+  // Ignore the first line
+  char aux[500];
+  is.getline(aux, 500);
+
+  // Get the number of camera poses and 3D points
+  size_t nrPoses, nrPoints;
+  is >> nrPoses >> nrPoints;
+
+  // Get the information for the camera poses
+  for (size_t i = 0; i < nrPoses; i++) {
+    // Get the focal length and the radial distortion parameters
+    float f, k1, k2;
+    is >> f >> k1 >> k2;
+    Cal3Bundler K(f, k1, k2);
+
+    // Get the rotation matrix
+    float r11, r12, r13;
+    float r21, r22, r23;
+    float r31, r32, r33;
+    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
+
+    // Bundler-OpenGL rotation matrix
+    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
+
+    // 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;
+    }
+
+    // Get the translation vector
+    float tx, ty, tz;
+    is >> tx >> ty >> tz;
+
+    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
+
+    data.cameras.emplace_back(pose, K);
+  }
+
+  // Get the information for the 3D points
+  data.tracks.reserve(nrPoints);
+  for (size_t j = 0; j < nrPoints; j++) {
+    SfmTrack track;
+
+    // Get the 3D position
+    float x, y, z;
+    is >> x >> y >> z;
+    track.p = Point3(x, y, z);
+
+    // Get the color information
+    float r, g, b;
+    is >> r >> g >> b;
+    track.r = r / 255.f;
+    track.g = g / 255.f;
+    track.b = b / 255.f;
+
+    // Now get the visibility information
+    size_t nvisible = 0;
+    is >> nvisible;
+
+    track.measurements.reserve(nvisible);
+    track.siftIndices.reserve(nvisible);
+    for (size_t k = 0; k < nvisible; k++) {
+      size_t cam_idx = 0, point_idx = 0;
+      float u, v;
+      is >> cam_idx >> point_idx >> u >> v;
+      track.measurements.emplace_back(cam_idx, Point2(u, -v));
+      track.siftIndices.emplace_back(cam_idx, point_idx);
+    }
+
+    data.tracks.push_back(track);
+  }
+
+  is.close();
+  return true;
+}
+
+/* ************************************************************************** */
+bool readBAL(const std::string &filename, SfmData &data) {
+  // 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;
+  }
+
+  // Get the number of camera poses and 3D points
+  size_t nrPoses, nrPoints, nrObservations;
+  is >> nrPoses >> nrPoints >> nrObservations;
+
+  data.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));
+  }
+
+  // Get the information for the camera poses
+  for (size_t i = 0; i < nrPoses; i++) {
+    // Get the Rodrigues vector
+    float wx, wy, wz;
+    is >> wx >> wy >> wz;
+    Rot3 R = Rot3::Rodrigues(wx, wy, wz);  // BAL-OpenGL rotation matrix
+
+    // Get the translation vector
+    float tx, ty, tz;
+    is >> tx >> ty >> tz;
+
+    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
+
+    // Get the focal length and the radial distortion parameters
+    float f, k1, k2;
+    is >> f >> k1 >> k2;
+    Cal3Bundler K(f, k1, k2);
+
+    data.cameras.emplace_back(pose, K);
+  }
+
+  // Get the information for the 3D points
+  for (size_t j = 0; j < nrPoints; j++) {
+    // Get the 3D position
+    float x, y, z;
+    is >> x >> y >> z;
+    SfmTrack &track = data.tracks[j];
+    track.p = Point3(x, y, z);
+    track.r = 0.4f;
+    track.g = 0.4f;
+    track.b = 0.4f;
+  }
+
+  is.close();
+  return true;
+}
+
+/* ************************************************************************** */
+SfmData readBal(const std::string &filename) {
+  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());
+  os.precision(20);
+  if (!os.is_open()) {
+    cout << "Error in writeBAL: can not open the file!!" << endl;
+    return false;
+  }
+
+  // Write the number of camera poses and 3D points
+  size_t nrObservations = 0;
+  for (size_t j = 0; j < data.nrTracks(); j++) {
+    nrObservations += data.tracks[j].nrMeasurements();
+  }
+
+  // Write observations
+  os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations
+     << endl;
+  os << endl;
+
+  for (size_t j = 0; j < data.nrTracks(); j++) {  // for each 3D point j
+    const SfmTrack &track = data.tracks[j];
+
+    for (size_t k = 0; k < track.nrMeasurements();
+         k++) {  // for each observation of the 3D point j
+      size_t i = track.measurements[k].first;  // camera id
+      double u0 = data.cameras[i].calibration().px();
+      double v0 = data.cameras[i].calibration().py();
+
+      if (u0 != 0 || v0 != 0) {
+        cout << "writeBAL has not been tested for calibration with nonzero "
+                "(u0,v0)"
+             << endl;
+      }
+
+      double pixelBALx = track.measurements[k].second.x() -
+                         u0;  // center of image is the origin
+      double pixelBALy = -(track.measurements[k].second.y() -
+                           v0);  // center of image is the origin
+      Point2 pixelMeasurement(pixelBALx, pixelBALy);
+      os << i /*camera id*/ << " " << j /*point id*/ << " "
+         << pixelMeasurement.x() /*u of the pixel*/ << " "
+         << pixelMeasurement.y() /*v of the pixel*/ << endl;
+    }
+  }
+  os << endl;
+
+  // Write cameras
+  for (size_t i = 0; i < data.nrCameras(); i++) {  // for each camera
+    Pose3 poseGTSAM = data.cameras[i].pose();
+    Cal3Bundler cameraCalibration = data.cameras[i].calibration();
+    Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
+    os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
+    os << poseOpenGL.translation().x() << endl;
+    os << poseOpenGL.translation().y() << endl;
+    os << poseOpenGL.translation().z() << endl;
+    os << cameraCalibration.fx() << endl;
+    os << cameraCalibration.k1() << endl;
+    os << cameraCalibration.k2() << endl;
+    os << endl;
+  }
+
+  // Write the points
+  for (size_t j = 0; j < data.nrTracks(); j++) {  // for each 3D point j
+    Point3 point = data.tracks[j].p;
+    os << point.x() << endl;
+    os << point.y() << endl;
+    os << point.z() << endl;
+    os << endl;
+  }
+
+  os.close();
+  return true;
+}
+
+/* ************************************************************************** */
+bool writeBALfromValues(const std::string &filename, const SfmData &data,
+                        Values &values) {
+  using Camera = PinholeCamera<Cal3Bundler>;
+  SfmData dataValues = data;
+
+  // Store poses or cameras in SfmData
+  size_t nrPoses = values.count<Pose3>();
+  if (nrPoses == dataValues.nrCameras()) {  // we only estimated camera poses
+    for (size_t i = 0; i < dataValues.nrCameras(); i++) {  // for each camera
+      Pose3 pose = values.at<Pose3>(X(i));
+      Cal3Bundler K = dataValues.cameras[i].calibration();
+      Camera camera(pose, K);
+      dataValues.cameras[i] = camera;
+    }
+  } else {
+    size_t nrCameras = values.count<Camera>();
+    if (nrCameras == dataValues.nrCameras()) {  // we only estimated camera
+                                                // poses and calibration
+      for (size_t i = 0; i < nrCameras; i++) {  // for each camera
+        Key cameraKey = i;                      // symbol('c',i);
+        Camera camera = values.at<Camera>(cameraKey);
+        dataValues.cameras[i] = camera;
+      }
+    } else {
+      cout << "writeBALfromValues: different number of cameras in "
+              "SfM_dataValues (#cameras "
+           << dataValues.nrCameras() << ") and values (#cameras " << nrPoses
+           << ", #poses " << nrCameras << ")!!" << endl;
+      return false;
+    }
+  }
+
+  // Store 3D points in SfmData
+  size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.nrTracks();
+  if (nrPoints != nrTracks) {
+    cout << "writeBALfromValues: different number of points in "
+            "SfM_dataValues (#points= "
+         << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
+  }
+
+  for (size_t j = 0; j < nrTracks; j++) {  // for each point
+    Key pointKey = P(j);
+    if (values.exists(pointKey)) {
+      Point3 point = values.at<Point3>(pointKey);
+      dataValues.tracks[j].p = point;
+    } else {
+      dataValues.tracks[j].r = 1.0;
+      dataValues.tracks[j].g = 0.0;
+      dataValues.tracks[j].b = 0.0;
+      dataValues.tracks[j].p = Point3(0, 0, 0);
+    }
+  }
+
+  // Write SfmData to file
+  return writeBAL(filename, dataValues);
+}
+
+/* ************************************************************************** */
+Values initialCamerasEstimate(const SfmData &db) {
+  Values initial;
+  size_t i = 0;  // NO POINTS:  j = 0;
+  for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
+  return initial;
+}
+
+/* ************************************************************************** */
+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 SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
+  return initial;
+}
+
+/* ************************************************************************** */
+
 }  // namespace gtsam

gtsam/sfm/SfmData.h

@@ -20,6 +20,7 @@
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/Values.h>
 #include <gtsam/sfm/SfmTrack.h>
 
 #include <string>
@@ -100,4 +101,95 @@ struct SfmData {
 template <>
 struct traits<SfmData> : public Testable<SfmData> {};
 
+/**
+ * @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);
+
+/**
+ * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and
+ * returns the data as a SfmData structure. Mainly used by wrapped code.
+ * @param filename The name of the BAL file.
+ * @return SfM structure where the data is stored.
+ */
+GTSAM_EXPORT SfmData readBal(const std::string& filename);
+
+/**
+ * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
+ * from a SfmData structure
+ * @param filename The name of the BAL file to write
+ * @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);
+
+/**
+ * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file
+ * from a SfmData structure and a value structure (measurements are the same as
+ * the SfM input data, while camera poses and values are read from Values)
+ * @param filename The name of the BAL file to write
+ * @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
+ * keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
+ * @return true if the parsing was successful, false otherwise
+ */
+GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
+                                     const SfmData& data, Values& values);
+
+/**
+ * @brief This function converts an openGL camera pose to an GTSAM camera pose
+ * @param R rotation in openGL
+ * @param tx x component of the translation in openGL
+ * @param ty y component of the translation in openGL
+ * @param tz z component of the translation in openGL
+ * @return Pose3 in GTSAM format
+ */
+GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
+
+/**
+ * @brief This function converts a GTSAM camera pose to an openGL camera pose
+ * @param R rotation in GTSAM
+ * @param tx x component of the translation in GTSAM
+ * @param ty y component of the translation in GTSAM
+ * @param tz z component of the translation in GTSAM
+ * @return Pose3 in openGL format
+ */
+GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
+
+/**
+ * @brief This function converts a GTSAM camera pose to an openGL camera pose
+ * @param PoseGTSAM pose in GTSAM format
+ * @return Pose3 in openGL format
+ */
+GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
+
+/**
+ * @brief This function creates initial values for cameras from db
+ * @param SfmData
+ * @return Values
+ */
+GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
+
+/**
+ * @brief This function creates initial values for cameras and points from db
+ * @param SfmData
+ * @return Values
+ */
+GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
+
 }  // namespace gtsam

gtsam/slam/dataset.cpp

@@ -54,8 +54,6 @@
 using namespace std;
 namespace fs = boost::filesystem;
 using gtsam::symbol_shorthand::L;
-using gtsam::symbol_shorthand::P;
-using gtsam::symbol_shorthand::X;
 
 #define LINESIZE 81920
 
@@ -945,352 +943,6 @@ GraphAndValues load3D(const string &filename) {
   return make_pair(graph, initial);
 }
 
-/* ************************************************************************* */
-Rot3 openGLFixedRotation() { // this is due to different convention for
-                             // cameras in gtsam and openGL
-  /* R = [ 1   0   0
-   *       0  -1   0
-   *       0   0  -1]
-   */
-  Matrix3 R_mat = Matrix3::Zero(3, 3);
-  R_mat(0, 0) = 1.0;
-  R_mat(1, 1) = -1.0;
-  R_mat(2, 2) = -1.0;
-  return Rot3(R_mat);
-}
-
-/* ************************************************************************* */
-Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) {
-  Rot3 R90 = openGLFixedRotation();
-  Rot3 wRc = (R.inverse()).compose(R90);
-
-  // Our camera-to-world translation wTc = -R'*t
-  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
-}
-
-/* ************************************************************************* */
-Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) {
-  Rot3 R90 = openGLFixedRotation();
-  Rot3 cRw_openGL = R90.compose(R.inverse());
-  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
-  return Pose3(cRw_openGL, t_openGL);
-}
-
-/* ************************************************************************* */
-Pose3 gtsam2openGL(const Pose3 &PoseGTSAM) {
-  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
-                      PoseGTSAM.z());
-}
-
-/* ************************************************************************* */
-bool readBundler(const string &filename, SfmData &data) {
-  // Load the data file
-  ifstream is(filename.c_str(), ifstream::in);
-  if (!is) {
-    cout << "Error in readBundler: can not find the file!!" << endl;
-    return false;
-  }
-
-  // Ignore the first line
-  char aux[500];
-  is.getline(aux, 500);
-
-  // Get the number of camera poses and 3D points
-  size_t nrPoses, nrPoints;
-  is >> nrPoses >> nrPoints;
-
-  // Get the information for the camera poses
-  for (size_t i = 0; i < nrPoses; i++) {
-    // Get the focal length and the radial distortion parameters
-    float f, k1, k2;
-    is >> f >> k1 >> k2;
-    Cal3Bundler K(f, k1, k2);
-
-    // Get the rotation matrix
-    float r11, r12, r13;
-    float r21, r22, r23;
-    float r31, r32, r33;
-    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
-
-    // Bundler-OpenGL rotation matrix
-    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
-
-    // 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;
-    }
-
-    // Get the translation vector
-    float tx, ty, tz;
-    is >> tx >> ty >> tz;
-
-    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
-
-    data.cameras.emplace_back(pose, K);
-  }
-
-  // Get the information for the 3D points
-  data.tracks.reserve(nrPoints);
-  for (size_t j = 0; j < nrPoints; j++) {
-    SfmTrack track;
-
-    // Get the 3D position
-    float x, y, z;
-    is >> x >> y >> z;
-    track.p = Point3(x, y, z);
-
-    // Get the color information
-    float r, g, b;
-    is >> r >> g >> b;
-    track.r = r / 255.f;
-    track.g = g / 255.f;
-    track.b = b / 255.f;
-
-    // Now get the visibility information
-    size_t nvisible = 0;
-    is >> nvisible;
-
-    track.measurements.reserve(nvisible);
-    track.siftIndices.reserve(nvisible);
-    for (size_t k = 0; k < nvisible; k++) {
-      size_t cam_idx = 0, point_idx = 0;
-      float u, v;
-      is >> cam_idx >> point_idx >> u >> v;
-      track.measurements.emplace_back(cam_idx, Point2(u, -v));
-      track.siftIndices.emplace_back(cam_idx, point_idx);
-    }
-
-    data.tracks.push_back(track);
-  }
-
-  is.close();
-  return true;
-}
-
-/* ************************************************************************* */
-bool readBAL(const string &filename, SfmData &data) {
-  // Load the data file
-  ifstream is(filename.c_str(), ifstream::in);
-  if (!is) {
-    cout << "Error in readBAL: can not find the file!!" << endl;
-    return false;
-  }
-
-  // Get the number of camera poses and 3D points
-  size_t nrPoses, nrPoints, nrObservations;
-  is >> nrPoses >> nrPoints >> nrObservations;
-
-  data.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));
-  }
-
-  // Get the information for the camera poses
-  for (size_t i = 0; i < nrPoses; i++) {
-    // Get the Rodrigues vector
-    float wx, wy, wz;
-    is >> wx >> wy >> wz;
-    Rot3 R = Rot3::Rodrigues(wx, wy, wz); // BAL-OpenGL rotation matrix
-
-    // Get the translation vector
-    float tx, ty, tz;
-    is >> tx >> ty >> tz;
-
-    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
-
-    // Get the focal length and the radial distortion parameters
-    float f, k1, k2;
-    is >> f >> k1 >> k2;
-    Cal3Bundler K(f, k1, k2);
-
-    data.cameras.emplace_back(pose, K);
-  }
-
-  // Get the information for the 3D points
-  for (size_t j = 0; j < nrPoints; j++) {
-    // Get the 3D position
-    float x, y, z;
-    is >> x >> y >> z;
-    SfmTrack &track = data.tracks[j];
-    track.p = Point3(x, y, z);
-    track.r = 0.4f;
-    track.g = 0.4f;
-    track.b = 0.4f;
-  }
-
-  is.close();
-  return true;
-}
-
-/* ************************************************************************* */
-SfmData readBal(const string &filename) {
-  SfmData data;
-  readBAL(filename, data);
-  return data;
-}
-
-/* ************************************************************************* */
-bool writeBAL(const string &filename, SfmData &data) {
-  // Open the output file
-  ofstream os;
-  os.open(filename.c_str());
-  os.precision(20);
-  if (!os.is_open()) {
-    cout << "Error in writeBAL: can not open the file!!" << endl;
-    return false;
-  }
-
-  // Write the number of camera poses and 3D points
-  size_t nrObservations = 0;
-  for (size_t j = 0; j < data.nrTracks(); j++) {
-    nrObservations += data.tracks[j].nrMeasurements();
-  }
-
-  // Write observations
-  os << data.nrCameras() << " " << data.nrTracks() << " "
-     << nrObservations << endl;
-  os << endl;
-
-  for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j
-    const SfmTrack &track = data.tracks[j];
-
-    for (size_t k = 0; k < track.nrMeasurements();
-         k++) { // for each observation of the 3D point j
-      size_t i = track.measurements[k].first; // camera id
-      double u0 = data.cameras[i].calibration().px();
-      double v0 = data.cameras[i].calibration().py();
-
-      if (u0 != 0 || v0 != 0) {
-        cout << "writeBAL has not been tested for calibration with nonzero "
-                "(u0,v0)"
-             << endl;
-      }
-
-      double pixelBALx = track.measurements[k].second.x() -
-                         u0; // center of image is the origin
-      double pixelBALy = -(track.measurements[k].second.y() -
-                           v0); // center of image is the origin
-      Point2 pixelMeasurement(pixelBALx, pixelBALy);
-      os << i /*camera id*/ << " " << j /*point id*/ << " "
-         << pixelMeasurement.x() /*u of the pixel*/ << " "
-         << pixelMeasurement.y() /*v of the pixel*/ << endl;
-    }
-  }
-  os << endl;
-
-  // Write cameras
-  for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera
-    Pose3 poseGTSAM = data.cameras[i].pose();
-    Cal3Bundler cameraCalibration = data.cameras[i].calibration();
-    Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
-    os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
-    os << poseOpenGL.translation().x() << endl;
-    os << poseOpenGL.translation().y() << endl;
-    os << poseOpenGL.translation().z() << endl;
-    os << cameraCalibration.fx() << endl;
-    os << cameraCalibration.k1() << endl;
-    os << cameraCalibration.k2() << endl;
-    os << endl;
-  }
-
-  // Write the points
-  for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j
-    Point3 point = data.tracks[j].p;
-    os << point.x() << endl;
-    os << point.y() << endl;
-    os << point.z() << endl;
-    os << endl;
-  }
-
-  os.close();
-  return true;
-}
-
-bool writeBALfromValues(const string &filename, const SfmData &data,
-                        Values &values) {
-  using Camera = PinholeCamera<Cal3Bundler>;
-  SfmData dataValues = data;
-
-  // Store poses or cameras in SfmData
-  size_t nrPoses = values.count<Pose3>();
-  if (nrPoses ==
-      dataValues.nrCameras()) { // we only estimated camera poses
-    for (size_t i = 0; i < dataValues.nrCameras();
-         i++) { // for each camera
-      Pose3 pose = values.at<Pose3>(X(i));
-      Cal3Bundler K = dataValues.cameras[i].calibration();
-      Camera camera(pose, K);
-      dataValues.cameras[i] = camera;
-    }
-  } else {
-    size_t nrCameras = values.count<Camera>();
-    if (nrCameras == dataValues.nrCameras()) { // we only estimated camera
-                                                    // poses and calibration
-      for (size_t i = 0; i < nrCameras; i++) {      // for each camera
-        Key cameraKey = i;                          // symbol('c',i);
-        Camera camera = values.at<Camera>(cameraKey);
-        dataValues.cameras[i] = camera;
-      }
-    } else {
-      cout << "writeBALfromValues: different number of cameras in "
-              "SfM_dataValues (#cameras "
-           << dataValues.nrCameras() << ") and values (#cameras "
-           << nrPoses << ", #poses " << nrCameras << ")!!" << endl;
-      return false;
-    }
-  }
-
-  // Store 3D points in SfmData
-  size_t nrPoints = values.count<Point3>(),
-         nrTracks = dataValues.nrTracks();
-  if (nrPoints != nrTracks) {
-    cout << "writeBALfromValues: different number of points in "
-            "SfM_dataValues (#points= "
-         << nrTracks << ") and values (#points " << nrPoints << ")!!" << endl;
-  }
-
-  for (size_t j = 0; j < nrTracks; j++) { // for each point
-    Key pointKey = P(j);
-    if (values.exists(pointKey)) {
-      Point3 point = values.at<Point3>(pointKey);
-      dataValues.tracks[j].p = point;
-    } else {
-      dataValues.tracks[j].r = 1.0;
-      dataValues.tracks[j].g = 0.0;
-      dataValues.tracks[j].b = 0.0;
-      dataValues.tracks[j].p = Point3(0, 0, 0);
-    }
-  }
-
-  // Write SfmData to file
-  return writeBAL(filename, dataValues);
-}
-
-Values initialCamerasEstimate(const SfmData &db) {
-  Values initial;
-  size_t i = 0; // NO POINTS:  j = 0;
-  for (const SfmCamera &camera : db.cameras)
-    initial.insert(i++, camera);
-  return initial;
-}
-
-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 SfmTrack &track : db.tracks)
-    initial.insert(P(j++), track.p);
-  return initial;
-}
-
 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
 BetweenFactorPose2s
 parse2DFactors(const std::string &filename,

gtsam/slam/dataset.h

@@ -209,96 +209,6 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
 /// Load TORO 3D Graph
 GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
 
-/**
- * @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);
-
-/**
- * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data
- * as a SfmData structure. Mainly used by wrapped code.
- * @param filename The name of the BAL file.
- * @return SfM structure where the data is stored.
- */
-GTSAM_EXPORT SfmData readBal(const std::string& filename);
-
-/**
- * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfmData structure
- * @param filename The name of the BAL file to write
- * @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);
-
-/**
- * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
- * SfmData structure and a value structure (measurements are the same as the SfM input data,
- * while camera poses and values are read from Values)
- * @param filename The name of the BAL file to write
- * @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 keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
- * @return true if the parsing was successful, false otherwise
- */
-GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
-    const SfmData &data, Values& values);
-
-/**
- * @brief This function converts an openGL camera pose to an GTSAM camera pose
- * @param R rotation in openGL
- * @param tx x component of the translation in openGL
- * @param ty y component of the translation in openGL
- * @param tz z component of the translation in openGL
- * @return Pose3 in GTSAM format
- */
-GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
-
-/**
- * @brief This function converts a GTSAM camera pose to an openGL camera pose
- * @param R rotation in GTSAM
- * @param tx x component of the translation in GTSAM
- * @param ty y component of the translation in GTSAM
- * @param tz z component of the translation in GTSAM
- * @return Pose3 in openGL format
- */
-GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
-
-/**
- * @brief This function converts a GTSAM camera pose to an openGL camera pose
- * @param PoseGTSAM pose in GTSAM format
- * @return Pose3 in openGL format
- */
-GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
-
-/**
- * @brief This function creates initial values for cameras from db
- * @param SfmData
- * @return Values
- */
-GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
-
-/**
- * @brief This function creates initial values for cameras and points from db
- * @param SfmData
- * @return Values
- */
-GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
-
 // Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
 using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
 GTSAM_EXPORT BetweenFactorPose2s