| // 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) | |
| // | |
| // The ProgramEvaluator runs the cost functions contained in each residual block | |
| // and stores the result into a jacobian. The particular type of jacobian is | |
| // abstracted out using two template parameters: | |
| // | |
| // - An "EvaluatePreparer" that is responsible for creating the array with | |
| // pointers to the jacobian blocks where the cost function evaluates to. | |
| // - A "JacobianWriter" that is responsible for storing the resulting | |
| // jacobian blocks in the passed sparse matrix. | |
| // | |
| // This abstraction affords an efficient evaluator implementation while still | |
| // supporting writing to multiple sparse matrix formats. For example, when the | |
| // ProgramEvaluator is parameterized for writing to block sparse matrices, the | |
| // residual jacobians are written directly into their final position in the | |
| // block sparse matrix by the user's CostFunction; there is no copying. | |
| // | |
| // The evaluation is threaded with OpenMP. | |
| // | |
| // The EvaluatePreparer and JacobianWriter interfaces are as follows: | |
| // | |
| // class EvaluatePreparer { | |
| // // Prepare the jacobians array for use as the destination of a call to | |
| // // a cost function's evaluate method. | |
| // void Prepare(const ResidualBlock* residual_block, | |
| // int residual_block_index, | |
| // SparseMatrix* jacobian, | |
| // double** jacobians); | |
| // } | |
| // | |
| // class JacobianWriter { | |
| // // Create a jacobian that this writer can write. Same as | |
| // // Evaluator::CreateJacobian. | |
| // SparseMatrix* CreateJacobian() const; | |
| // | |
| // // Create num_threads evaluate preparers. Caller owns result which must | |
| // // be freed with delete[]. Resulting preparers are valid while *this is. | |
| // EvaluatePreparer* CreateEvaluatePreparers(int num_threads); | |
| // | |
| // // Write the block jacobians from a residual block evaluation to the | |
| // // larger sparse jacobian. | |
| // void Write(int residual_id, | |
| // int residual_offset, | |
| // double** jacobians, | |
| // SparseMatrix* jacobian); | |
| // } | |
| // | |
| // Note: The ProgramEvaluator is not thread safe, since internally it maintains | |
| // some per-thread scratch space. | |
| #ifndef CERES_INTERNAL_PROGRAM_EVALUATOR_H_ | |
| #define CERES_INTERNAL_PROGRAM_EVALUATOR_H_ | |
| // This include must come before any #ifndef check on Ceres compile options. | |
| #include "ceres/internal/port.h" | |
| #ifdef CERES_USE_OPENMP | |
| #include <omp.h> | |
| #endif | |
| #include <map> | |
| #include <string> | |
| #include <vector> | |
| #include "ceres/execution_summary.h" | |
| #include "ceres/internal/eigen.h" | |
| #include "ceres/internal/scoped_ptr.h" | |
| #include "ceres/parameter_block.h" | |
| #include "ceres/program.h" | |
| #include "ceres/residual_block.h" | |
| #include "ceres/small_blas.h" | |
| namespace ceres { | |
| namespace internal { | |
| struct NullJacobianFinalizer { | |
| void operator()(SparseMatrix* jacobian, int num_parameters) {} | |
| }; | |
| template<typename EvaluatePreparer, | |
| typename JacobianWriter, | |
| typename JacobianFinalizer = NullJacobianFinalizer> | |
| class ProgramEvaluator : public Evaluator { | |
| public: | |
| ProgramEvaluator(const Evaluator::Options &options, Program* program) | |
| : options_(options), | |
| program_(program), | |
| jacobian_writer_(options, program), | |
| evaluate_preparers_( | |
| jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { | |
| #ifndef CERES_USE_OPENMP | |
| CHECK_EQ(1, options_.num_threads) | |
| << "OpenMP support is not compiled into this binary; " | |
| << "only options.num_threads=1 is supported."; | |
| #endif | |
| BuildResidualLayout(*program, &residual_layout_); | |
| evaluate_scratch_.reset(CreateEvaluatorScratch(*program, | |
| options.num_threads)); | |
| } | |
| // Implementation of Evaluator interface. | |
| SparseMatrix* CreateJacobian() const { | |
| return jacobian_writer_.CreateJacobian(); | |
| } | |
| bool Evaluate(const Evaluator::EvaluateOptions& evaluate_options, | |
| const double* state, | |
| double* cost, | |
| double* residuals, | |
| double* gradient, | |
| SparseMatrix* jacobian) { | |
| ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); | |
| ScopedExecutionTimer call_type_timer(gradient == NULL && jacobian == NULL | |
| ? "Evaluator::Residual" | |
| : "Evaluator::Jacobian", | |
| &execution_summary_); | |
| // The parameters are stateful, so set the state before evaluating. | |
| if (!program_->StateVectorToParameterBlocks(state)) { | |
| return false; | |
| } | |
| if (residuals != NULL) { | |
| VectorRef(residuals, program_->NumResiduals()).setZero(); | |
| } | |
| if (jacobian != NULL) { | |
| jacobian->SetZero(); | |
| } | |
| // Each thread gets it's own cost and evaluate scratch space. | |
| for (int i = 0; i < options_.num_threads; ++i) { | |
| evaluate_scratch_[i].cost = 0.0; | |
| if (gradient != NULL) { | |
| VectorRef(evaluate_scratch_[i].gradient.get(), | |
| program_->NumEffectiveParameters()).setZero(); | |
| } | |
| } | |
| // This bool is used to disable the loop if an error is encountered | |
| // without breaking out of it. The remaining loop iterations are still run, | |
| // but with an empty body, and so will finish quickly. | |
| bool abort = false; | |
| int num_residual_blocks = program_->NumResidualBlocks(); | |
| #pragma omp parallel for num_threads(options_.num_threads) | |
| for (int i = 0; i < num_residual_blocks; ++i) { | |
| // Disable the loop instead of breaking, as required by OpenMP. | |
| #pragma omp flush(abort) | |
| if (abort) { | |
| continue; | |
| } | |
| #ifdef CERES_USE_OPENMP | |
| int thread_id = omp_get_thread_num(); | |
| #else | |
| int thread_id = 0; | |
| #endif | |
| EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; | |
| EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; | |
| // Prepare block residuals if requested. | |
| const ResidualBlock* residual_block = program_->residual_blocks()[i]; | |
| double* block_residuals = NULL; | |
| if (residuals != NULL) { | |
| block_residuals = residuals + residual_layout_[i]; | |
| } else if (gradient != NULL) { | |
| block_residuals = scratch->residual_block_residuals.get(); | |
| } | |
| // Prepare block jacobians if requested. | |
| double** block_jacobians = NULL; | |
| if (jacobian != NULL || gradient != NULL) { | |
| preparer->Prepare(residual_block, | |
| i, | |
| jacobian, | |
| scratch->jacobian_block_ptrs.get()); | |
| block_jacobians = scratch->jacobian_block_ptrs.get(); | |
| } | |
| // Evaluate the cost, residuals, and jacobians. | |
| double block_cost; | |
| if (!residual_block->Evaluate( | |
| evaluate_options.apply_loss_function, | |
| &block_cost, | |
| block_residuals, | |
| block_jacobians, | |
| scratch->residual_block_evaluate_scratch.get())) { | |
| abort = true; | |
| // This ensures that the OpenMP threads have a consistent view of 'abort'. Do | |
| // the flush inside the failure case so that there is usually only one | |
| // synchronization point per loop iteration instead of two. | |
| #pragma omp flush(abort) | |
| continue; | |
| } | |
| scratch->cost += block_cost; | |
| // Store the jacobians, if they were requested. | |
| if (jacobian != NULL) { | |
| jacobian_writer_.Write(i, | |
| residual_layout_[i], | |
| block_jacobians, | |
| jacobian); | |
| } | |
| // Compute and store the gradient, if it was requested. | |
| if (gradient != NULL) { | |
| int num_residuals = residual_block->NumResiduals(); | |
| int num_parameter_blocks = residual_block->NumParameterBlocks(); | |
| for (int j = 0; j < num_parameter_blocks; ++j) { | |
| const ParameterBlock* parameter_block = | |
| residual_block->parameter_blocks()[j]; | |
| if (parameter_block->IsConstant()) { | |
| continue; | |
| } | |
| MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( | |
| block_jacobians[j], | |
| num_residuals, | |
| parameter_block->LocalSize(), | |
| block_residuals, | |
| scratch->gradient.get() + parameter_block->delta_offset()); | |
| } | |
| } | |
| } | |
| if (!abort) { | |
| const int num_parameters = program_->NumEffectiveParameters(); | |
| // Sum the cost and gradient (if requested) from each thread. | |
| (*cost) = 0.0; | |
| if (gradient != NULL) { | |
| VectorRef(gradient, num_parameters).setZero(); | |
| } | |
| for (int i = 0; i < options_.num_threads; ++i) { | |
| (*cost) += evaluate_scratch_[i].cost; | |
| if (gradient != NULL) { | |
| VectorRef(gradient, num_parameters) += | |
| VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); | |
| } | |
| } | |
| // Finalize the Jacobian if it is available. | |
| // `num_parameters` is passed to the finalizer so that additional | |
| // storage can be reserved for additional diagonal elements if | |
| // necessary. | |
| if (jacobian != NULL) { | |
| JacobianFinalizer f; | |
| f(jacobian, num_parameters); | |
| } | |
| } | |
| return !abort; | |
| } | |
| bool Plus(const double* state, | |
| const double* delta, | |
| double* state_plus_delta) const { | |
| return program_->Plus(state, delta, state_plus_delta); | |
| } | |
| int NumParameters() const { | |
| return program_->NumParameters(); | |
| } | |
| int NumEffectiveParameters() const { | |
| return program_->NumEffectiveParameters(); | |
| } | |
| int NumResiduals() const { | |
| return program_->NumResiduals(); | |
| } | |
| virtual map<string, int> CallStatistics() const { | |
| return execution_summary_.calls(); | |
| } | |
| virtual map<string, double> TimeStatistics() const { | |
| return execution_summary_.times(); | |
| } | |
| private: | |
| // Per-thread scratch space needed to evaluate and store each residual block. | |
| struct EvaluateScratch { | |
| void Init(int max_parameters_per_residual_block, | |
| int max_scratch_doubles_needed_for_evaluate, | |
| int max_residuals_per_residual_block, | |
| int num_parameters) { | |
| residual_block_evaluate_scratch.reset( | |
| new double[max_scratch_doubles_needed_for_evaluate]); | |
| gradient.reset(new double[num_parameters]); | |
| VectorRef(gradient.get(), num_parameters).setZero(); | |
| residual_block_residuals.reset( | |
| new double[max_residuals_per_residual_block]); | |
| jacobian_block_ptrs.reset( | |
| new double*[max_parameters_per_residual_block]); | |
| } | |
| double cost; | |
| scoped_array<double> residual_block_evaluate_scratch; | |
| // The gradient in the local parameterization. | |
| scoped_array<double> gradient; | |
| // Enough space to store the residual for the largest residual block. | |
| scoped_array<double> residual_block_residuals; | |
| scoped_array<double*> jacobian_block_ptrs; | |
| }; | |
| static void BuildResidualLayout(const Program& program, | |
| vector<int>* residual_layout) { | |
| const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); | |
| residual_layout->resize(program.NumResidualBlocks()); | |
| int residual_pos = 0; | |
| for (int i = 0; i < residual_blocks.size(); ++i) { | |
| const int num_residuals = residual_blocks[i]->NumResiduals(); | |
| (*residual_layout)[i] = residual_pos; | |
| residual_pos += num_residuals; | |
| } | |
| } | |
| // Create scratch space for each thread evaluating the program. | |
| static EvaluateScratch* CreateEvaluatorScratch(const Program& program, | |
| int num_threads) { | |
| int max_parameters_per_residual_block = | |
| program.MaxParametersPerResidualBlock(); | |
| int max_scratch_doubles_needed_for_evaluate = | |
| program.MaxScratchDoublesNeededForEvaluate(); | |
| int max_residuals_per_residual_block = | |
| program.MaxResidualsPerResidualBlock(); | |
| int num_parameters = program.NumEffectiveParameters(); | |
| EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; | |
| for (int i = 0; i < num_threads; i++) { | |
| evaluate_scratch[i].Init(max_parameters_per_residual_block, | |
| max_scratch_doubles_needed_for_evaluate, | |
| max_residuals_per_residual_block, | |
| num_parameters); | |
| } | |
| return evaluate_scratch; | |
| } | |
| Evaluator::Options options_; | |
| Program* program_; | |
| JacobianWriter jacobian_writer_; | |
| scoped_array<EvaluatePreparer> evaluate_preparers_; | |
| scoped_array<EvaluateScratch> evaluate_scratch_; | |
| vector<int> residual_layout_; | |
| ::ceres::internal::ExecutionSummary execution_summary_; | |
| }; | |
| } // namespace internal | |
| } // namespace ceres | |
| #endif // CERES_INTERNAL_PROGRAM_EVALUATOR_H_ |