Add PLY file logger before and after BA in order to ease visual comparison.

Change-Id: Ib14e8f4b2de686ab6494de270458392f81a0b946

examples/bal_problem.cc

@@ -32,6 +32,7 @@
 
 #include <cstdio>
 #include <cstdlib>
+#include <fstream>
 #include <string>
 #include <vector>
 #include "Eigen/Core"
@@ -176,9 +177,46 @@ void BALProblem::WriteToFile(const std::string& filename) const {
   fclose(fptr);
 }
 
+// Write the problem to a PLY file for inspection in Matlab or CloudCompare.
+void BALProblem::WriteToPLYFile(const std::string& filename) const {
+  std::ofstream of(filename.c_str());
+
+  of << "ply"
+     << '\n' << "format ascii 1.0"
+     << '\n' << "element vertex " << num_cameras_ + num_points_
+     << '\n' << "property float x"
+     << '\n' << "property float y"
+     << '\n' << "property float z"
+     << '\n' << "property uchar red"
+     << '\n' << "property uchar green"
+     << '\n' << "property uchar blue"
+     << '\n' << "end_header" << std::endl;
+
+  // Export extrinsic data (i.e. camera centers) as green points.
+  double angle_axis[3];
+  double center[3];
+  for (int i = 0; i < num_cameras(); ++i)  {
+    const double* camera = cameras() + camera_block_size() * i;
+    CameraToAngleAxisAndCenter(camera, angle_axis, center);
+    of << center[0] << ' ' << center[1] << ' ' << center[2]
+       << " 0 255 0" << '\n';
+  }
+
+  // Export the structure (i.e. 3D Points) as white points.
+  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) {
+      of << point[j] << ' ';
+    }
+    of << "255 255 255\n";
+  }
+  of.close();
+}
+
 void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
                                             double* angle_axis,
-                                            double* center) {
+                                            double* center) const {
   VectorRef angle_axis_ref(angle_axis, 3);
   if (use_quaternions_) {
     QuaternionToAngleAxis(camera, angle_axis);
@@ -196,7 +234,7 @@ void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
 
 void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
                                             const double* center,
-                                            double* camera) {
+                                            double* camera) const {
   ConstVectorRef angle_axis_ref(angle_axis, 3);
   if (use_quaternions_) {
     AngleAxisToQuaternion(angle_axis, camera);

examples/bal_problem.h

@@ -48,6 +48,7 @@ class BALProblem {
   ~BALProblem();
 
   void WriteToFile(const std::string& filename) const;
+  void WriteToPLYFile(const std::string& filename) const;
 
   // Move the "center" of the reconstruction to the origin, where the
   // center is determined by computing the marginal median of the
@@ -74,6 +75,7 @@ class BALProblem {
   const int* camera_index()    const { return camera_index_;             }
   const double* observations() const { return observations_;             }
   const double* parameters()   const { return parameters_;               }
+  const double* cameras()      const { return parameters_;               }
   double* mutable_cameras()          { return parameters_;               }
   double* mutable_points() {
     return parameters_  + camera_block_size() * num_cameras_;
@@ -82,11 +84,11 @@ class BALProblem {
  private:
   void CameraToAngleAxisAndCenter(const double* camera,
                                   double* angle_axis,
-                                  double* center);
+                                  double* center) const;
 
   void AngleAxisAndCenterToCamera(const double* angle_axis,
                                   const double* center,
-                                  double* camera);
+                                  double* camera) const;
   int num_cameras_;
   int num_points_;
   int num_observations_;

examples/bundle_adjuster.cc

@@ -120,6 +120,9 @@ DEFINE_int32(random_seed, 38401, "Random seed used to set the state "
              "the pertubations.");
 DEFINE_bool(line_search, false, "Use a line search instead of trust region "
             "algorithm.");
+DEFINE_string(initial_ply, "", "Export the BAL file data as a PLY file.");
+DEFINE_string(final_ply, "", "Export the refined BAL file data as a PLY "
+            "file.");
 
 namespace ceres {
 namespace examples {
@@ -311,6 +314,11 @@ void BuildProblem(BALProblem* bal_problem, Problem* problem) {
 
 void SolveProblem(const char* filename) {
   BALProblem bal_problem(filename, FLAGS_use_quaternions);
+
+  if (!FLAGS_initial_ply.empty()) {
+    bal_problem.WriteToPLYFile(FLAGS_initial_ply);
+  }
+
   Problem problem;
 
   srand(FLAGS_random_seed);
@@ -327,6 +335,10 @@ void SolveProblem(const char* filename) {
   Solver::Summary summary;
   Solve(options, &problem, &summary);
   std::cout << summary.FullReport() << "\n";
+
+  if (!FLAGS_final_ply.empty()) {
+    bal_problem.WriteToPLYFile(FLAGS_final_ply);
+  }
 }
 
 }  // namespace examples