Problem::Evaluate implementation.

1. Add Problem::Evaluate and tests.
2. Remove Solver::Summary::initial/final_*
3. Remove Solver::Options::return_* members.
4. Various cpplint cleanups.

Change-Id: I4266de53489896f72d9c6798c5efde6748d68a47

internal/ceres/problem_impl.cc

@@ -37,7 +37,11 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include "ceres/casts.h"
+#include "ceres/compressed_row_sparse_matrix.h"
 #include "ceres/cost_function.h"
+#include "ceres/crs_matrix.h"
+#include "ceres/evaluator.h"
 #include "ceres/loss_function.h"
 #include "ceres/map_util.h"
 #include "ceres/parameter_block.h"
@@ -224,7 +228,7 @@ ResidualBlock* ProblemImpl::AddResidualBlock(
     if (duplicate_items != sorted_parameter_blocks.end()) {
       string blocks;
       for (int i = 0; i < parameter_blocks.size(); ++i) {
-        blocks += internal::StringPrintf(" %p ", parameter_blocks[i]);
+        blocks += StringPrintf(" %p ", parameter_blocks[i]);
       }
 
       LOG(FATAL) << "Duplicate parameter blocks in a residual parameter "
@@ -513,6 +517,164 @@ void ProblemImpl::SetParameterization(
       ->SetParameterization(local_parameterization);
 }
 
+bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
+                           double* cost,
+                           vector<double>* residuals,
+                           vector<double>* gradient,
+                           CRSMatrix* jacobian) {
+  if (cost == NULL &&
+      residuals == NULL &&
+      gradient == NULL &&
+      jacobian == NULL) {
+    LOG(INFO) << "Nothing to do.";
+    return true;
+  }
+
+  // If the user supplied residual blocks, then use them, otherwise
+  // take the residual blocks from the underlying program.
+  Program program;
+  *program.mutable_residual_blocks() =
+      ((evaluate_options.residual_blocks.size() > 0)
+       ? evaluate_options.residual_blocks : program_->residual_blocks());
+
+  const vector<double*>& parameter_block_ptrs =
+      evaluate_options.parameter_blocks;
+
+  vector<ParameterBlock*> variable_parameter_blocks;
+  vector<ParameterBlock*>& parameter_blocks =
+      *program.mutable_parameter_blocks();
+
+  if (parameter_block_ptrs.size() == 0) {
+    // The user did not provide any parameter blocks, so default to
+    // using all the parameter blocks in the order that they are in
+    // the underlying program object.
+    parameter_blocks = program_->parameter_blocks();
+  } else {
+    // The user supplied a vector of parameter blocks. Using this list
+    // requires a number of steps.
+
+    // 1. Convert double* into ParameterBlock*
+    parameter_blocks.resize(parameter_block_ptrs.size());
+    for (int i = 0; i < parameter_block_ptrs.size(); ++i) {
+      parameter_blocks[i] =
+          FindOrDie(parameter_block_map_, parameter_block_ptrs[i]);
+    }
+
+    // 2. The user may have only supplied a subset of parameter
+    // blocks, so identify the ones that are not supplied by the user
+    // and are NOT constant. These parameter blocks are stored in
+    // variable_parameter_blocks.
+    //
+    // To ensure that the parameter blocks are not included in the
+    // columns of the jacobian, we need to make sure that they are
+    // constant during evaluation and then make them variable again
+    // after we are done.
+    vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks());
+    vector<ParameterBlock*> included_parameter_blocks(
+        program.parameter_blocks());
+
+    vector<ParameterBlock*> excluded_parameter_blocks;
+    sort(all_parameter_blocks.begin(), all_parameter_blocks.end());
+    sort(included_parameter_blocks.begin(), included_parameter_blocks.end());
+    set_difference(all_parameter_blocks.begin(),
+                   all_parameter_blocks.end(),
+                   included_parameter_blocks.begin(),
+                   included_parameter_blocks.end(),
+                   back_inserter(excluded_parameter_blocks));
+
+    variable_parameter_blocks.reserve(excluded_parameter_blocks.size());
+    for (int i = 0; i < excluded_parameter_blocks.size(); ++i) {
+      ParameterBlock* parameter_block = excluded_parameter_blocks[i];
+      if (!parameter_block->IsConstant()) {
+        variable_parameter_blocks.push_back(parameter_block);
+        parameter_block->SetConstant();
+      }
+    }
+  }
+
+  // Setup the Parameter indices and offsets before an evaluator can
+  // be constructed and used.
+  program.SetParameterOffsetsAndIndex();
+
+  Evaluator::Options evaluator_options;
+
+  // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or
+  // CXSparse, here it just being used for telling the evaluator to
+  // use a SparseRowCompressedMatrix for the jacobian. This is because
+  // the Evaluator decides the storage for the Jacobian based on the
+  // type of linear solver being used.
+  evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
+  evaluator_options.num_threads = evaluate_options.num_threads;
+
+  string error;
+  scoped_ptr<Evaluator> evaluator(
+      Evaluator::Create(evaluator_options, &program, &error));
+  if (evaluator.get() == NULL) {
+    LOG(ERROR) << "Unable to create an Evaluator object. "
+               << "Error: " << error
+               << "This is a Ceres bug; please contact the developers!";
+
+    // Make the parameter blocks that were temporarily marked
+    // constant, variable again.
+    for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
+      variable_parameter_blocks[i]->SetVarying();
+    }
+    return false;
+  }
+
+  if (residuals !=NULL) {
+    residuals->resize(evaluator->NumResiduals());
+  }
+
+  if (gradient != NULL) {
+    gradient->resize(evaluator->NumEffectiveParameters());
+  }
+
+  scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian;
+  if (jacobian != NULL) {
+    tmp_jacobian.reset(
+        down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian()));
+  }
+
+  // Point the state pointers to the user state pointers. This is
+  // needed so that we can extract a parameter vector which is then
+  // passed to Evaluator::Evaluate.
+  program.SetParameterBlockStatePtrsToUserStatePtrs();
+
+  // Copy the value of the parameter blocks into a vector, since the
+  // Evaluate::Evaluate method needs its input as such. The previous
+  // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that
+  // these values are the ones corresponding to the actual state of
+  // the parameter blocks, rather than the temporary state pointer
+  // used for evaluation.
+  Vector parameters(program.NumParameters());
+  program.ParameterBlocksToStateVector(parameters.data());
+
+  double tmp_cost = 0;
+  bool status = evaluator->Evaluate(parameters.data(),
+                                    &tmp_cost,
+                                    residuals != NULL ? &(*residuals)[0] : NULL,
+                                    gradient != NULL ? &(*gradient)[0] : NULL,
+                                    tmp_jacobian.get());
+
+  // Make the parameter blocks that were temporarirly marked
+  // constant, variable again.
+  for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
+    variable_parameter_blocks[i]->SetVarying();
+  }
+
+  if (status) {
+    if (cost != NULL) {
+      *cost = tmp_cost;
+    }
+    if (jacobian != NULL) {
+      tmp_jacobian->ToCRSMatrix(jacobian);
+    }
+  }
+
+  return status;
+}
+
 int ProblemImpl::NumParameterBlocks() const {
   return program_->NumParameterBlocks();
 }