Extend support writing linear least squares problems to disk.
1. Make the mechanism for writing problems to disk, generic and
controllable using an enum DumpType visible in the API.
2. Instead of single file containing protocol buffers, now matrices can
be written in a matlab/octave friendly format. This is now the default.
3. The support for writing problems to disk is moved into
linear_least_squares_problem.cc/h
4. SparseMatrix now has a ToTextFile virtual method which is
implemented by each of its subclasses to write a (i,j,s) triplets.
5. Minor changes to simple_bundle_adjuster to enable logging at startup.
diff --git a/internal/ceres/block_sparse_matrix.cc b/internal/ceres/block_sparse_matrix.cc
index c1be940..7dd395e 100644
--- a/internal/ceres/block_sparse_matrix.cc
+++ b/internal/ceres/block_sparse_matrix.cc
@@ -259,5 +259,28 @@
}
#endif
+void BlockSparseMatrix::ToTextFile(FILE* file) const {
+ CHECK_NOTNULL(file);
+ for (int i = 0; i < block_structure_->rows.size(); ++i) {
+ const int row_block_pos = block_structure_->rows[i].block.position;
+ const int row_block_size = block_structure_->rows[i].block.size;
+ const vector<Cell>& cells = block_structure_->rows[i].cells;
+ for (int j = 0; j < cells.size(); ++j) {
+ const int col_block_id = cells[j].block_id;
+ const int col_block_size = block_structure_->cols[col_block_id].size;
+ const int col_block_pos = block_structure_->cols[col_block_id].position;
+ int jac_pos = cells[j].position;
+ for (int r = 0; r < row_block_size; ++r) {
+ for (int c = 0; c < col_block_size; ++c) {
+ fprintf(file, "% 10d % 10d %17f\n",
+ row_block_pos + r,
+ col_block_pos + c,
+ values_[jac_pos++]);
+ }
+ }
+ }
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/block_sparse_matrix.h b/internal/ceres/block_sparse_matrix.h
index b151dd0..f71446e 100644
--- a/internal/ceres/block_sparse_matrix.h
+++ b/internal/ceres/block_sparse_matrix.h
@@ -113,6 +113,8 @@
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
virtual void ToProto(SparseMatrixProto* proto) const;
#endif
+ virtual void ToTextFile(FILE* file) const;
+
virtual int num_rows() const { return num_rows_; }
virtual int num_cols() const { return num_cols_; }
virtual int num_nonzeros() const { return num_nonzeros_; }
diff --git a/internal/ceres/compressed_row_sparse_matrix.cc b/internal/ceres/compressed_row_sparse_matrix.cc
index 8fd568f..95edf53 100644
--- a/internal/ceres/compressed_row_sparse_matrix.cc
+++ b/internal/ceres/compressed_row_sparse_matrix.cc
@@ -321,5 +321,14 @@
num_rows_ += m.num_rows();
}
+void CompressedRowSparseMatrix::ToTextFile(FILE* file) const {
+ CHECK_NOTNULL(file);
+ for (int r = 0; r < num_rows_; ++r) {
+ for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) {
+ fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]);
+ }
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/compressed_row_sparse_matrix.h b/internal/ceres/compressed_row_sparse_matrix.h
index 43712a8..9a39d28 100644
--- a/internal/ceres/compressed_row_sparse_matrix.h
+++ b/internal/ceres/compressed_row_sparse_matrix.h
@@ -88,7 +88,7 @@
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
virtual void ToProto(SparseMatrixProto* proto) const;
#endif
-
+ virtual void ToTextFile(FILE* file) const;
virtual int num_rows() const { return num_rows_; }
virtual int num_cols() const { return num_cols_; }
virtual int num_nonzeros() const { return rows_[num_rows_]; }
diff --git a/internal/ceres/dense_sparse_matrix.cc b/internal/ceres/dense_sparse_matrix.cc
index ffbfab6..5d392ba 100644
--- a/internal/ceres/dense_sparse_matrix.cc
+++ b/internal/ceres/dense_sparse_matrix.cc
@@ -179,6 +179,19 @@
return AlignedMatrixRef(m_.data(), m_.rows(), m_.cols());
}
+void DenseSparseMatrix::ToTextFile(FILE* file) const {
+ CHECK_NOTNULL(file);
+ const int active_rows =
+ (has_diagonal_reserved_ && !has_diagonal_appended_)
+ ? (m_.rows() - m_.cols())
+ : m_.rows();
+
+ for (int r = 0; r < active_rows; ++r) {
+ for (int c = 0; c < m_.cols(); ++c) {
+ fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c));
+ }
+ }
+}
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/dense_sparse_matrix.h b/internal/ceres/dense_sparse_matrix.h
index 5ce29ee..416c214 100644
--- a/internal/ceres/dense_sparse_matrix.h
+++ b/internal/ceres/dense_sparse_matrix.h
@@ -70,6 +70,7 @@
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
virtual void ToProto(SparseMatrixProto* proto) const;
#endif
+ virtual void ToTextFile(FILE* file) const;
virtual int num_rows() const;
virtual int num_cols() const;
virtual int num_nonzeros() const;
diff --git a/internal/ceres/levenberg_marquardt.cc b/internal/ceres/levenberg_marquardt.cc
index 3ecae5d..4ada794 100644
--- a/internal/ceres/levenberg_marquardt.cc
+++ b/internal/ceres/levenberg_marquardt.cc
@@ -58,6 +58,7 @@
#include "Eigen/Core"
#include "ceres/evaluator.h"
#include "ceres/file.h"
+#include "ceres/linear_least_squares_problems.h"
#include "ceres/linear_solver.h"
#include "ceres/matrix_proto.h"
#include "ceres/sparse_matrix.h"
@@ -107,62 +108,6 @@
}
}
-string DumpLinearSolverProblem(
- int iteration,
- const SparseMatrix* A,
- const double* D,
- const double* b,
- const double* x,
- const Minimizer::Options& solver_options) {
- if (solver_options.lsqp_dump_format == "ascii") {
- // Dump to the screen instead of to file. Useful for debugging.
- Matrix AA;
- A->ToDenseMatrix(&AA);
- LOG(INFO) << "A^T: \n" << AA.transpose();
- if (D) {
- LOG(INFO) << "A's appended diagonal:\n"
- << ConstVectorRef(D, A->num_cols());
- }
- LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows());
- LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols());
- return "";
- }
-#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
- LinearLeastSquaresProblemProto lsqp;
- A->ToProto(lsqp.mutable_a());
- for (int i = 0; i < A->num_rows(); ++i) {
- lsqp.add_b(b[i]);
- }
- if (D) {
- for (int i = 0; i < A->num_cols(); ++i) {
- lsqp.add_d(D[i]);
- }
- }
- if (x) {
- for (int i = 0; i < A->num_cols(); ++i) {
- lsqp.add_x(x[i]);
- }
- }
-
- lsqp.set_num_eliminate_blocks(solver_options.num_eliminate_blocks);
-
- CHECK(solver_options.lsqp_dump_format.size());
- string filename =
- StringPrintf(solver_options.lsqp_dump_format.c_str(), // NOLINT
- iteration);
- VLOG(1) << "Dumping least squares problem for iteration " << iteration
- << " to disk. File: " << filename;
- WriteStringToFileOrDie(lsqp.SerializeAsString(), filename);
- VLOG(2) << "Done dumping to disk";
- return filename;
-#else
- LOG(ERROR) << "Dumping least squares problems is only "
- << "supported when Ceres is compiled with "
- << "protocol buffer support.";
- return "";
-#endif
-}
-
bool RunCallback(IterationCallback* callback,
const IterationSummary& iteration_summary,
Solver::Summary* summary) {
@@ -379,12 +324,17 @@
if (binary_search(iterations_to_dump.begin(),
iterations_to_dump.end(),
iteration)) {
- DumpLinearSolverProblem(iteration,
- jacobian.get(),
- muD.data(),
- f.data(),
- lm_step.data(),
- options);
+ CHECK(DumpLinearLeastSquaresProblem(options.lsqp_dump_directory,
+ iteration,
+ options.lsqp_dump_format_type,
+ jacobian.get(),
+ muD.data(),
+ f.data(),
+ lm_step.data(),
+ options.num_eliminate_blocks))
+ << "Tried writing linear least squares problem: "
+ << options.lsqp_dump_directory
+ << " but failed.";
}
// We ignore the case where the linear solver did not converge,
@@ -412,7 +362,7 @@
(x_norm + options.parameter_tolerance)) {
summary->termination_type = PARAMETER_TOLERANCE;
VLOG(1) << "Terminating on PARAMETER_TOLERANCE."
- << "Relative step size: " << step_norm / step_size_tolerance
+ << "Relative step size: " << step_norm / step_size_tolerance
<< " <= " << options.parameter_tolerance;
return;
}
@@ -544,33 +494,37 @@
}
if (num_consecutive_insane_steps == kMaxLinearSolverRetries) {
- VLOG(1) << "Too many consecutive retries; ending with numerical fail.";
summary->termination_type = NUMERICAL_FAILURE;
+ VLOG(1) << "Too many consecutive retries; ending with numerical fail.";
if (!options.crash_and_dump_lsqp_on_failure) {
return;
}
// Dump debugging information to disk.
- CHECK(!options.lsqp_dump_format.empty())
+ CHECK(options.lsqp_dump_format_type == TEXTFILE ||
+ options.lsqp_dump_format_type == PROTOBUF)
<< "Dumping the linear least squares problem on crash "
- << "requires Solver::Options::lsqp_dump_format set a "
- << "filename";
- CHECK_NE(options.lsqp_dump_format, "ascii")
- << "Dumping the linear least squares problem on crash "
- << "requires Solver::Options::lsqp_dump_format set a "
- << "filename";
+ << "requires Solver::Options::lsqp_dump_format_type to be "
+ << "PROTOBUF or TEXTFILE.";
- const string filename = DumpLinearSolverProblem(iteration,
- jacobian.get(),
- muD.data(),
- f.data(),
- lm_step.data(),
- options);
- LOG(FATAL) << "Linear least squares problem saved to " << filename
- << " please provide this to the Ceres developers for "
- << " debugging along with the v=2 log.";
- return;
+ if (DumpLinearLeastSquaresProblem(options.lsqp_dump_directory,
+ iteration,
+ options.lsqp_dump_format_type,
+ jacobian.get(),
+ muD.data(),
+ f.data(),
+ lm_step.data(),
+ options.num_eliminate_blocks)) {
+ LOG(FATAL) << "Linear least squares problem saved to: "
+ << options.lsqp_dump_directory
+ << ". Please provide this to the Ceres developers for "
+ << " debugging along with the v=2 log.";
+ } else {
+ LOG(FATAL) << "Tried writing linear least squares problem: "
+ << options.lsqp_dump_directory
+ << " but failed.";
+ }
}
if (!step_is_successful) {
diff --git a/internal/ceres/linear_least_squares_problems.cc b/internal/ceres/linear_least_squares_problems.cc
index 9e3d8bd..da3ed8c 100644
--- a/internal/ceres/linear_least_squares_problems.cc
+++ b/internal/ceres/linear_least_squares_problems.cc
@@ -30,15 +30,18 @@
#include "ceres/linear_least_squares_problems.h"
+#include <cstdio>
#include <string>
#include <vector>
#include <glog/logging.h>
#include "ceres/block_sparse_matrix.h"
#include "ceres/block_structure.h"
+#include "ceres/casts.h"
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/file.h"
#include "ceres/matrix_proto.h"
#include "ceres/triplet_sparse_matrix.h"
+#include "ceres/stringprintf.h"
#include "ceres/internal/scoped_ptr.h"
#include "ceres/types.h"
@@ -569,5 +572,172 @@
return problem;
}
+bool DumpLinearLeastSquaresProblemToConsole(const string& directory,
+ int iteration,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ CHECK_NOTNULL(A);
+ Matrix AA;
+ A->ToDenseMatrix(&AA);
+ LOG(INFO) << "A^T: \n" << AA.transpose();
+
+ if (D != NULL) {
+ LOG(INFO) << "A's appended diagonal:\n"
+ << ConstVectorRef(D, A->num_cols());
+ }
+
+ if (b != NULL) {
+ LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows());
+ }
+
+ if (x != NULL) {
+ LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols());
+ }
+ return true;
+};
+
+#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
+bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory,
+ int iteration,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ CHECK_NOTNULL(A);
+ LinearLeastSquaresProblemProto lsqp;
+ A->ToProto(lsqp.mutable_a());
+
+ if (D != NULL) {
+ for (int i = 0; i < A->num_cols(); ++i) {
+ lsqp.add_d(D[i]);
+ }
+ }
+
+ if (b != NULL) {
+ for (int i = 0; i < A->num_rows(); ++i) {
+ lsqp.add_b(b[i]);
+ }
+ }
+
+ if (x != NULL) {
+ for (int i = 0; i < A->num_cols(); ++i) {
+ lsqp.add_x(x[i]);
+ }
+ }
+
+ lsqp.set_num_eliminate_blocks(num_eliminate_blocks);
+ string format_string = JoinPath(directory,
+ "lm_iteration_%03d.lsqp");
+ string filename =
+ StringPrintf(format_string.c_str(), iteration);
+ LOG(INFO) << "Dumping least squares problem for iteration " << iteration
+ << " to disk. File: " << filename;
+ WriteStringToFileOrDie(lsqp.SerializeAsString(), filename);
+ return true;
+}
+#else
+bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory,
+ int iteration,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ LOG(ERROR) << "Dumping least squares problems is only "
+ << "supported when Ceres is compiled with "
+ << "protocol buffer support.";
+ return false;
+}
+#endif
+
+void WriteArrayToFileOrDie(const string& filename,
+ const double* x,
+ const int size) {
+ CHECK_NOTNULL(x);
+ VLOG(2) << "Writing array to: " << filename;
+ FILE* fptr = fopen(filename.c_str(), "w");
+ CHECK_NOTNULL(fptr);
+ for (int i = 0; i < size; ++i) {
+ fprintf(fptr, "%17f\n", x[i]);
+ }
+ fclose(fptr);
+}
+
+bool DumpLinearLeastSquaresProblemToTextFile(const string& directory,
+ int iteration,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ CHECK_NOTNULL(A);
+ string format_string = JoinPath(directory,
+ "lm_iteration_%03d");
+ string filename_prefix =
+ StringPrintf(format_string.c_str(), iteration);
+
+ {
+ string filename = filename_prefix + "_A.txt";
+ LOG(INFO) << "writing to: " << filename;
+ FILE* fptr = fopen(filename.c_str(), "w");
+ CHECK_NOTNULL(fptr);
+ A->ToTextFile(fptr);
+ fclose(fptr);
+ }
+
+ if (D != NULL) {
+ string filename = filename_prefix + "_D.txt";
+ WriteArrayToFileOrDie(filename, D, A->num_cols());
+ }
+
+ if (b != NULL) {
+ string filename = filename_prefix + "_b.txt";
+ WriteArrayToFileOrDie(filename, b, A->num_rows());
+ }
+
+ if (x != NULL) {
+ string filename = filename_prefix + "_x.txt";
+ WriteArrayToFileOrDie(filename, x, A->num_cols());
+ }
+
+ return true;
+}
+
+bool DumpLinearLeastSquaresProblem(const string& directory,
+ int iteration,
+ DumpFormatType dump_format_type,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks) {
+ switch (dump_format_type) {
+ case (CONSOLE):
+ return DumpLinearLeastSquaresProblemToConsole(directory,
+ iteration,
+ A, D, b, x,
+ num_eliminate_blocks);
+ case (PROTOBUF):
+ return DumpLinearLeastSquaresProblemToProtocolBuffer(
+ directory,
+ iteration,
+ A, D, b, x,
+ num_eliminate_blocks);
+ case (TEXTFILE):
+ return DumpLinearLeastSquaresProblemToTextFile(directory,
+ iteration,
+ A, D, b, x,
+ num_eliminate_blocks);
+ default:
+ LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type;
+ };
+
+ return true;
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/linear_least_squares_problems.h b/internal/ceres/linear_least_squares_problems.h
index 46a624b..553cc0d 100644
--- a/internal/ceres/linear_least_squares_problems.h
+++ b/internal/ceres/linear_least_squares_problems.h
@@ -32,7 +32,7 @@
#define CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_
#include <string>
-
+#include <vector>
#include "ceres/sparse_matrix.h"
#include "ceres/internal/port.h"
#include "ceres/internal/scoped_ptr.h"
@@ -71,6 +71,16 @@
LinearLeastSquaresProblem* LinearLeastSquaresProblem2();
LinearLeastSquaresProblem* LinearLeastSquaresProblem3();
+// Write the linear least squares problem to disk. The exact format
+// depends on dump_format_type.
+bool DumpLinearLeastSquaresProblem(const string& directory,
+ int iteration,
+ DumpFormatType dump_format_type,
+ const SparseMatrix* A,
+ const double* D,
+ const double* b,
+ const double* x,
+ int num_eliminate_blocks);
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/minimizer.h b/internal/ceres/minimizer.h
index 71163a8..77cb00c 100644
--- a/internal/ceres/minimizer.h
+++ b/internal/ceres/minimizer.h
@@ -59,8 +59,9 @@
tau = options.tau;
jacobi_scaling = options.jacobi_scaling;
crash_and_dump_lsqp_on_failure = options.crash_and_dump_lsqp_on_failure;
- lsqp_dump_format = options.lsqp_dump_format;
+ lsqp_dump_directory = options.lsqp_dump_directory;
lsqp_iterations_to_dump = options.lsqp_iterations_to_dump;
+ lsqp_dump_format_type = options.lsqp_dump_format_type;
num_eliminate_blocks = options.num_eliminate_blocks;
logging_type = options.logging_type;
}
@@ -75,8 +76,9 @@
double tau;
bool jacobi_scaling;
bool crash_and_dump_lsqp_on_failure;
- string lsqp_dump_format;
vector<int> lsqp_iterations_to_dump;
+ DumpFormatType lsqp_dump_format_type;
+ string lsqp_dump_directory;
int num_eliminate_blocks;
LoggingType logging_type;
diff --git a/internal/ceres/sparse_matrix.h b/internal/ceres/sparse_matrix.h
index 962b803..562210d 100644
--- a/internal/ceres/sparse_matrix.h
+++ b/internal/ceres/sparse_matrix.h
@@ -33,6 +33,7 @@
#ifndef CERES_INTERNAL_SPARSE_MATRIX_H_
#define CERES_INTERNAL_SPARSE_MATRIX_H_
+#include <cstdio>
#include "ceres/linear_operator.h"
#include "ceres/internal/eigen.h"
#include "ceres/types.h"
@@ -87,9 +88,14 @@
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
// Dump the sparse matrix to a proto. Destroys the contents of proto.
- virtual void ToProto(SparseMatrixProto *proto) const = 0;
+ virtual void ToProto(SparseMatrixProto* proto) const = 0;
#endif
+ // Write out the matrix as a sequence of (i,j,s) triplets. This
+ // format is useful for loading the matrix into MATLAB/octave as a
+ // sparse matrix.
+ virtual void ToTextFile(FILE* file) const = 0;
+
// Accessors for the values array that stores the entries of the
// sparse matrix. The exact interpreptation of the values of this
// array depends on the particular kind of SparseMatrix being
diff --git a/internal/ceres/triplet_sparse_matrix.cc b/internal/ceres/triplet_sparse_matrix.cc
index 7d7c3df..247ab2e 100644
--- a/internal/ceres/triplet_sparse_matrix.cc
+++ b/internal/ceres/triplet_sparse_matrix.cc
@@ -295,5 +295,12 @@
return m;
}
+void TripletSparseMatrix::ToTextFile(FILE* file) const {
+ CHECK_NOTNULL(file);
+ for (int i = 0; i < num_nonzeros_; ++i) {
+ fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]);
+ }
+}
+
} // namespace internal
} // namespace ceres
diff --git a/internal/ceres/triplet_sparse_matrix.h b/internal/ceres/triplet_sparse_matrix.h
index 3c90a62..300e74d 100644
--- a/internal/ceres/triplet_sparse_matrix.h
+++ b/internal/ceres/triplet_sparse_matrix.h
@@ -68,6 +68,7 @@
#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS
virtual void ToProto(SparseMatrixProto *proto) const;
#endif
+ virtual void ToTextFile(FILE* file) const;
virtual int num_rows() const { return num_rows_; }
virtual int num_cols() const { return num_cols_; }
virtual int num_nonzeros() const { return num_nonzeros_; }
