Improve the performance of DenseQRSolver

1. Reduce amount of reallocations.

Change-Id: I91b17c781ae94ed12014d647f0162cfce4f6ed7b

internal/ceres/coordinate_descent_minimizer.cc

@@ -106,11 +106,6 @@ bool CoordinateDescentMinimizer::Init(
     }
   }
 
-  LinearSolver::Options linear_solver_options;
-  linear_solver_options.type = DENSE_QR;
-  linear_solver_.reset(LinearSolver::Create(linear_solver_options));
-  CHECK_NOTNULL(linear_solver_.get());
-
   evaluator_options_.linear_solver_type = DENSE_QR;
   evaluator_options_.num_eliminate_blocks = 0;
   evaluator_options_.num_threads = 1;
@@ -129,6 +124,15 @@ void CoordinateDescentMinimizer::Minimize(
     parameter_block->SetConstant();
   }
 
+  scoped_array<LinearSolver*> linear_solvers(new LinearSolver*[options.num_threads]);
+
+  LinearSolver::Options linear_solver_options;
+  linear_solver_options.type = DENSE_QR;
+
+  for (int i = 0; i < options.num_threads; ++i) {
+    linear_solvers[i] = LinearSolver::Create(linear_solver_options);
+  }
+
   for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) {
     // No point paying the price for an OpemMP call if the set if of
     // size zero.
@@ -142,6 +146,11 @@ void CoordinateDescentMinimizer::Minimize(
     for (int j = independent_set_offsets_[i];
          j < independent_set_offsets_[i + 1];
          ++j) {
+#ifdef CERES_USE_OPENMP
+      int thread_id = omp_get_thread_num();
+#else
+      int thread_id = 0;
+#endif
 
       ParameterBlock* parameter_block = parameter_blocks_[j];
       const int old_index = parameter_block->index();
@@ -164,6 +173,7 @@ void CoordinateDescentMinimizer::Minimize(
       // we are fine.
       Solver::Summary inner_summary;
       Solve(&inner_program,
+            linear_solvers[thread_id],
             parameters + parameter_block->state_offset(),
             &inner_summary);
 
@@ -177,10 +187,15 @@ void CoordinateDescentMinimizer::Minimize(
   for (int i =  0; i < parameter_blocks_.size(); ++i) {
     parameter_blocks_[i]->SetVarying();
   }
+
+  for (int i = 0; i < options.num_threads; ++i) {
+    delete linear_solvers[i];
+  }
 }
 
 // Solve the optimization problem for one parameter block.
 void CoordinateDescentMinimizer::Solve(Program* program,
+                                       LinearSolver* linear_solver,
                                        double* parameter,
                                        Solver::Summary* summary) {
   *summary = Solver::Summary();
@@ -196,11 +211,11 @@ void CoordinateDescentMinimizer::Solve(Program* program,
   scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
   CHECK_NOTNULL(jacobian.get());
 
-  TrustRegionStrategy::Options trust_region_strategy_options;
-  trust_region_strategy_options.linear_solver = linear_solver_.get();
+  TrustRegionStrategy::Options trs_options;
+  trs_options.linear_solver = linear_solver;
+
   scoped_ptr<TrustRegionStrategy>trust_region_strategy(
-      TrustRegionStrategy::Create(trust_region_strategy_options));
-  CHECK_NOTNULL(trust_region_strategy.get());
+      CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options)));
 
   Minimizer::Options minimizer_options;
   minimizer_options.evaluator = evaluator.get();

internal/ceres/coordinate_descent_minimizer.h

@@ -65,6 +65,7 @@ class CoordinateDescentMinimizer : public Minimizer {
 
  private:
   void Solve(Program* program,
+             LinearSolver* linear_solver,
              double* parameters,
              Solver::Summary* summary);
 
@@ -77,7 +78,6 @@ class CoordinateDescentMinimizer : public Minimizer {
   vector<int> independent_set_offsets_;
 
   Evaluator::Options evaluator_options_;
-  scoped_ptr<LinearSolver> linear_solver_;
 };
 
 }  // namespace internal

internal/ceres/dense_jacobian_writer.h

@@ -62,7 +62,8 @@ class DenseJacobianWriter {
 
   SparseMatrix* CreateJacobian() const {
     return new DenseSparseMatrix(program_->NumResiduals(),
-                                 program_->NumEffectiveParameters());
+                                 program_->NumEffectiveParameters(),
+                                 true);
   }
 
   void Write(int residual_id,
@@ -87,8 +88,8 @@ class DenseJacobianWriter {
         continue;
       }
 
-      int parameter_block_size = parameter_block->LocalSize();
-      MatrixRef parameter_jacobian(jacobians[j],
+      const int parameter_block_size = parameter_block->LocalSize();
+      ConstMatrixRef parameter_jacobian(jacobians[j],
                                         num_residuals,
                                         parameter_block_size);
 

internal/ceres/dense_qr_solver.cc

@@ -62,17 +62,20 @@ LinearSolver::Summary DenseQRSolver::SolveImpl(
   }
 
   // rhs = [b;0] to account for the additional rows in the lhs.
-  Vector rhs(num_rows + ((per_solve_options.D != NULL) ? num_cols : 0));
-  rhs.setZero();
-  rhs.head(num_rows) = ConstVectorRef(b, num_rows);
+  const int augmented_num_rows = num_rows + ((per_solve_options.D != NULL) ? num_cols : 0);
+  if (rhs_.rows() != augmented_num_rows) {
+    rhs_.resize(augmented_num_rows);
+    rhs_.setZero();
+  }
+  rhs_.head(num_rows) = ConstVectorRef(b, num_rows);
 
   // Solve the system.
-  VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs);
+  VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs_);
 
   VLOG(3) << "A:\n" << A->matrix();
   VLOG(3) << "x:\n" << VectorRef(x, num_cols);
-  VLOG(3) << "b:\n" << rhs;
-  VLOG(3) << "error: " << (A->matrix() * VectorRef(x, num_cols) - rhs).norm();
+  VLOG(3) << "b:\n" << rhs_;
+  VLOG(3) << "error: " << (A->matrix() * VectorRef(x, num_cols) - rhs_).norm();
 
 
   if (per_solve_options.D != NULL) {

internal/ceres/dense_qr_solver.h

@@ -33,6 +33,7 @@
 #define CERES_INTERNAL_DENSE_QR_SOLVER_H_
 
 #include "ceres/linear_solver.h"
+#include "ceres/internal/eigen.h"
 #include "ceres/internal/macros.h"
 
 namespace ceres {
@@ -90,6 +91,7 @@ class DenseQRSolver: public DenseSparseMatrixSolver {
       double* x);
 
   const LinearSolver::Options options_;
+  Vector rhs_;
   CERES_DISALLOW_COPY_AND_ASSIGN(DenseQRSolver);
 };
 

internal/ceres/dense_sparse_matrix.cc

@@ -47,6 +47,18 @@ DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols)
   m_.setZero();
 }
 
+DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal)
+    : has_diagonal_appended_(false),
+      has_diagonal_reserved_(reserve_diagonal) {
+  // Allocate enough space for the diagonal.
+  if (reserve_diagonal) {
+    m_.resize(num_rows +  num_cols, num_cols);
+  } else {
+    m_.resize(num_rows, num_cols);
+  }
+  m_.setZero();
+}
+
 DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m)
     : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())),
       has_diagonal_appended_(false),

internal/ceres/dense_sparse_matrix.h

@@ -57,6 +57,7 @@ class DenseSparseMatrix : public SparseMatrix {
 #endif
 
   DenseSparseMatrix(int num_rows, int num_cols);
+  DenseSparseMatrix(int num_rows, int num_cols, bool reserve_diagonal);
 
   virtual ~DenseSparseMatrix() {}