added functionalities in dataset.cpp (writeBALfromValues) and BAL example

gtsam/slam/dataset.cpp

@@ -12,7 +12,7 @@
 /**
  * @file dataset.cpp
  * @date Jan 22, 2010
- * @author nikai
+ * @author nikai, Luca Carlone
  * @brief utility functions for loading datasets
  */
 
@@ -605,7 +605,7 @@ bool readBAL(const string& filename, SfM_data &data)
 /* ************************************************************************* */
 bool writeBAL(const string& filename, SfM_data &data)
 {
-  // Load the data file
+  // Open the output file
   ofstream os;
   os.open(filename.c_str());
   os.precision(20);
@@ -614,6 +614,8 @@ bool writeBAL(const string& filename, SfM_data &data)
     return false;
   }
 
+  cout << "writeBAL assumes the origin of the camera frame to coincide with camera center!!" << endl;
+
   // Write the number of camera poses and 3D points
   int nrObservations=0;
   for (size_t j = 0; j < data.number_tracks(); j++){
@@ -660,5 +662,46 @@ bool writeBAL(const string& filename, SfM_data &data)
   return true;
 }
 
+bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
+
+  // CHECKS
+  Values valuesPoses = values.filter<Pose3>();
+  if( valuesPoses.size() != data.number_cameras()){
+    cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
+        <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl;
+    return false;
+  }
+  Values valuesPoints = values.filter<Point3>();
+  if( valuesPoints.size() != data.number_tracks()){
+    cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks()
+        <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
+    return false;
+  }
+  if(valuesPoints.size() + valuesPoses.size() != values.size()){
+    cout << "writeBALfromValues write only poses and points values!!" << endl;
+    return false;
+  }
+  if(valuesPoints.size()==0 || valuesPoses.size()==0){
+    cout << "writeBALfromValues: No point or pose in values!!" << endl;
+    return false;
+  }
+
+  for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
+    Key poseKey = symbol('x',i);
+    Pose3 pose = values.at<Pose3>(poseKey);
+    Cal3Bundler K = data.cameras[i].calibration();
+    PinholeCamera<Cal3Bundler> camera(pose, K);
+    data.cameras[i] = camera;
+  }
+
+  for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
+    Key pointKey = symbol('l',j);
+    Point3 point = values.at<Point3>(pointKey);
+    data.tracks[j].p = point;
+  }
+
+  return writeBAL(filename, data);
+}
+
 
 } // \namespace gtsam

gtsam/slam/dataset.h

@@ -12,7 +12,7 @@
 /**
  * @file dataset.h
  * @date Jan 22, 2010
- * @author nikai
+ * @author nikai, Luca Carlone
  * @brief utility functions for loading datasets
  */
 
@@ -135,6 +135,17 @@ bool readBAL(const std::string& filename, SfM_data &data);
  */
 bool writeBAL(const std::string& filename, SfM_data &data);
 
+/**
+ * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
+ * SfM_data 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
+ * @return true if the parsing was successful, false otherwise
+ */
+bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values);
+
 /**
  * @brief This function converts an openGL camera pose to an GTSAM camera pose
  * @param R rotation in openGL

gtsam/slam/tests/testDataset.cpp

@@ -12,7 +12,7 @@
 /**
  * @file    testDataset.cpp
  * @brief   Unit test for dataset.cpp
- * @author  Richard Roberts
+ * @author  Richard Roberts, Luca Carlone
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -20,6 +20,7 @@
 #include <boost/algorithm/string.hpp>
 
 #include <gtsam/base/TestableAssertions.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
 
 using namespace std;
@@ -116,7 +117,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
   ///< Read a file using the unit tested readBAL
   const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
   SfM_data readData;
-  CHECK(readBAL(filenameToRead, readData));
+  readBAL(filenameToRead, readData);
 
   // Write readData to file filenameToWrite
   const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
@@ -157,6 +158,62 @@ TEST( dataSet, writeBAL_Dubrovnik)
   }
 }
 
+
+/* ************************************************************************* */
+TEST( dataSet, writeBALfromValues_Dubrovnik){
+
+  ///< Read a file using the unit tested readBAL
+  const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre");
+  SfM_data readData;
+  readBAL(filenameToRead, readData);
+
+  Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
+
+  Values value;
+  for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
+    Key poseKey = symbol('x',i);
+    Pose3 pose = poseChange.compose(readData.cameras[i].pose());
+    value.insert(poseKey, pose);
+  }
+  for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
+    Key pointKey = symbol('l',j);
+    Point3 point = poseChange.transform_from( readData.tracks[j].p );
+    value.insert(pointKey, point);
+  }
+
+  // Write values and readData to a file
+  const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
+  writeBALfromValues(filenameToWrite, readData, value);
+
+  // Read the file we wrote
+  SfM_data writtenData;
+  readBAL(filenameToWrite, writtenData);
+
+  // Check that the reprojection errors are the same and the poses are correct
+  // Check number of things
+  EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
+  EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
+  const SfM_Track& track0 = writtenData.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 = writtenData.cameras[0];
+  Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second;
+  EXPECT(assert_equal(expected,actual,12));
+
+  Pose3 expectedPose = camera0.pose();
+  Key poseKey = symbol('x',0);
+  Pose3 actualPose = value.at<Pose3>(poseKey);
+  EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
+
+  Point3 expectedPoint = track0.p;
+  Key pointKey = symbol('l',0);
+  Point3 actualPoint = value.at<Point3>(pointKey);
+  EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam_unstable/examples/KITTItoBALConverter.cpp

@@ -41,7 +41,7 @@
 // have been provided with the library for solving robotics SLAM problems.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam_unstable/slam/SmartProjectionFactor.h>
+#include <gtsam_unstable/slam/SmartProjectionHessianFactor.h>
 
 // We need to use SFM_data to save it to BAL format
 #include <gtsam/slam/dataset.h>

gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp

@@ -59,7 +59,6 @@ using namespace gtsam;
 using namespace boost::assign;
 namespace NM = gtsam::noiseModel;
 
-#define USE_BUNDLER
 
 using symbol_shorthand::X;
 using symbol_shorthand::L;
@@ -67,13 +66,8 @@ using symbol_shorthand::L;
 typedef PriorFactor<Pose3> Pose3Prior;
 typedef FastMap<Key, int> OrderingMap;
 
-#ifdef USE_BUNDLER
 typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> SmartFactorsCreator;
 typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> ProjectionFactorsCreator;
-#else
-typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator;
-typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3_S2> ProjectionFactorsCreator;
-#endif
 
 bool debug = false;
 
@@ -120,68 +114,58 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
   params.verbosity = NonlinearOptimizerParams::ERROR;
   params.lambdaInitial = 1;
-  params.lambdaFactor = 10;
+  // Other parameters: if needed
+  // params.lambdaFactor = 10;
   // Profile a single iteration
-  //  params.maxIterations = 1;
-  params.maxIterations = 100;
-  std::cout << " LM max iterations: " << params.maxIterations << std::endl;
-  // // params.relativeErrorTol = 1e-5;
-  params.absoluteErrorTol = 1.0;
-  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
-  params.verbosity = NonlinearOptimizerParams::ERROR;
-  params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
+  params.maxIterations = 1;
+  // params.relativeErrorTol = 1e-5;
+  // params.absoluteErrorTol = 1.0;
+  cout << "==================== Optimization ==================" << endl;
+  cout << "- Number of factors: " << graph.size() << endl;
+  cout << "- Number of variables: " << graphValues->size() << endl;
 
-  cout << "Graph size: " << graph.size() << endl;
-  cout << "Number of variables: " << graphValues->size() << endl;
-  std::cout << " OPTIMIZATION " << std::endl;
+  params.print("PARAMETERS FOR LM: \n");
 
   if (debug) {
-    std::cout << "\n\n=================================================\n\n";
+    cout << "\n\n===============================================\n\n";
     graph.print("thegraph");
   }
-  std::cout << "\n\n=================================================\n\n";
+  cout << "-----------------------------------------------------" << endl;
 
   if (ordering && ordering->size() > 0) {
-    if (debug) {
-      std::cout << "Have an ordering\n" << std::endl;
-      BOOST_FOREACH(const Key& key, *ordering) {
-        std::cout << key << " ";
-      }
-      std::cout << std::endl;
-    }
-
+    std::cout << "Starting graph optimization with user specified ordering" << std::endl;
     params.ordering = *ordering;
-
-    //for (int i = 0; i < 3; i++) {
     LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
     gttic_(GenericProjectionFactorExample_kitti);
     result = optimizer.optimize();
     gttoc_(GenericProjectionFactorExample_kitti);
     tictoc_finishedIteration_();
-    //}
-  } else {
-    std::cout << "Using COLAMD ordering\n" << std::endl;
-    //boost::shared_ptr<Ordering> ordering2(new Ordering()); ordering = ordering2;
+    cout << "-----------------------------------------------------" << endl;
+    std::cout << "Number of outer LM iterations: " << optimizer.state().iterations << std::endl;
+    std::cout << "Total number of LM iterations (inner and outer): " << optimizer.getInnerIterations() << std::endl;
 
+  } else {
+    std::cout << "Starting graph optimization with COLAMD ordering" << std::endl;
     LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params);
     params.ordering = Ordering::COLAMD(graph);
-    gttic_(SmartProjectionFactorExample_kitti);
+    gttic_(smartProjectionFactorExample);
     result = optimizer.optimize();
-    gttoc_(SmartProjectionFactorExample_kitti);
+    gttoc_(smartProjectionFactorExample);
     tictoc_finishedIteration_();
-
+    cout << "-----------------------------------------------------" << endl;
     std::cout << "Number of outer LM iterations: " << optimizer.state().iterations << std::endl;
     std::cout << "Total number of LM iterations (inner and outer): " << optimizer.getInnerIterations() << std::endl;
 
     //*ordering = params.ordering;
     if (params.ordering) {
-      std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl;
+      if(debug) std::cout << "Graph size: " << graph.size() << " Ordering: " << params.ordering->size() << std::endl;
       ordering = boost::make_shared<Ordering>(*(new Ordering()));
       *ordering = *params.ordering;
     } else {
       std::cout << "WARNING: NULL ordering!" << std::endl;
     }
   }
+  cout << "======================================================" << endl;
 }
 
 void optimizeGraphGN(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) {
@@ -190,31 +174,31 @@ void optimizeGraphGN(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap
   params.verbosity = NonlinearOptimizerParams::DELTA;
 
   GaussNewtonOptimizer optimizer(graph, *graphValues, params);
-  gttic_(SmartProjectionFactorExample_kitti);
+  gttic_(smartProjectionFactorExample);
   result = optimizer.optimize();
-  gttoc_(SmartProjectionFactorExample_kitti);
+  gttoc_(smartProjectionFactorExample);
   tictoc_finishedIteration_();
-
 }
 
 void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) {
   ISAM2 isam;
-  gttic_(SmartProjectionFactorExample_kitti);
+  gttic_(smartProjectionFactorExample);
   isam.update(graph, *graphValues);
   result = isam.calculateEstimate();
-  gttoc_(SmartProjectionFactorExample_kitti);
+  gttoc_(smartProjectionFactorExample);
   tictoc_finishedIteration_();
 }
 
+// ************************************************************************************************
+// ************************************************************************************************
 // main
 int main(int argc, char** argv) {
 
-  // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
-  bool useSmartProjectionFactor = true;
-  bool doTriangulation = true; // we read points initial guess from file or we triangulate
+  bool useSmartProjectionFactor = true; // default choice is to use the smart projection factors
+  bool doTriangulation = true; // default choice is to initialize points from triangulation (only for standard projection factors)
 
+  bool addNoise = false; // add (fixed) noise to the initial guess of camera poses
   bool useLM = true; 
-  bool addNoise = false;
 
   // Smart factors settings
   double linThreshold = -1.0; // negative is disabled
@@ -225,6 +209,7 @@ int main(int argc, char** argv) {
   string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt";
 
   // --------------- READ USER INPUTS (main arguments) ------------------------------------
+  // COMMAND TO RUN (EXAMPLE): ./SmartProjectionFactorExampleBAL smart triangulation=0 /home/aspn/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt
   if(argc>1){ // if we have any input arguments
     // Arg1: "smart" or "standard" (select if to use smart factors or standard projection factors)
     // Arg2: "triangulation=0" or "triangulation=1" (select whether to initialize initial guess for points using triangulation)
@@ -260,18 +245,19 @@ int main(int argc, char** argv) {
   std::cout << "- datasetFile: " << datasetFile << std::endl;
 
   if (linThreshold >= 0)
-    std::cout << "PARAM linThreshold (negative is disabled): " << linThreshold << std::endl;
+    std::cout << "- linThreshold (negative is disabled): " << linThreshold << std::endl;
 
   if(addNoise)
-    std::cout << "PARAM Noise: " << addNoise << std::endl;
+    std::cout << "- Noise: " << addNoise << std::endl;
 
   // --------------- READ INPUT DATA  ----------------------------------------
+  std::cout << "- reading dataset from file... " << std::endl;
   SfM_data inputData;
   readBAL(datasetFile, inputData);
-  std::cout << "read data from file... " << std::endl;
 
   // --------------- CREATE FACTOR GRAPH  ------------------------------------
-  static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2));
+  std::cout << "- creating factor graph... " << std::endl;
+  static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); // pixel noise
   boost::shared_ptr<Ordering> ordering(new Ordering());
 
   NonlinearFactorGraph graphSmart;
@@ -280,13 +266,8 @@ int main(int argc, char** argv) {
   NonlinearFactorGraph graphProjection;
   gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values());
 
-#ifdef USE_BUNDLER
   std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras;
   boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler());
-#else
-  std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
-  Cal3_S2::shared_ptr K(new Cal3_S2());
-#endif
 
   SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used
   ProjectionFactorsCreator projectionCreator(pixel_sigma, K);  // this initial K is not used
@@ -306,8 +287,8 @@ int main(int argc, char** argv) {
       Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3));
       cameraPose = cameraPose.compose(noise_pose);
     }
-    loadedValues->insert(X(i), cameraPose);
-    graphSmartValues->insert(X(i), cameraPose);
+    loadedValues->insert(X(i), cameraPose); // this will be used for the graphProjection
+    graphSmartValues->insert(X(i), cameraPose); // we insert the value for the graphSmart that only contains poses
   }
   if(debug) std::cout << "Initialized values " << std::endl;
 
@@ -324,15 +305,13 @@ int main(int argc, char** argv) {
 
     for (size_t k = 0; k < track.number_measurements(); k++){ // for each measurement of the point
       SfM_Measurement measurement = track.measurements[k];
-      int    i = measurement.first;
+      int    i = measurement.first; // camera id
       double u = measurement.second.x();
       double v = measurement.second.y();
       boost::shared_ptr<Cal3Bundler> Ki(new Cal3Bundler(inputData.cameras[i].calibration()));
-      //boost::shared_ptr<Cal3_S2> Ki(new Cal3_S2());
 
-      // insert data in a format that can be understood
       if (useSmartProjectionFactor) {
-        // Use smart factors
+        // use SMART PROJECTION FACTORS
         smartCreator.add(L(j), X(i), Point2(u,v), pixel_sigma, Ki, graphSmart);
         numLandmarks = smartCreator.getNumLandmarks();
       } else {
@@ -342,31 +321,33 @@ int main(int argc, char** argv) {
       }
     }
   }
-  if(debug) std::cout << "Included measurements in the graph " << std::endl;
-
-  cout << "Number of landmarks  " << numLandmarks << endl;
-
-  cout << "Before call to update: ------------------ " << endl;
-  cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
-  Values valuesProjPoses = graphProjectionValues->filter<Pose3>();
-  cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
-  Values valuesProjPoints = graphProjectionValues->filter<Point3>();
-  cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
-  cout << "---------------------------------------------------------- " << endl;
+  if(debug){
+    cout << "Included measurements in the graph " << endl;
+    cout << "Number of landmarks  " << numLandmarks << endl;
+    cout << "Before call to update: ------------------ " << endl;
+    cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
+    Values valuesProjPoses = graphProjectionValues->filter<Pose3>();
+    cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
+    Values valuesProjPoints = graphProjectionValues->filter<Point3>();
+    cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
+    cout << "---------------------------------------------------------- " << endl;
+  }
 
   if (!useSmartProjectionFactor) {
     projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation);
-    // graphProjectionValues = loadedValues;
     ordering = projectionCreator.getOrdering();
   }
 
-  cout << "After call to update: ------------------ " << endl;
-  cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
-  valuesProjPoses = graphProjectionValues->filter<Pose3>();
-  cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
-  valuesProjPoints = graphProjectionValues->filter<Point3>();
-  cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
-  cout << "---------------------------------------------------------- " << endl;
+  if(debug) {
+    cout << "After call to update: ------------------ " << endl;
+    cout << "--------------------------------------------------------- " << endl;
+    cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
+    Values valuesProjPoses = graphProjectionValues->filter<Pose3>();
+    cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
+    Values valuesProjPoints = graphProjectionValues->filter<Point3>();
+    cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
+    cout << "---------------------------------------------------------- " << endl;
+  }
 
   Values result;
   if (useSmartProjectionFactor) {
@@ -374,20 +355,28 @@ int main(int argc, char** argv) {
       optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
     else
       optimizeGraphISAM2(graphSmart, graphSmartValues, result);
-    cout << "Final reprojection error (smart): " << graphSmart.error(result);
+    cout << "Initial reprojection error (smart): " << graphSmart.error(*graphSmartValues) << endl;;
+    cout << "Final reprojection error (smart): " << graphSmart.error(result) << endl;;
   } else {
     if (useLM)
       optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering);
     else
-      optimizeGraphISAM2(graphProjection, graphProjectionValues, result); // ?
-    cout << "Final reprojection error (standard): " << graphProjection.error(result);
+      optimizeGraphISAM2(graphProjection, graphProjectionValues, result);
+    cout << "Initial reprojection error (standard): " << graphProjection.error(*graphProjectionValues) << endl;;
+    cout << "Final reprojection error (standard): " << graphProjection.error(result) << endl;;
   }
 
-  cout << "===================================================" << endl;
   tictoc_print_();
   cout << "===================================================" << endl;
-  writeValues("./", result);
 
+  // --------------- WRITE OUTPUT TO BAL FILE  ----------------------------------------
+  cout << "- writing results to (BAL) file... " << endl;
+  std::size_t stringCut1 = datasetFile.rfind("/");
+  std::size_t stringCut2 = datasetFile.rfind(".txt");
+  string outputFile = "." + datasetFile.substr(stringCut1, stringCut2-stringCut1) + "-optimized.txt";
+  if(debug) cout << outputFile << endl;
+  writeBALfromValues(outputFile, inputData, result);
+  cout << "- mission accomplished! " << endl;
   exit(0);
 }