implemented read of BAL datasets and removed redundant code

gtsam/slam/dataset.cpp

@@ -62,10 +62,10 @@ string findExampleDataFile(const string& name) {
 
   // If we did not return already, then we did not find the file
   throw std::invalid_argument(
-    "gtsam::findExampleDataFile could not find a matching file in\n"
+      "gtsam::findExampleDataFile could not find a matching file in\n"
-    SOURCE_TREE_DATASET_DIR " or\n"
+      SOURCE_TREE_DATASET_DIR " or\n"
-    INSTALLED_DATASET_DIR " named\n" +
+      INSTALLED_DATASET_DIR " named\n" +
-    name + ", " + name + ".graph, or " + name + ".txt");
+      name + ", " + name + ".graph, or " + name + ".txt");
 }
 #endif
 
@@ -179,7 +179,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
         continue;
 
       noiseModel::Diagonal::shared_ptr measurementNoise =
-           noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
+          noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
       *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
 
       // Insert poses or points if they do not exist yet
@@ -217,7 +217,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
       {
         if(!haveLandmark) {
           cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
-            "non-uniform covariance on LANDMARK measurements in this file." << endl;
+              "non-uniform covariance on LANDMARK measurements in this file." << endl;
           haveLandmark = true;
         }
       }
@@ -229,7 +229,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
   }
 
   cout << "load2D read a graph file with " << initial->size()
-            << " vertices and " << graph->nrFactors() << " factors" << endl;
+                << " vertices and " << graph->nrFactors() << " factors" << endl;
 
   return make_pair(graph, initial);
 }
@@ -386,7 +386,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
         continue;
 
       noiseModel::Diagonal::shared_ptr measurementNoise =
-           noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
+          noiseModel::Diagonal::Sigmas(Vector_(2, bearing_std, range_std));
       *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
 
       // Insert poses or points if they do not exist yet
@@ -403,110 +403,183 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
   }
 
   cout << "load2D read a graph file with " << initial->size()
-            << " vertices and " << graph->nrFactors() << " factors" << endl;
+                << " vertices and " << graph->nrFactors() << " factors" << endl;
 
   return make_pair(graph, initial);
 }
 
 /* ************************************************************************* */
-bool readBundler(const string& cad, SfM_data &data)
+Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
 {
-    // Load the data file
+// Our camera-to-world rotation matrix wRc' = [e1 -e2 -e3] * R
-    ifstream is(cad.c_str(),ifstream::in);
+Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
-    if(!is)
+Rot3 cRw(
-    {
+    r1.x(),  r2.x(),  r3.x(),
-        cout << "Error in readBundler: can not find the file!!" << endl;
+   -r1.y(), -r2.y(), -r3.y(),
-        return false;
+    -r1.z(), -r2.z(), -r3.z());
-    }
+Rot3 wRc = cRw.inverse();
 
-    // Ignore the first line
+// Our camera-to-world translation wTc = -R'*t
-    char aux[500];
+return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz)));
-    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;
 }
 
+/* ************************************************************************* */
+bool readBundler(const string& filename, SfM_data &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.push_back(SfM_Camera(pose,K));
+  }
+
+  // Get the information for the 3D points
+  for( size_t j = 0; j < nrPoints; j++ )
+  {
+    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 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.push_back(make_pair(cam_idx,Point2(u,-v)));
+    }
+
+    data.tracks.push_back(track);
+  }
+
+  is.close();
+  return true;
+}
+
+/* ************************************************************************* */
+bool readBAL(const string& filename, SfM_data &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.push_back(make_pair(i,Point2(u,-v)));
+  }
+
+  // Get the information for the camera poses
+  for( size_t i = 0; i < nrPoses; i++ )
+  {
+    // Get the rodriguez vector
+    float wx, wy, wz;
+    is >> wx >> wy >> wz;
+    Rot3 R = Rot3::rodriguez(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.push_back(SfM_Camera(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;
+    SfM_Track& track = data.tracks[j];
+    track.p = Point3(x,y,z);
+    track.r = 0.4;
+    track.g = 0.4;
+    track.b = 0.4;
+  }
+
+  is.close();
+  return true;
+}
+
+
 } // \namespace gtsam

gtsam/slam/dataset.h

@@ -22,7 +22,7 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>
 
 #include <vector>
@@ -97,7 +97,7 @@ struct SfM_Track
 };
 
 /// Define the structure for the camera poses
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> SfM_Camera;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
 
 /// Define the structure for SfM data
 struct SfM_data
@@ -111,11 +111,19 @@ struct SfM_data
 /**
  * @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 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
  */
-bool readBundler(const std::string& cad, SfM_data &data);
+bool readBundler(const std::string& filename, SfM_data &data);
 
+/**
+ * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
+ * SfM_data 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
+ */
+bool readBAL(const std::string& filename, SfM_data &data);
 
 } // namespace gtsam

gtsam/slam/tests/testDataset.cpp

@@ -51,12 +51,29 @@ TEST( dataSet, Balbianello)
   // 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;
+  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
-  EXPECT(assert_equal(measured,predicted,1));
+  EXPECT(assert_equal(expected,actual,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();
+TEST( dataSet, Dubrovnik)
-  EXPECT_DOUBLES_EQUAL(2.32894391, 2*du*du+2*dv*dv, 0.01);
+{
+  ///< The structure where we will save the SfM data
+  const string filename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfM_data mydata;
+  CHECK(readBAL(filename, mydata));
+
+  // Check number of things
+  EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
+  EXPECT_LONGS_EQUAL(7,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 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
+  EXPECT(assert_equal(expected,actual,12));
 }
 
 /* ************************************************************************* */