// 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/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) {

  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_linear_solver_threads;

  pp->linear_solver_options.use_postordering = options.use_postordering;

  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.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);

  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)));

}

}  // namespace

TrustRegionPreprocessor::~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