Add profiling to covariance estimation.

Prevent GetCovarianceBlock from being called before
Compute or when Compute failed.

Change-Id: I5c28d27a88081e230d316c5e365d3e21d6e23376

internal/ceres/covariance_impl.cc

diff --git a/internal/ceres/covariance_impl.cc b/internal/ceres/covariance_impl.cc
index 1f73ffd..1a96193 100644
--- a/internal/ceres/covariance_impl.cc
+++ b/internal/ceres/covariance_impl.cc
@@ -42,6 +42,7 @@
 #include "ceres/parameter_block.h"
 #include "ceres/problem_impl.h"
 #include "ceres/suitesparse.h"
+#include "ceres/wall_time.h"
 #include "glog/logging.h"
 
 namespace ceres {
@@ -50,7 +51,9 @@
 typedef vector<pair<const double*, const double*> > CovarianceBlocks;
 
 CovarianceImpl::CovarianceImpl(const Covariance::Options& options)
-    : options_(options) {
+    : options_(options),
+      is_computed_(false),
+      is_valid_(false) {
   evaluate_options_.num_threads = options.num_threads;
   evaluate_options_.apply_loss_function = options.apply_loss_function;
 }
@@ -61,17 +64,23 @@
 bool CovarianceImpl::Compute(const CovarianceBlocks& covariance_blocks,
                              ProblemImpl* problem) {
   problem_ = problem;
-
   parameter_block_to_row_index_.clear();
   covariance_matrix_.reset(NULL);
-
-  return (ComputeCovarianceSparsity(covariance_blocks, problem) &&
-          ComputeCovarianceValues());
+  is_valid_ = (ComputeCovarianceSparsity(covariance_blocks, problem) &&
+               ComputeCovarianceValues());
+  is_computed_ = true;
+  return is_valid_;
 }
 
 bool CovarianceImpl::GetCovarianceBlock(const double* original_parameter_block1,
                                         const double* original_parameter_block2,
                                         double* covariance_block) const {
+  CHECK(is_computed_)
+      << "Covariance::GetCovarianceBlock called before Covariance::Compute";
+  CHECK(is_valid_)
+      << "Covariance::GetCovarianceBlock called when Covariance::Compute "
+      << "returned false.";
+
   // If either of the two parameter blocks is constant, then the
   // covariance block is also zero.
   if (constant_parameter_blocks_.count(original_parameter_block1) > 0 ||
@@ -206,6 +215,7 @@
 bool CovarianceImpl::ComputeCovarianceSparsity(
     const CovarianceBlocks&  original_covariance_blocks,
     ProblemImpl* problem) {
+  EventLogger event_logger("CovarianceImpl::ComputeCovarianceSparsity");
 
   // Determine an ordering for the parameter block, by sorting the
   // parameter blocks by their pointers.
@@ -356,6 +366,8 @@
 }
 
 bool CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparse() {
+  EventLogger event_logger(
+      "CovarianceImpl::ComputeCovarianceValuesUsingSuiteSparse");
 #ifndef CERES_NO_SUITESPARSE
   if (covariance_matrix_.get() == NULL) {
     // Nothing to do, all zeros covariance matrix.
@@ -365,6 +377,7 @@
   CRSMatrix jacobian;
   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
 
+  event_logger.AddEvent("Evaluate");
   // m is a transposed view of the Jacobian.
   cholmod_sparse cholmod_jacobian_view;
   cholmod_jacobian_view.nrow = jacobian.num_cols;
@@ -383,7 +396,10 @@
   cholmod_jacobian_view.packed = 1;
 
   cholmod_factor* factor = ss_.AnalyzeCholesky(&cholmod_jacobian_view);
-  if (!ss_.Cholesky(&cholmod_jacobian_view, factor)) {
+  event_logger.AddEvent("Symbolic Factorization");
+  bool status = ss_.Cholesky(&cholmod_jacobian_view, factor);
+  event_logger.AddEvent("Numeric Factorization");
+  if (!status) {
     ss_.Free(factor);
     LOG(WARNING) << "Cholesky factorization failed.";
     return false;
@@ -473,6 +489,7 @@
 
   ss_.Free(rhs);
   ss_.Free(factor);
+  event_logger.AddEvent("Inversion");
   return true;
 #else
   return false;
@@ -480,6 +497,8 @@
 };
 
 bool CovarianceImpl::ComputeCovarianceValuesUsingEigen() {
+  EventLogger event_logger(
+      "CovarianceImpl::ComputeCovarianceValuesUsingEigen");
   if (covariance_matrix_.get() == NULL) {
     // Nothing to do, all zeros covariance matrix.
     return true;
@@ -487,6 +506,8 @@
 
   CRSMatrix jacobian;
   problem_->Evaluate(evaluate_options_, NULL, NULL, NULL, &jacobian);
+  event_logger.AddEvent("Evaluate");
+
   Matrix dense_jacobian(jacobian.num_rows, jacobian.num_cols);
   dense_jacobian.setZero();
   for (int r = 0; r < jacobian.num_rows; ++r) {
@@ -495,10 +516,12 @@
       dense_jacobian(r, c) = jacobian.values[idx];
     }
   }
+  event_logger.AddEvent("ConvertToDenseMatrix");
 
   Eigen::JacobiSVD<Matrix> svd(dense_jacobian,
                                Eigen::ComputeThinU | Eigen::ComputeThinV);
   Vector inverse_singular_values = svd.singularValues();
+  event_logger.AddEvent("SVD");
 
   for (int i = 0; i < inverse_singular_values.rows(); ++i) {
     if (inverse_singular_values[i] > options_.min_singular_value_threshold &&
@@ -514,6 +537,7 @@
       svd.matrixV() *
       inverse_singular_values.asDiagonal() *
       svd.matrixV().transpose();
+  event_logger.AddEvent("PseudoInverse");
 
   const int num_rows = covariance_matrix_->num_rows();
   const int* rows = covariance_matrix_->rows();
@@ -526,6 +550,7 @@
       values[idx] = dense_covariance(r, c);
     }
   }
+  event_logger.AddEvent("CopyToCovarianceMatrix");
   return true;
 };
 

internal/ceres/covariance_impl.h

diff --git a/internal/ceres/covariance_impl.h b/internal/ceres/covariance_impl.h
index 596c73f..42b262e 100644
--- a/internal/ceres/covariance_impl.h
+++ b/internal/ceres/covariance_impl.h
@@ -75,6 +75,8 @@
   ProblemImpl* problem_;
   Covariance::Options options_;
   Problem::EvaluateOptions evaluate_options_;
+  bool is_computed_;
+  bool is_valid_;
   map<const double*, int> parameter_block_to_row_index_;
   set<const double*> constant_parameter_blocks_;
   scoped_ptr<CompressedRowSparseMatrix> covariance_matrix_;