Add WriteToFile function to BALProblem.

Change-Id: I1a4f14dad650fbb2103ee8b897889ac6194a084c

examples/bal_problem.cc

diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc
index d5bef20..6f3cadf 100644
--- a/examples/bal_problem.cc
+++ b/examples/bal_problem.cc
@@ -49,10 +49,10 @@
   }
 }
 
-BALProblem::BALProblem(const std::string filename, bool use_quaternions) {
+BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
   FILE* fptr = fopen(filename.c_str(), "r");
 
-  if (!fptr) {
+  if (fptr == NULL) {
     LOG(FATAL) << "Error: unable to open file " << filename;
     return;
   };
@@ -112,6 +112,51 @@
   }
 }
 
+// This function writes the problem to a file in the same format that
+// is read by the constructor.
+void BALProblem::WriteToFile(const std::string& filename) const {
+  FILE* fptr = fopen(filename.c_str(), "w");
+
+  if (fptr == NULL) {
+    LOG(FATAL) << "Error: unable to open file " << filename;
+    return;
+  };
+
+  fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);
+
+  for (int i = 0; i < num_observations_; ++i) {
+    fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
+    for (int j = 0; j < 2; ++j) {
+      fprintf(fptr, " %g", observations_[2 * i + j]);
+    }
+    fprintf(fptr, "\n");
+  }
+
+  for (int i = 0; i < num_cameras(); ++i) {
+    double angleaxis[9];
+    if (use_quaternions_) {
+      // Output in angle-axis format.
+      QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
+      memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
+    } else {
+      memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
+    }
+    for (int j = 0; j < 9; ++j) {
+      fprintf(fptr, "%.16g\n", angleaxis[j]);
+    }
+  }
+
+  const double* points = parameters_ + camera_block_size() * num_cameras_;
+  for (int i = 0; i < num_points(); ++i) {
+    const double* point = points + i * point_block_size();
+    for (int j = 0; j < point_block_size(); ++j) {
+      fprintf(fptr, "%.16g\n", point[j]);
+    }
+  }
+
+  fclose(fptr);
+}
+
 void BALProblem::Perturb(const double rotation_sigma,
                          const double translation_sigma,
                          const double point_sigma) {

examples/bal_problem.h

diff --git a/examples/bal_problem.h b/examples/bal_problem.h
index 95e8d8f..64672cc 100644
--- a/examples/bal_problem.h
+++ b/examples/bal_problem.h
@@ -44,9 +44,11 @@
 
 class BALProblem {
  public:
-  explicit BALProblem(const std::string filename, bool use_quaternions);
+  explicit BALProblem(const std::string& filename, bool use_quaternions);
   ~BALProblem();
 
+  void WriteToFile(const std::string& filename) const;
+
   // Perturb the camera pose and the geometry with random normal
   // numbers with corresponding standard deviations.
   void Perturb(const double rotation_sigma,