From 8755e08dad4df137a6cdb3a38464827e56ec6cd5 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Fri, 24 Jan 2014 17:36:41 -0500
Subject: [PATCH] writeBALfromValues can now write either cameras or poses

---
 gtsam/slam/dataset.cpp | 53 ++++++++++++++++++++++--------------------
 gtsam/slam/dataset.h   |  4 +++-
 2 files changed, 31 insertions(+), 26 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 43927757e..db91fbd45 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -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);
 }
@@ -403,7 +403,7 @@ 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);
 }
@@ -671,33 +671,36 @@ bool writeBAL(const string& filename, SfM_data &data)
 
 bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
 
-  // CHECKS
+  // Store poses or cameras in SfM_data
   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;
+  if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
+    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;
+    }
+  } else {
+    Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
+    if ( valuesCameras.size() == data.number_cameras() ){  // we only estimated camera poses and calibration
+      for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
+        Key cameraKey = symbol('c',i);
+        PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
+        data.cameras[i] = camera;
+      }
+    }else{
+      cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
+              <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
+      return false;
+    }
   }
+
+  // Store 3D points in SfM_data
   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;
-  }
-  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;
+            <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
   }
 
   for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
@@ -713,8 +716,8 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
     }
   }
 
+  // Write SfM_data to file
   return writeBAL(filename, data);
 }
 
-
 } // \namespace gtsam
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index bd5a28cdd..0ee5aad9f 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -141,7 +141,9 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &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
+ * @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, SfM_data &data, Values& values);