moved readBundler code from BundlerUtils to gtsam

gtsam/slam/dataset.cpp

@@ -50,6 +50,7 @@ string findExampleDataFile(const string& name) {
   namesToSearch.push_back(name);
   namesToSearch.push_back(name + ".graph");
   namesToSearch.push_back(name + ".txt");
+  namesToSearch.push_back(name + ".out");
 
   // Find first name that exists
   BOOST_FOREACH(const fs::path& root, rootsToSearch) {
@@ -407,4 +408,105 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
   return make_pair(graph, initial);
 }
 
+/* ************************************************************************* */
+bool readBundler(const string& cad, SfM_data &data)
+{
+    // Load the data file
+    ifstream is(cad.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 nposes, npoints;
+    is >> nposes >> npoints;
+
+    // Get the information for the camera poses
+    for( size_t i = 0; i < nposes; i++ )
+    {
+        // Get the focal length and the radial distortion parameters
+        float f, k1, k2;
+        is >> f >> k1 >> k2;
+        Cal3DS2 K(f, f, 0, 0, 0, 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;
+        }
+
+        // Our camera-to-world rotation matrix wRc' = [e1 -e2 -e3] * R
+        Rot3 cRw(
+             r11,  r12,  r13,
+            -r21, -r22, -r23,
+            -r31, -r32, -r33);
+        Rot3 wRc = cRw.inverse();
+
+        // Get the translation vector
+        float tx, ty, tz;
+        is >> tx >> ty >> tz;
+
+        // Our camera-to-world translation wTc = -R'*t
+        Pose3 pose(wRc, R.unrotate(Point3(-tx,-ty,-tz)));
+
+        data.cameras.push_back(SfM_Camera(pose,K));
+    }
+
+    // Get the information for the 3D points
+    for( size_t i = 0; i < npoints; i++ )
+    {
+        SfM_Track 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.0;
+        track.g = g/255.0;
+        track.b = b/255.0;
+
+        // Now get the visibility information
+        size_t nvisible = 0;
+        is >> nvisible;
+
+        for( size_t j = 0; j < nvisible; j++ )
+        {
+            size_t cam_idx = 0, sift_idx = 0;
+            float u, v;
+            is >> cam_idx >> sift_idx >> u >> v;
+            track.measurements.push_back(make_pair(cam_idx,Point2(u,-v)));
+        }
+
+        data.tracks.push_back(track);
+    }
+
+    is.close();
+    return true;
+}
+
 } // \namespace gtsam

gtsam/slam/dataset.h

@@ -20,13 +20,19 @@
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/linear/NoiseModel.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
+#include <vector>
+#include <utility> // for pair
 #include <string>
 
 namespace gtsam {
 
 #ifndef MATLAB_MEX_FILE
-  /**
+/**
  * Find the full path to an example dataset distributed with gtsam.  The name
  * may be specified with or without a file extension - if no extension is
  * give, this function first looks for the .graph extension, then .txt.  We
@@ -37,21 +43,21 @@ namespace gtsam {
  * @throw std::invalid_argument if no matching file could be found using the
  * search process described above.
  */
-  GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
+GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
 #endif
 
-  /**
+/**
  * Load TORO 2D Graph
  * @param dataset/model pair as constructed by [dataset]
  * @param maxID if non-zero cut out vertices >= maxID
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
  */
-  GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
     std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
     int maxID = 0, bool addNoise = false, bool smart = true);
 
-  /**
+/**
  * Load TORO 2D Graph
  * @param filename
  * @param model optional noise model to use instead of one specified by file
@@ -59,23 +65,57 @@ namespace gtsam {
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
  */
-  GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
     const std::string& filename,
     boost::optional<gtsam::SharedDiagonal> model = boost::optional<
     noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
     bool smart = true);
 
-  GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
+GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
     const std::string& filename,
     gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
 
-  /** save 2d graph */
+/** save 2d graph */
-  GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
+GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
     const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
 
-  /**
+/**
  * Load TORO 3D Graph
  */
-  GTSAM_EXPORT bool load3D(const std::string& filename);
+GTSAM_EXPORT bool load3D(const std::string& filename);
+
+/// A measurement with its camera index
+typedef std::pair<size_t,gtsam::Point2> SfM_Measurement;
+
+/// Define the structure for the 3D points
+struct SfM_Track
+{
+  gtsam::Point3 p; ///< 3D position of the point
+  float r,g,b; ///< RGB color of the 3D point
+  std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
+  size_t number_measurements() const { return measurements.size();}
+};
+
+/// Define the structure for the camera poses
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> SfM_Camera;
+
+/// Define the structure for SfM data
+struct SfM_data
+{
+  std::vector<SfM_Camera> cameras;    ///< Set of cameras
+  std::vector<SfM_Track> tracks; ///< Sparse set of points
+  size_t number_cameras() const { return cameras.size();}   ///< The number of camera poses
+  size_t number_tracks()  const { return tracks.size();}  ///< The number of reconstructed 3D points
+};
+
+/**
+ * @brief This function parses a bundler output file and stores the data into a
+ * SfM_data structure
+ * @param cad The name of the bundler file
+ * @param data SfM structure where the data is stored
+ * @return true if the parsing was successful, false otherwise
+ */
+bool readBundler(const std::string& cad, SfM_data &data);
+
 
 } // namespace gtsam

gtsam/slam/tests/testDataset.cpp

@@ -25,6 +25,7 @@
 using namespace std;
 using namespace gtsam;
 
+/* ************************************************************************* */
 TEST(dataSet, findExampleDataFile) {
   const string expected_end = "examples/Data/example.graph";
   const string actual = findExampleDataFile("example");
@@ -33,6 +34,31 @@ TEST(dataSet, findExampleDataFile) {
   EXPECT(assert_equal(expected_end, actual_end));
 }
 
+/* ************************************************************************* */
+TEST( dataSet, Balbianello)
+{
+  ///< The structure where we will save the SfM data
+  const string filename = findExampleDataFile("Balbianello");
+  SfM_data mydata;
+  CHECK(readBundler(filename, mydata));
+
+  // Check number of things
+  EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
+  EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
+  const SfM_Track& track0 = mydata.tracks[0];
+  EXPECT_LONGS_EQUAL(3,track0.number_measurements());
+
+  // Check projection of a given point
+  EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
+  const SfM_Camera& camera0 = mydata.cameras[0];
+  Point2 predicted = camera0.project(track0.p), measured = track0.measurements[0].second;
+  EXPECT(assert_equal(measured,predicted,1));
+
+  // Check projection-derived error 0.5*(du^2/0.25+dv^2/0.25)
+  double du = predicted.x() - measured.x(), dv = predicted.y() - measured.y();
+  EXPECT_DOUBLES_EQUAL(2.32894391, 2*du*du+2*dv*dv, 0.01);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */