diff options
Diffstat (limited to 'extern/ceres/internal/ceres/program_evaluator.h')
| -rw-r--r-- | extern/ceres/internal/ceres/program_evaluator.h | 252 |
1 files changed, 121 insertions, 131 deletions
diff --git a/extern/ceres/internal/ceres/program_evaluator.h b/extern/ceres/internal/ceres/program_evaluator.h index 74a812adeef..97ee590fbab 100644 --- a/extern/ceres/internal/ceres/program_evaluator.h +++ b/extern/ceres/internal/ceres/program_evaluator.h @@ -43,7 +43,7 @@ // 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 evaluation is threaded with OpenMP or C++ threads. // // The EvaluatePreparer and JacobianWriter interfaces are as follows: // @@ -82,16 +82,16 @@ // 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 <atomic> #include <map> +#include <memory> #include <string> #include <vector> + +#include "ceres/evaluation_callback.h" #include "ceres/execution_summary.h" #include "ceres/internal/eigen.h" -#include "ceres/internal/scoped_ptr.h" +#include "ceres/parallel_for.h" #include "ceres/parameter_block.h" #include "ceres/program.h" #include "ceres/residual_block.h" @@ -104,34 +104,33 @@ struct NullJacobianFinalizer { void operator()(SparseMatrix* jacobian, int num_parameters) {} }; -template<typename EvaluatePreparer, - typename JacobianWriter, - typename JacobianFinalizer = NullJacobianFinalizer> +template <typename EvaluatePreparer, + typename JacobianWriter, + typename JacobianFinalizer = NullJacobianFinalizer> class ProgramEvaluator : public Evaluator { public: - ProgramEvaluator(const Evaluator::Options &options, Program* program) + 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 +#ifdef CERES_NO_THREADS if (options_.num_threads > 1) { - LOG(WARNING) - << "OpenMP support is not compiled into this binary; " - << "only options.num_threads = 1 is supported. Switching " - << "to single threaded mode."; + LOG(WARNING) << "No threading support is compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; options_.num_threads = 1; } -#endif +#endif // CERES_NO_THREADS BuildResidualLayout(*program, &residual_layout_); - evaluate_scratch_.reset(CreateEvaluatorScratch(*program, - options.num_threads)); + evaluate_scratch_.reset( + CreateEvaluatorScratch(*program, options.num_threads)); } // Implementation of Evaluator interface. - SparseMatrix* CreateJacobian() const { + SparseMatrix* CreateJacobian() const final { return jacobian_writer_.CreateJacobian(); } @@ -140,133 +139,133 @@ class ProgramEvaluator : public Evaluator { double* cost, double* residuals, double* gradient, - SparseMatrix* jacobian) { + SparseMatrix* jacobian) final { ScopedExecutionTimer total_timer("Evaluator::Total", &execution_summary_); - ScopedExecutionTimer call_type_timer(gradient == NULL && jacobian == NULL - ? "Evaluator::Residual" - : "Evaluator::Jacobian", - &execution_summary_); + ScopedExecutionTimer call_type_timer( + gradient == nullptr && jacobian == nullptr ? "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) { + // Notify the user about a new evaluation point if they are interested. + if (options_.evaluation_callback != nullptr) { + program_->CopyParameterBlockStateToUserState(); + options_.evaluation_callback->PrepareForEvaluation( + /*jacobians=*/(gradient != nullptr || jacobian != nullptr), + evaluate_options.new_evaluation_point); + } + + if (residuals != nullptr) { VectorRef(residuals, program_->NumResiduals()).setZero(); } - if (jacobian != NULL) { + if (jacobian != nullptr) { 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) { + if (gradient != nullptr) { VectorRef(evaluate_scratch_[i].gradient.get(), - program_->NumEffectiveParameters()).setZero(); + 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; - } + const int num_residual_blocks = program_->NumResidualBlocks(); + // 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. + std::atomic_bool abort(false); + ParallelFor( + options_.context, + 0, + num_residual_blocks, + options_.num_threads, + [&](int thread_id, int i) { + if (abort) { + return; + } -#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(); - } + EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; + EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; - // 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(); - } + // Prepare block residuals if requested. + const ResidualBlock* residual_block = program_->residual_blocks()[i]; + double* block_residuals = nullptr; + if (residuals != nullptr) { + block_residuals = residuals + residual_layout_[i]; + } else if (gradient != nullptr) { + block_residuals = scratch->residual_block_residuals.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; - } + // Prepare block jacobians if requested. + double** block_jacobians = nullptr; + if (jacobian != nullptr || gradient != nullptr) { + preparer->Prepare(residual_block, + i, + jacobian, + scratch->jacobian_block_ptrs.get()); + block_jacobians = scratch->jacobian_block_ptrs.get(); + } - scratch->cost += block_cost; + // 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; + return; + } - // Store the jacobians, if they were requested. - if (jacobian != NULL) { - jacobian_writer_.Write(i, - residual_layout_[i], - block_jacobians, - jacobian); - } + scratch->cost += block_cost; - // 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; + // Store the jacobians, if they were requested. + if (jacobian != nullptr) { + jacobian_writer_.Write( + i, residual_layout_[i], block_jacobians, jacobian); } - MatrixTransposeVectorMultiply<Eigen::Dynamic, Eigen::Dynamic, 1>( - block_jacobians[j], - num_residuals, - parameter_block->LocalSize(), - block_residuals, - scratch->gradient.get() + parameter_block->delta_offset()); - } - } - } + // Compute and store the gradient, if it was requested. + if (gradient != nullptr) { + 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) { + if (gradient != nullptr) { VectorRef(gradient, num_parameters).setZero(); } for (int i = 0; i < options_.num_threads; ++i) { (*cost) += evaluate_scratch_[i].cost; - if (gradient != NULL) { + if (gradient != nullptr) { VectorRef(gradient, num_parameters) += VectorRef(evaluate_scratch_[i].gradient.get(), num_parameters); } @@ -276,7 +275,7 @@ class ProgramEvaluator : public Evaluator { // `num_parameters` is passed to the finalizer so that additional // storage can be reserved for additional diagonal elements if // necessary. - if (jacobian != NULL) { + if (jacobian != nullptr) { JacobianFinalizer f; f(jacobian, num_parameters); } @@ -286,27 +285,19 @@ class ProgramEvaluator : public Evaluator { bool Plus(const double* state, const double* delta, - double* state_plus_delta) const { + double* state_plus_delta) const final { return program_->Plus(state, delta, state_plus_delta); } - int NumParameters() const { - return program_->NumParameters(); - } - int NumEffectiveParameters() const { + int NumParameters() const final { return program_->NumParameters(); } + int NumEffectiveParameters() const final { return program_->NumEffectiveParameters(); } - int NumResiduals() const { - return program_->NumResiduals(); - } - - virtual std::map<std::string, int> CallStatistics() const { - return execution_summary_.calls(); - } + int NumResiduals() const final { return program_->NumResiduals(); } - virtual std::map<std::string, double> TimeStatistics() const { - return execution_summary_.times(); + std::map<std::string, CallStatistics> Statistics() const final { + return execution_summary_.statistics(); } private: @@ -322,17 +313,16 @@ class ProgramEvaluator : public Evaluator { 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]); + jacobian_block_ptrs.reset(new double*[max_parameters_per_residual_block]); } double cost; - scoped_array<double> residual_block_evaluate_scratch; + std::unique_ptr<double[]> residual_block_evaluate_scratch; // The gradient in the local parameterization. - scoped_array<double> gradient; + std::unique_ptr<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; + std::unique_ptr<double[]> residual_block_residuals; + std::unique_ptr<double*[]> jacobian_block_ptrs; }; static void BuildResidualLayout(const Program& program, @@ -372,8 +362,8 @@ class ProgramEvaluator : public Evaluator { Evaluator::Options options_; Program* program_; JacobianWriter jacobian_writer_; - scoped_array<EvaluatePreparer> evaluate_preparers_; - scoped_array<EvaluateScratch> evaluate_scratch_; + std::unique_ptr<EvaluatePreparer[]> evaluate_preparers_; + std::unique_ptr<EvaluateScratch[]> evaluate_scratch_; std::vector<int> residual_layout_; ::ceres::internal::ExecutionSummary execution_summary_; }; |