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
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc
index ae4e7ed..bc378aa 100644
--- a/internal/ceres/problem_impl.cc
+++ b/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 @@
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 @@
->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();
}
