Call EvaluationCallback before evaluating the fixed cost. Fixe a subtle bug in Program::RemoveFixedBlocks, where we call ResidualBlock::Evaluate on residual blocks with all constant parameter blocks without paying attention to the presence of an EvaluationCallback. In the process also run clang-format on some of the files touched by this change. https://github.com/ceres-solver/ceres-solver/issues/482 Change-Id: I342b66f6f975fdee2eef139a31f24d4a3e568e84
diff --git a/internal/ceres/evaluation_callback_test.cc b/internal/ceres/evaluation_callback_test.cc index f814b01..c8f257f 100644 --- a/internal/ceres/evaluation_callback_test.cc +++ b/internal/ceres/evaluation_callback_test.cc
@@ -28,24 +28,25 @@ // // Author: mierle@gmail.com (Keir Mierle) -#include "ceres/solver.h" +#include "ceres/evaluation_callback.h" #include <cmath> #include <limits> #include <vector> -#include "gtest/gtest.h" -#include "ceres/evaluation_callback.h" -#include "ceres/sized_cost_function.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" +#include "ceres/sized_cost_function.h" +#include "ceres/autodiff_cost_function.h" +#include "ceres/solver.h" +#include "gtest/gtest.h" namespace ceres { namespace internal { // Use an inline hash function to avoid portability wrangling. Algorithm from // Daniel Bernstein, known as the "djb2" hash. -template<typename T> +template <typename T> uint64_t Djb2Hash(const T* data, const int size) { uint64_t hash = 5381; const uint8_t* data_as_bytes = reinterpret_cast<const uint8_t*>(data); @@ -59,11 +60,9 @@ // Generally multiple inheritance is a terrible idea, but in this (test) // case it makes for a relatively elegant test implementation. -struct WigglyBowlCostFunctionAndEvaluationCallback : - SizedCostFunction<2, 2>, - EvaluationCallback { - - explicit WigglyBowlCostFunctionAndEvaluationCallback(double *parameter) +struct WigglyBowlCostFunctionAndEvaluationCallback : SizedCostFunction<2, 2>, + EvaluationCallback { + explicit WigglyBowlCostFunctionAndEvaluationCallback(double* parameter) : EvaluationCallback(), user_parameter_block(parameter), prepare_num_calls(0), @@ -134,10 +133,10 @@ residuals[0] = y - a * sin(x); residuals[1] = x; if (jacobians != NULL) { - (*jacobians)[2 * 0 + 0] = - a * cos(x); // df1/dx - (*jacobians)[2 * 0 + 1] = 1.0; // df1/dy - (*jacobians)[2 * 1 + 0] = 1.0; // df2/dx - (*jacobians)[2 * 1 + 1] = 0.0; // df2/dy + (*jacobians)[2 * 0 + 0] = -a * cos(x); // df1/dx + (*jacobians)[2 * 0 + 1] = 1.0; // df1/dy + (*jacobians)[2 * 1 + 0] = 1.0; // df2/dx + (*jacobians)[2 * 1 + 1] = 0.0; // df2/dy } uint64_t incoming_parameter_hash = Djb2Hash(*parameters, 2); @@ -212,26 +211,107 @@ EXPECT_GT(summary.num_unsuccessful_steps, 10); // Ensure PrepareForEvaluation() is called the appropriate number of times. - EXPECT_EQ(cost_function.prepare_num_calls, - // Unsuccessful steps are evaluated only once (no jacobians). - summary.num_unsuccessful_steps + - // Successful steps are evaluated twice: with and without jacobians. - 2 * summary.num_successful_steps - // Final iteration doesn't re-evaluate the jacobian. - // Note: This may be sensitive to tweaks to the TR algorithm; if - // this becomes too brittle, remove this EXPECT_EQ() entirely. - - 1); + EXPECT_EQ( + cost_function.prepare_num_calls, + // Unsuccessful steps are evaluated only once (no jacobians). + summary.num_unsuccessful_steps + + // Successful steps are evaluated twice: with and without jacobians. + 2 * summary.num_successful_steps + // Final iteration doesn't re-evaluate the jacobian. + // Note: This may be sensitive to tweaks to the TR algorithm; if + // this becomes too brittle, remove this EXPECT_EQ() entirely. + - 1); // Ensure the callback calls ran a reasonable number of times. EXPECT_GT(cost_function.prepare_num_calls, 0); EXPECT_GT(cost_function.evaluate_num_calls, 0); - EXPECT_EQ(cost_function.prepare_num_calls, - cost_function.evaluate_num_calls); + EXPECT_EQ(cost_function.prepare_num_calls, cost_function.evaluate_num_calls); // Ensure that the parameters did actually change. EXPECT_NE(Djb2Hash(parameters, 2), original_parameters_hash); } +// r = 1 - x +struct LinearResidual { + template <typename T> + bool operator()(const T* x, T* residuals) const { + residuals[0] = 1.0 - x[0]; + return true; + } + + static CostFunction* Create() { + return new AutoDiffCostFunction<LinearResidual, 1, 1>(new LinearResidual); + }; +}; + +// Increments a counter everytime PrepareForEvaluation is called. +class IncrementingEvaluationCallback : public EvaluationCallback { + public: + void PrepareForEvaluation(bool evaluate_jacobians, + bool new_evaluation_point) final { + (void) evaluate_jacobians; + (void) new_evaluation_point; + counter_ += 1.0; + } + + const double counter() const { return counter_; } + + private: + double counter_ = -1; +}; + + +// r = IncrementingEvaluationCallback::counter - x +struct EvaluationCallbackResidual { + EvaluationCallbackResidual(const IncrementingEvaluationCallback& callback) + : callback_(callback) {} + + template <typename T> + bool operator()(const T* x, T* residuals) const { + residuals[0] = callback_.counter() - x[0]; + return true; + } + + const IncrementingEvaluationCallback& callback_; + + static CostFunction* Create(IncrementingEvaluationCallback& callback) { + return new AutoDiffCostFunction<EvaluationCallbackResidual, 1, 1>( + new EvaluationCallbackResidual(callback)); + }; +}; + +// The following test, constructs a problem with residual blocks all +// of whose parameters are constant, so they are evaluated once +// outside the Minimizer to compute Solver::Summary::fixed_cost. +// +// The cost function for this residual block depends on the +// IncrementingEvaluationCallback::counter_, by checking the value of +// the fixed cost, we can check if the IncrementingEvaluationCallback +// was called. +TEST(EvaluationCallback, EvaluationCallbackIsCalledBeforeFixedCostIsEvaluated) { + double x = 1; + double y = 2; + std::unique_ptr<IncrementingEvaluationCallback> callback( + new IncrementingEvaluationCallback); + Problem::Options problem_options; + problem_options.evaluation_callback = callback.get(); + Problem problem(problem_options); + problem.AddResidualBlock(LinearResidual::Create(), nullptr, &x); + problem.AddResidualBlock( + EvaluationCallbackResidual::Create(*callback), + nullptr, + &y); + problem.SetParameterBlockConstant(&y); + + Solver::Options options; + options.linear_solver_type = DENSE_QR; + Solver::Summary summary; + Solve(options, &problem, &summary); + EXPECT_EQ(summary.fixed_cost, 2.0); + EXPECT_EQ(summary.final_cost, summary.fixed_cost); + EXPECT_GT(callback->counter(), 0); +} + static void WithLineSearchMinimizerImpl( LineSearchType line_search, LineSearchDirectionType line_search_direction, @@ -262,8 +342,7 @@ // Ensure the callback calls ran a reasonable number of times. EXPECT_GT(summary.num_line_search_steps, 10); EXPECT_GT(cost_function.prepare_num_calls, 30); - EXPECT_EQ(cost_function.prepare_num_calls, - cost_function.evaluate_num_calls); + EXPECT_EQ(cost_function.prepare_num_calls, cost_function.evaluate_num_calls); // Ensure that the parameters did actually change. EXPECT_NE(Djb2Hash(parameters, 2), original_parameters_hash);
diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc index 3a8f0e3..f1ded2e 100644 --- a/internal/ceres/program.cc +++ b/internal/ceres/program.cc
@@ -63,8 +63,7 @@ Program::Program(const Program& program) : parameter_blocks_(program.parameter_blocks_), residual_blocks_(program.residual_blocks_), - evaluation_callback_(program.evaluation_callback_){ -} + evaluation_callback_(program.evaluation_callback_) {} const vector<ParameterBlock*>& Program::parameter_blocks() const { return parameter_blocks_; @@ -86,7 +85,7 @@ return evaluation_callback_; } -bool Program::StateVectorToParameterBlocks(const double *state) { +bool Program::StateVectorToParameterBlocks(const double* state) { for (int i = 0; i < parameter_blocks_.size(); ++i) { if (!parameter_blocks_[i]->IsConstant() && !parameter_blocks_[i]->SetState(state)) { @@ -97,7 +96,7 @@ return true; } -void Program::ParameterBlocksToStateVector(double *state) const { +void Program::ParameterBlocksToStateVector(double* state) const { for (int i = 0; i < parameter_blocks_.size(); ++i) { parameter_blocks_[i]->GetState(state); state += parameter_blocks_[i]->Size(); @@ -197,7 +196,9 @@ "ParameterBlock: %p with size %d has at least one invalid value.\n" "First invalid value is at index: %d.\n" "Parameter block values: ", - array, size, invalid_index); + array, + size, + invalid_index); AppendArrayToString(size, array, message); return false; } @@ -244,7 +245,12 @@ "\nFirst infeasible value is at index: %d." "\nLower bound: %e, value: %e, upper bound: %e" "\nParameter block values: ", - parameters, size, j, lower_bound, parameters[j], upper_bound); + parameters, + size, + j, + lower_bound, + parameters[j], + upper_bound); AppendArrayToString(size, parameters, message); return false; } @@ -263,7 +269,11 @@ "\nFirst infeasible bound is at index: %d." "\nLower bound: %e, upper bound: %e" "\nParameter block values: ", - parameters, size, j, lower_bound, upper_bound); + parameters, + size, + j, + lower_bound, + upper_bound); AppendArrayToString(size, parameters, message); return false; } @@ -283,10 +293,9 @@ CHECK(error != nullptr); std::unique_ptr<Program> reduced_program(new Program(*this)); - if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks, - fixed_cost, - error)) { - return NULL; + if (!reduced_program->RemoveFixedBlocks( + removed_parameter_blocks, fixed_cost, error)) { + return nullptr; } reduced_program->SetParameterOffsetsAndIndex(); @@ -305,6 +314,8 @@ new double[MaxScratchDoublesNeededForEvaluate()]); *fixed_cost = 0.0; + bool need_to_call_prepare_for_evaluation = evaluation_callback_ != nullptr; + // Mark all the parameters as unused. Abuse the index member of the // parameter blocks for the marking. for (int i = 0; i < parameter_blocks_.size(); ++i) { @@ -334,18 +345,45 @@ continue; } + // This is an exceedingly rare case, where the user has residual + // blocks which are effectively constant but they are also + // performance sensitive enough to add an EvaluationCallback. + // + // In this case before we evaluate the cost of the constant + // residual blocks, we must call + // EvaluationCallback::PrepareForEvaluation(). Because this call + // can be costly, we only call this if we actually encounter a + // residual block with all constant parameter blocks. + // + // It is worth nothing that there is a minor inefficiency here, + // that the iteration 0 of TrustRegionMinimizer will also cause + // PrepareForEvaluation to be called on the same point, but with + // evaluate_jacobians = true. We could try and optimize this here, + // but given the rarity of this case, the additional complexity + // and long range dependency is not worth it. + if (need_to_call_prepare_for_evaluation) { + constexpr bool kNewPoint = true; + constexpr bool kDoNotEvaluateJacobians = false; + evaluation_callback_->PrepareForEvaluation(kDoNotEvaluateJacobians, + kNewPoint); + need_to_call_prepare_for_evaluation = false; + } + // The residual is constant and will be removed, so its cost is // added to the variable fixed_cost. double cost = 0.0; if (!residual_block->Evaluate(true, &cost, - NULL, - NULL, + nullptr, + nullptr, residual_block_evaluate_scratch.get())) { - *error = StringPrintf("Evaluation of the residual %d failed during " - "removal of fixed residual blocks.", i); + *error = StringPrintf( + "Evaluation of the residual %d failed during " + "removal of fixed residual blocks.", + i); return false; } + *fixed_cost += cost; } residual_blocks_.resize(num_active_residual_blocks); @@ -364,11 +402,9 @@ } parameter_blocks_.resize(num_active_parameter_blocks); - if (!(((NumResidualBlocks() == 0) && - (NumParameterBlocks() == 0)) || - ((NumResidualBlocks() != 0) && - (NumParameterBlocks() != 0)))) { - *error = "Congratulations, you found a bug in Ceres. Please report it."; + if (!(((NumResidualBlocks() == 0) && (NumParameterBlocks() == 0)) || + ((NumResidualBlocks() != 0) && (NumParameterBlocks() != 0)))) { + *error = "Congratulations, you found a bug in Ceres. Please report it."; return false; } @@ -387,8 +423,7 @@ const int num_parameter_blocks = residual_block->NumParameterBlocks(); int count = 0; for (int i = 0; i < num_parameter_blocks; ++i) { - count += independent_set.count( - parameter_blocks[i]->mutable_user_state()); + count += independent_set.count(parameter_blocks[i]->mutable_user_state()); } if (count > 1) { return false; @@ -442,13 +477,9 @@ return tsm; } -int Program::NumResidualBlocks() const { - return residual_blocks_.size(); -} +int Program::NumResidualBlocks() const { return residual_blocks_.size(); } -int Program::NumParameterBlocks() const { - return parameter_blocks_.size(); -} +int Program::NumParameterBlocks() const { return parameter_blocks_.size(); } int Program::NumResiduals() const { int num_residuals = 0; @@ -474,6 +505,9 @@ return num_parameters; } +// TODO(sameeragarwal): The following methods should just be updated +// incrementally and the values cached, rather than the linear +// complexity we have right now on every call. int Program::MaxScratchDoublesNeededForEvaluate() const { // Compute the scratch space needed for evaluate. int max_scratch_bytes_for_evaluate = 0; @@ -503,8 +537,8 @@ int Program::MaxParametersPerResidualBlock() const { int max_parameters = 0; for (int i = 0; i < residual_blocks_.size(); ++i) { - max_parameters = max(max_parameters, - residual_blocks_[i]->NumParameterBlocks()); + max_parameters = + max(max_parameters, residual_blocks_[i]->NumParameterBlocks()); } return max_parameters; } @@ -523,8 +557,8 @@ ret += StringPrintf("Number of parameters: %d\n", NumParameters()); ret += "Parameters:\n"; for (int i = 0; i < parameter_blocks_.size(); ++i) { - ret += StringPrintf("%d: %s\n", - i, parameter_blocks_[i]->ToString().c_str()); + ret += + StringPrintf("%d: %s\n", i, parameter_blocks_[i]->ToString().c_str()); } return ret; }
diff --git a/internal/ceres/program.h b/internal/ceres/program.h index dd5f6e1..7971299 100644 --- a/internal/ceres/program.h +++ b/internal/ceres/program.h
@@ -35,12 +35,11 @@ #include <set> #include <string> #include <vector> + #include "ceres/internal/port.h" +#include "ceres/evaluation_callback.h" namespace ceres { - -class EvaluationCallback; - namespace internal { class ParameterBlock; @@ -76,8 +75,8 @@ // computation of the Jacobian of its local parameterization. If // this computation fails for some reason, then this method returns // false and the state of the parameter blocks cannot be trusted. - bool StateVectorToParameterBlocks(const double *state); - void ParameterBlocksToStateVector(double *state) const; + bool StateVectorToParameterBlocks(const double* state); + void ParameterBlocksToStateVector(double* state) const; // Copy internal state to the user's parameters. void CopyParameterBlockStateToUserState();
diff --git a/internal/ceres/trust_region_preprocessor.cc b/internal/ceres/trust_region_preprocessor.cc index 65dee8a..b8c6b49 100644 --- a/internal/ceres/trust_region_preprocessor.cc +++ b/internal/ceres/trust_region_preprocessor.cc
@@ -32,6 +32,7 @@ #include <numeric> #include <string> + #include "ceres/callbacks.h" #include "ceres/context_impl.h" #include "ceres/evaluator.h" @@ -57,8 +58,7 @@ ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( const Program& program) { ParameterBlockOrdering* ordering = new ParameterBlockOrdering; - const vector<ParameterBlock*>& parameter_blocks = - program.parameter_blocks(); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); for (int i = 0; i < parameter_blocks.size(); ++i) { ordering->AddElementToGroup( const_cast<double*>(parameter_blocks[i]->user_state()), 0); @@ -69,8 +69,7 @@ // Check if all the user supplied values in the parameter blocks are // sane or not, and if the program is feasible or not. bool IsProgramValid(const Program& program, std::string* error) { - return (program.ParameterBlocksAreFinite(error) && - program.IsFeasible(error)); + return (program.ParameterBlocksAreFinite(error) && program.IsFeasible(error)); } void AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( @@ -82,27 +81,25 @@ const LinearSolverType linear_solver_type_given = options->linear_solver_type; const PreconditionerType preconditioner_type_given = options->preconditioner_type; - options->linear_solver_type = LinearSolver::LinearSolverForZeroEBlocks( - linear_solver_type_given); + options->linear_solver_type = + LinearSolver::LinearSolverForZeroEBlocks(linear_solver_type_given); std::string message; if (linear_solver_type_given == ITERATIVE_SCHUR) { - options->preconditioner_type = Preconditioner::PreconditionerForZeroEBlocks( - preconditioner_type_given); + options->preconditioner_type = + Preconditioner::PreconditionerForZeroEBlocks(preconditioner_type_given); message = - StringPrintf( - "No E blocks. Switching from %s(%s) to %s(%s).", - LinearSolverTypeToString(linear_solver_type_given), - PreconditionerTypeToString(preconditioner_type_given), - LinearSolverTypeToString(options->linear_solver_type), - PreconditionerTypeToString(options->preconditioner_type)); + StringPrintf("No E blocks. Switching from %s(%s) to %s(%s).", + LinearSolverTypeToString(linear_solver_type_given), + PreconditionerTypeToString(preconditioner_type_given), + LinearSolverTypeToString(options->linear_solver_type), + PreconditionerTypeToString(options->preconditioner_type)); } else { message = - StringPrintf( - "No E blocks. Switching from %s to %s.", - LinearSolverTypeToString(linear_solver_type_given), - LinearSolverTypeToString(options->linear_solver_type)); + StringPrintf("No E blocks. Switching from %s to %s.", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(options->linear_solver_type)); } VLOG_IF(1, options->logging_type != SILENT) << message; @@ -181,8 +178,7 @@ ordering->Remove(pp->removed_parameter_blocks); if (IsSchurType(options.linear_solver_type) && min_group_id != ordering->MinNonZeroGroup()) { - AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver( - &options); + AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver(&options); } } @@ -243,7 +239,7 @@ // blocks for CX_SPARSE. if ((options.sparse_linear_algebra_library_type == SUITE_SPARSE && !SuiteSparse:: - IsConstrainedApproximateMinimumDegreeOrderingAvailable()) || + IsConstrainedApproximateMinimumDegreeOrderingAvailable()) || (options.sparse_linear_algebra_library_type == CX_SPARSE)) { pp->linear_solver_options.use_postordering = true; } @@ -262,10 +258,9 @@ pp->evaluator_options.num_eliminate_blocks = 0; if (IsSchurType(options.linear_solver_type)) { pp->evaluator_options.num_eliminate_blocks = - options - .linear_solver_ordering - ->group_to_elements().begin() - ->second.size(); + options.linear_solver_ordering->group_to_elements() + .begin() + ->second.size(); } pp->evaluator_options.num_threads = options.num_threads; @@ -273,9 +268,8 @@ pp->evaluator_options.context = pp->problem->context(); pp->evaluator_options.evaluation_callback = pp->reduced_program->mutable_evaluation_callback(); - pp->evaluator.reset(Evaluator::Create(pp->evaluator_options, - pp->reduced_program.get(), - &pp->error)); + pp->evaluator.reset(Evaluator::Create( + pp->evaluator_options, pp->reduced_program.get(), &pp->error)); return (pp->evaluator != nullptr); } @@ -346,8 +340,7 @@ TrustRegionStrategy::Options strategy_options; strategy_options.linear_solver = pp->linear_solver.get(); - strategy_options.initial_radius = - options.initial_trust_region_radius; + strategy_options.initial_radius = options.initial_trust_region_radius; strategy_options.max_radius = options.max_trust_region_radius; strategy_options.min_lm_diagonal = options.min_lm_diagonal; strategy_options.max_lm_diagonal = options.max_lm_diagonal; @@ -361,8 +354,7 @@ } // namespace -TrustRegionPreprocessor::~TrustRegionPreprocessor() { -} +TrustRegionPreprocessor::~TrustRegionPreprocessor() {} bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options, ProblemImpl* problem, @@ -377,10 +369,8 @@ return false; } - pp->reduced_program.reset( - program->CreateReducedProgram(&pp->removed_parameter_blocks, - &pp->fixed_cost, - &pp->error)); + pp->reduced_program.reset(program->CreateReducedProgram( + &pp->removed_parameter_blocks, &pp->fixed_cost, &pp->error)); if (pp->reduced_program.get() == NULL) { return false; @@ -392,8 +382,7 @@ return true; } - if (!SetupLinearSolver(pp) || - !SetupEvaluator(pp) || + if (!SetupLinearSolver(pp) || !SetupEvaluator(pp) || !SetupInnerIterationMinimizer(pp)) { return false; }