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

363 lines
14 KiB
C++
Raw Normal View History

2019-01-03 10:25:18 +02:00
// 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