| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2015 Google Inc. All rights reserved.// http://ceres-solver.org///// 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: sameeragarwal@google.com (Sameer Agarwal)#include "ceres/trust_region_preprocessor.h"#include <numeric>#include <string>#include "ceres/callbacks.h"#include "ceres/context_impl.h"#include "ceres/evaluator.h"#include "ceres/linear_solver.h"#include "ceres/minimizer.h"#include "ceres/parameter_block.h"#include "ceres/preconditioner.h"#include "ceres/preprocessor.h"#include "ceres/problem_impl.h"#include "ceres/program.h"#include "ceres/reorder_program.h"#include "ceres/suitesparse.h"#include "ceres/trust_region_strategy.h"#include "ceres/wall_time.h"namespace ceres {namespace internal {using std::vector;namespace {ParameterBlockOrdering* CreateDefaultLinearSolverOrdering(    const Program& program) {  ParameterBlockOrdering* ordering = new ParameterBlockOrdering;  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);  }  return ordering;}// 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));}void AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver(    Solver::Options* options) {  if (!IsSchurType(options->linear_solver_type)) {    return;  }  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);  std::string message;  if (linear_solver_type_given == ITERATIVE_SCHUR) {    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));  } else {    message =        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;}// For Schur type and SPARSE_NORMAL_CHOLESKY linear solvers, reorder// the program to reduce fill-in and increase cache coherency.bool ReorderProgram(PreprocessedProblem* pp) {  const Solver::Options& options = pp->options;  if (IsSchurType(options.linear_solver_type)) {    return ReorderProgramForSchurTypeLinearSolver(        options.linear_solver_type,        options.sparse_linear_algebra_library_type,        pp->problem->parameter_map(),        options.linear_solver_ordering.get(),        pp->reduced_program.get(),        &pp->error);  }  if (options.linear_solver_type == SPARSE_NORMAL_CHOLESKY &&      !options.dynamic_sparsity) {    return ReorderProgramForSparseNormalCholesky(        options.sparse_linear_algebra_library_type,        *options.linear_solver_ordering,        pp->reduced_program.get(),        &pp->error);  }  return true;}// Configure and create a linear solver object. In doing so, if a// sparse direct factorization based linear solver is being used, then// find a fill reducing ordering and reorder the program as needed// too.bool SetupLinearSolver(PreprocessedProblem* pp) {  Solver::Options& options = pp->options;  if (options.linear_solver_ordering.get() == NULL) {    // If the user has not supplied a linear solver ordering, then we    // assume that they are giving all the freedom to us in choosing    // the best possible ordering. This intent can be indicated by    // putting all the parameter blocks in the same elimination group.    options.linear_solver_ordering.reset(        CreateDefaultLinearSolverOrdering(*pp->reduced_program));  } else {    // If the user supplied an ordering, then check if the first    // elimination group is still non-empty after the reduced problem    // has been constructed.    //    // This is important for Schur type linear solvers, where the    // first elimination group is special -- it needs to be an    // independent set.    //    // If the first elimination group is empty, then we cannot use the    // user's requested linear solver (and a preconditioner as the    // case may be) so we must use a different one.    ParameterBlockOrdering* ordering = options.linear_solver_ordering.get();    const int min_group_id = ordering->MinNonZeroGroup();    ordering->Remove(pp->removed_parameter_blocks);    if (IsSchurType(options.linear_solver_type) &&        min_group_id != ordering->MinNonZeroGroup()) {      AlternateLinearSolverAndPreconditionerForSchurTypeLinearSolver(          &options);    }  }  // Reorder the program to reduce fill in and improve cache coherency  // of the Jacobian.  if (!ReorderProgram(pp)) {    return false;  }  // Configure the linear solver.  pp->linear_solver_options = LinearSolver::Options();  pp->linear_solver_options.min_num_iterations =      options.min_linear_solver_iterations;  pp->linear_solver_options.max_num_iterations =      options.max_linear_solver_iterations;  pp->linear_solver_options.type = options.linear_solver_type;  pp->linear_solver_options.preconditioner_type = options.preconditioner_type;  pp->linear_solver_options.visibility_clustering_type =      options.visibility_clustering_type;  pp->linear_solver_options.sparse_linear_algebra_library_type =      options.sparse_linear_algebra_library_type;  pp->linear_solver_options.dense_linear_algebra_library_type =      options.dense_linear_algebra_library_type;  pp->linear_solver_options.use_explicit_schur_complement =      options.use_explicit_schur_complement;  pp->linear_solver_options.dynamic_sparsity = options.dynamic_sparsity;  pp->linear_solver_options.num_threads = options.num_threads;  pp->linear_solver_options.use_postordering = options.use_postordering;  pp->linear_solver_options.context = pp->problem->context();  if (IsSchurType(pp->linear_solver_options.type)) {    OrderingToGroupSizes(options.linear_solver_ordering.get(),                         &pp->linear_solver_options.elimination_groups);    // Schur type solvers expect at least two elimination groups. If    // there is only one elimination group, then it is guaranteed that    // this group only contains e_blocks. Thus we add a dummy    // elimination group with zero blocks in it.    if (pp->linear_solver_options.elimination_groups.size() == 1) {      pp->linear_solver_options.elimination_groups.push_back(0);    }    if (options.linear_solver_type == SPARSE_SCHUR) {      // When using SPARSE_SCHUR, we ignore the user's postordering      // preferences in certain cases.      //      // 1. SUITE_SPARSE is the sparse linear algebra library requested      //    but cholmod_camd is not available.      // 2. CX_SPARSE is the sparse linear algebra library requested.      //      // This ensures that the linear solver does not assume that a      // fill-reducing pre-ordering has been done.      //      // TODO(sameeragarwal): Implement the reordering of parameter      // blocks for CX_SPARSE.      if ((options.sparse_linear_algebra_library_type == SUITE_SPARSE &&           !SuiteSparse::           IsConstrainedApproximateMinimumDegreeOrderingAvailable()) ||          (options.sparse_linear_algebra_library_type == CX_SPARSE)) {        pp->linear_solver_options.use_postordering = true;      }    }  }  pp->linear_solver.reset(LinearSolver::Create(pp->linear_solver_options));  return (pp->linear_solver.get() != NULL);}// Configure and create the evaluator.bool SetupEvaluator(PreprocessedProblem* pp) {  const Solver::Options& options = pp->options;  pp->evaluator_options = Evaluator::Options();  pp->evaluator_options.linear_solver_type = options.linear_solver_type;  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();  }  pp->evaluator_options.num_threads = options.num_threads;  pp->evaluator_options.dynamic_sparsity = options.dynamic_sparsity;  pp->evaluator_options.context = pp->problem->context();  pp->evaluator_options.evaluation_callback = options.evaluation_callback;  pp->evaluator.reset(Evaluator::Create(pp->evaluator_options,                                        pp->reduced_program.get(),                                        &pp->error));  return (pp->evaluator.get() != NULL);}// If the user requested inner iterations, then find an inner// iteration ordering as needed and configure and create a// CoordinateDescentMinimizer object to perform the inner iterations.bool SetupInnerIterationMinimizer(PreprocessedProblem* pp) {  Solver::Options& options = pp->options;  if (!options.use_inner_iterations) {    return true;  }  // With just one parameter block, the outer iteration of the trust  // region method and inner iterations are doing exactly the same  // thing, and thus inner iterations are not needed.  if (pp->reduced_program->NumParameterBlocks() == 1) {    LOG(WARNING) << "Reduced problem only contains one parameter block."                 << "Disabling inner iterations.";    return true;  }  if (options.inner_iteration_ordering.get() != NULL) {    // If the user supplied an ordering, then remove the set of    // inactive parameter blocks from it    options.inner_iteration_ordering->Remove(pp->removed_parameter_blocks);    if (options.inner_iteration_ordering->NumElements() == 0) {      LOG(WARNING) << "No remaining elements in the inner iteration ordering.";      return true;    }    // Validate the reduced ordering.    if (!CoordinateDescentMinimizer::IsOrderingValid(            *pp->reduced_program,            *options.inner_iteration_ordering,            &pp->error)) {      return false;    }  } else {    // The user did not supply an ordering, so create one.    options.inner_iteration_ordering.reset(        CoordinateDescentMinimizer::CreateOrdering(*pp->reduced_program));  }  pp->inner_iteration_minimizer.reset(      new CoordinateDescentMinimizer(pp->problem->context()));  return pp->inner_iteration_minimizer->Init(*pp->reduced_program,                                             pp->problem->parameter_map(),                                             *options.inner_iteration_ordering,                                             &pp->error);}// Configure and create a TrustRegionMinimizer object.void SetupMinimizerOptions(PreprocessedProblem* pp) {  const Solver::Options& options = pp->options;  SetupCommonMinimizerOptions(pp);  pp->minimizer_options.is_constrained =      pp->reduced_program->IsBoundsConstrained();  pp->minimizer_options.jacobian.reset(pp->evaluator->CreateJacobian());  pp->minimizer_options.inner_iteration_minimizer =      pp->inner_iteration_minimizer;  TrustRegionStrategy::Options strategy_options;  strategy_options.linear_solver = pp->linear_solver.get();  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;  strategy_options.trust_region_strategy_type =      options.trust_region_strategy_type;  strategy_options.dogleg_type = options.dogleg_type;  pp->minimizer_options.trust_region_strategy.reset(      CHECK_NOTNULL(TrustRegionStrategy::Create(strategy_options)));}}  // namespaceTrustRegionPreprocessor::~TrustRegionPreprocessor() {}bool TrustRegionPreprocessor::Preprocess(const Solver::Options& options,                                         ProblemImpl* problem,                                         PreprocessedProblem* pp) {  CHECK_NOTNULL(pp);  pp->options = options;  ChangeNumThreadsIfNeeded(&pp->options);  pp->problem = problem;  Program* program = problem->mutable_program();  if (!IsProgramValid(*program, &pp->error)) {    return false;  }  pp->reduced_program.reset(      program->CreateReducedProgram(&pp->removed_parameter_blocks,                                    &pp->fixed_cost,                                    &pp->error));  if (pp->reduced_program.get() == NULL) {    return false;  }  if (pp->reduced_program->NumParameterBlocks() == 0) {    // The reduced problem has no parameter or residual blocks. There    // is nothing more to do.    return true;  }  if (!SetupLinearSolver(pp) ||      !SetupEvaluator(pp) ||      !SetupInnerIterationMinimizer(pp)) {    return false;  }  SetupMinimizerOptions(pp);  return true;}}  // namespace internal}  // namespace ceres
 |