From 38c91e19136f8020509f989c3855d35c556f5d64 Mon Sep 17 00:00:00 2001
From: Luca Carlone <lucacar85@gmail.com>
Date: Sat, 19 Oct 2013 20:41:21 +0000
Subject: [PATCH] added functions to get u0 and v0 from Cal3Bundler object
 (introduced in writeBAL)

---
 gtsam/geometry/Cal3Bundler.h | 10 ++++++++++
 gtsam/slam/dataset.cpp       | 15 +++++++++++----
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index 2e4825ec1..fff9a6e6d 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -92,6 +92,16 @@ public:
     return k2_;
   }
 
+  /// get parameter u0
+  inline double u0() const {
+    return u0_;
+  }
+
+  /// get parameter v0
+  inline double v0() const {
+    return v0_;
+  }
+
 
   /**
    * convert intrinsic coordinates xy to image coordinates uv
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 24bca54cb..06177a6cc 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -614,8 +614,6 @@ 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++){
@@ -628,9 +626,18 @@ bool writeBAL(const string& filename, SfM_data &data)
 
   for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
     SfM_Track track = data.tracks[j];
+
     for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j
-      Point2 pixelMeasurement(track.measurements[k].second.x(), -track.measurements[k].second.y());
-      os <<  track.measurements[k].first /*camera id*/ << " " << j /*point id*/ << " "
+      int i = track.measurements[k].first; // camera id
+      double u0 = data.cameras[i].calibration().u0();
+      double v0 = data.cameras[i].calibration().v0();
+
+      if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;}
+
+      double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
+      double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin
+      Point2 pixelMeasurement(pixelBALx, pixelBALy);
+      os <<  i /*camera id*/ << " " << j /*point id*/ << " "
           << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl;
     }
   }