Add profiling to covariance estimation.
Prevent GetCovarianceBlock from being called before
Compute or when Compute failed.
Change-Id: I5c28d27a88081e230d316c5e365d3e21d6e23376
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;
};
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_;
