From afb5bac2f7baf035b08bed5443f1bc50fa60e434 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Thu, 13 Mar 2014 01:36:38 -0400
Subject: [PATCH] writeBALfromValues input made const

---
 gtsam/slam/dataset.cpp | 38 ++++++++++++++++++++------------------
 gtsam/slam/dataset.h   |  2 +-
 2 files changed, 21 insertions(+), 19 deletions(-)

diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 6548b1449..4a7166f38 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -669,28 +669,30 @@ bool writeBAL(const string& filename, SfM_data &data)
   return true;
 }
 
-bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
+bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){
+
+  SfM_data dataValues = data;
 
   // Store poses or cameras in SfM_data
   Values valuesPoses = values.filter<Pose3>();
-  if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses
-    for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
+  if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses
+    for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
       Key poseKey = symbol('x',i);
       Pose3 pose = values.at<Pose3>(poseKey);
-      Cal3Bundler K = data.cameras[i].calibration();
+      Cal3Bundler K = dataValues.cameras[i].calibration();
       PinholeCamera<Cal3Bundler> camera(pose, K);
-      data.cameras[i] = camera;
+      dataValues.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
+    if ( valuesCameras.size() == dataValues.number_cameras() ){  // we only estimated camera poses and calibration
+      for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
         Key cameraKey = i; // symbol('c',i);
         PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
-        data.cameras[i] = camera;
+        dataValues.cameras[i] = camera;
       }
     }else{
-      cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras()
+      cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras()
               <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
       return false;
     }
@@ -698,26 +700,26 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){
 
   // 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()
+  if( valuesPoints.size() != dataValues.number_tracks()){
+    cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks()
             <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
   }
 
-  for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
+  for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point
     Key pointKey = symbol('l',j);
     if(values.exists(pointKey)){
       Point3 point = values.at<Point3>(pointKey);
-      data.tracks[j].p = point;
+      dataValues.tracks[j].p = point;
     }else{
-      data.tracks[j].r = 1.0;
-      data.tracks[j].g = 0.0;
-      data.tracks[j].b = 0.0;
-      data.tracks[j].p = Point3();
+      dataValues.tracks[j].r = 1.0;
+      dataValues.tracks[j].g = 0.0;
+      dataValues.tracks[j].b = 0.0;
+      dataValues.tracks[j].p = Point3();
     }
   }
 
   // Write SfM_data to file
-  return writeBAL(filename, data);
+  return writeBAL(filename, dataValues);
 }
 
 Values initialCamerasEstimate(const SfM_data& db) {
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 070bfc000..64ccd37b5 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -146,7 +146,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
  * 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);
+GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values);
 
 /**
  * @brief This function converts an openGL camera pose to an GTSAM camera pose