MYNT-EYE-S-SDK/3rdparty/ceres-solver-1.11.0/internal/ceres/trust_region_preprocessor.cc

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

  // Ignore user's postordering preferences and force it to be true if
  // cholmod_camd is not available. This ensures that the linear
  // solver does not assume that a fill-reducing pre-ordering has been
  // done.
  pp->linear_solver_options.use_postordering = options.use_postordering;
  if (options.linear_solver_type == SPARSE_SCHUR &&
      options.sparse_linear_algebra_library_type == SUITE_SPARSE &&
      !SuiteSparse::IsConstrainedApproximateMinimumDegreeOrderingAvailable()) {
    pp->linear_solver_options.use_postordering = true;
  }

  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 (IsSchurType(pp->linear_solver_options.type) &&
      pp->linear_solver_options.elimination_groups.size() == 1) {
    pp->linear_solver_options.elimination_groups.push_back(0);
  }

  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