Add PLY file logger before and after BA in order to ease visual comparison.
Change-Id: Ib14e8f4b2de686ab6494de270458392f81a0b946
diff --git a/examples/bal_problem.cc b/examples/bal_problem.cc
index 46fba4f..fa1d82a 100644
--- a/examples/bal_problem.cc
+++ b/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 @@
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::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);
diff --git a/examples/bal_problem.h b/examples/bal_problem.h
index d3b2aff..e9a348e 100644
--- a/examples/bal_problem.h
+++ b/examples/bal_problem.h
@@ -48,6 +48,7 @@
~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 @@
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 @@
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_;
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc
index 495094b..e64fdb5 100644
--- a/examples/bundle_adjuster.cc
+++ b/examples/bundle_adjuster.cc
@@ -120,6 +120,9 @@
"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 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 @@
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
