diff options
Diffstat (limited to 'internal/ceres/evaluator.cc')
| -rw-r--r-- | internal/ceres/evaluator.cc | 147 |
1 files changed, 147 insertions, 0 deletions
diff --git a/internal/ceres/evaluator.cc b/internal/ceres/evaluator.cc new file mode 100644 index 0000000..a3ce6f0 --- /dev/null +++ b/internal/ceres/evaluator.cc @@ -0,0 +1,147 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include <vector> +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_jacobian_writer.h" +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/crs_matrix.h" +#include "ceres/dense_jacobian_writer.h" +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" +#include "ceres/program_evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +Evaluator::~Evaluator() {} + +Evaluator* Evaluator::Create(const Evaluator::Options& options, + Program* program, + string* error) { + switch (options.linear_solver_type) { + case DENSE_QR: + case DENSE_NORMAL_CHOLESKY: + return new ProgramEvaluator<ScratchEvaluatePreparer, + DenseJacobianWriter>(options, + program); + case DENSE_SCHUR: + case SPARSE_SCHUR: + case ITERATIVE_SCHUR: + case CGNR: + return new ProgramEvaluator<BlockEvaluatePreparer, + BlockJacobianWriter>(options, + program); + case SPARSE_NORMAL_CHOLESKY: + return new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(options, + program); + default: + *error = "Invalid Linear Solver Type. Unable to create evaluator."; + return NULL; + } +} + +bool Evaluator::Evaluate(Program* program, + int num_threads, + double* cost, + vector<double>* residuals, + vector<double>* gradient, + CRSMatrix* output_jacobian) { + CHECK_GE(num_threads, 1) + << "This is a Ceres bug; please contact the developers!"; + CHECK_NOTNULL(cost); + + // Setup the Parameter indices and offsets before an evaluator can + // be constructed and used. + program->SetParameterOffsetsAndIndex(); + + Evaluator::Options evaluator_options; + evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; + evaluator_options.num_threads = 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!"; + return false; + } + + if (residuals !=NULL) { + residuals->resize(evaluator->NumResiduals()); + } + + if (gradient != NULL) { + gradient->resize(evaluator->NumEffectiveParameters()); + } + + scoped_ptr<CompressedRowSparseMatrix> jacobian; + if (output_jacobian != NULL) { + 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()); + + if (!evaluator->Evaluate(parameters.data(), + cost, + residuals != NULL ? &(*residuals)[0] : NULL, + gradient != NULL ? &(*gradient)[0] : NULL, + jacobian.get())) { + return false; + } + + if (output_jacobian != NULL) { + jacobian->ToCRSMatrix(output_jacobian); + } + + return true; +} + +} // namespace internal +} // namespace ceres |