diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 4d4f8734a..5e1fe0f82 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -87,6 +87,7 @@ SET(CERES_INTERNAL_SRC problem.cc problem_impl.cc program.cc + preprocessor.cc reorder_program.cc residual_block.cc residual_block_utils.cc @@ -104,6 +105,7 @@ SET(CERES_INTERNAL_SRC suitesparse.cc summary_utils.cc triplet_sparse_matrix.cc + trust_region_preprocessor.cc trust_region_minimizer.cc trust_region_strategy.cc types.cc @@ -289,6 +291,7 @@ IF (BUILD_TESTING AND GFLAGS) CERES_TEST(symmetric_linear_solver) CERES_TEST(triplet_sparse_matrix) CERES_TEST(trust_region_minimizer) + CERES_TEST(trust_region_preprocessor) CERES_TEST(unsymmetric_linear_solver) CERES_TEST(visibility) CERES_TEST(visibility_based_preconditioner) diff --git a/internal/ceres/preprocessor.cc b/internal/ceres/preprocessor.cc new file mode 100644 index 000000000..a572d2b7f --- /dev/null +++ b/internal/ceres/preprocessor.cc @@ -0,0 +1,111 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#include "ceres/callbacks.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/preprocessor.h" +#include "ceres/problem_impl.h" +#include "ceres/solver.h" +#include "ceres/trust_region_preprocessor.h" + +namespace ceres { +namespace internal { + +Preprocessor* Preprocessor::Create(MinimizerType minimizer_type) { + if (minimizer_type == TRUST_REGION) { + return new TrustRegionPreprocessor; + } + + // TODO(sameeragarwal): Add the LineSearchPreprocessor when it is + // ready. + + LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type; + return NULL; +} + +Preprocessor::~Preprocessor() { +} + +void ChangeNumThreadsIfNeeded(Solver::Options* options) { +#ifndef CERES_USE_OPENMP + if (options->num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads = 1 is supported. Switching " + << "to single threaded mode."; + options->num_threads = 1; + } + + // Only the Trust Region solver currently uses a linear solver. + if (options->minimizer_type == TRUST_REGION && + options->num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching " + << "to single threaded mode."; + options->num_linear_solver_threads = 1; + } +#endif // CERES_USE_OPENMP +} + +void SetupCommonMinimizerOptions(PreprocessedProblem* pp) { + const Solver::Options& options = pp->options; + Program* program = pp->reduced_program.get(); + + // Assuming that the parameter blocks in the program have been + // reordered as needed, extract them into a contiguous vector. + pp->reduced_parameters.resize(program->NumParameters()); + double* reduced_parameters = pp->reduced_parameters.data(); + program->ParameterBlocksToStateVector(reduced_parameters); + + Minimizer::Options& minimizer_options = pp->minimizer_options; + minimizer_options = Minimizer::Options(options); + minimizer_options.evaluator = pp->evaluator; + + if (options.logging_type != SILENT) { + pp->logging_callback.reset( + new LoggingCallback(options.minimizer_type, + options.minimizer_progress_to_stdout)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + pp->logging_callback.get()); + } + + if (options.update_state_every_iteration) { + pp->state_updating_callback.reset( + new StateUpdatingCallback(program, reduced_parameters)); + // This must get pushed to the front of the callbacks so that it + // is run before any of the user callbacks. + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + pp->state_updating_callback.get()); + } +} + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/preprocessor.h b/internal/ceres/preprocessor.h new file mode 100644 index 000000000..703ba9db8 --- /dev/null +++ b/internal/ceres/preprocessor.h @@ -0,0 +1,119 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_PREPROCESSOR_H_ +#define CERES_INTERNAL_PREPROCESSOR_H_ + +#include "ceres/coordinate_descent_minimizer.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/iteration_callback.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +struct PreprocessedProblem; + +// Given a Problem object and a Solver::Options object indicating the +// configuration of the solver, the job of the Preprocessor is to +// analyze the Problem and perform the setup needed to solve it using +// the desired Minimization algorithm. The setup involves, removing +// redundancies in the input problem (inactive parameter and residual +// blocks), finding fill reducing orderings as needed, configuring and +// creating various objects needed by the Minimizer to solve the +// problem such as an evaluator, a linear solver etc. +// +// Each Minimizer (LineSearchMinimizer and TrustRegionMinimizer) comes +// with a corresponding Preprocessor (LineSearchPreprocessor and +// TrustRegionPreprocessor) that knows about its needs and performs +// the preprocessing needed. +// +// The output of the Preprocessor is stored in a PreprocessedProblem +// object. +class Preprocessor { +public: + // Factory. + static Preprocessor* Create(MinimizerType minimizer_type); + virtual ~Preprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* pp) = 0; +}; + +// A PreprocessedProblem is the result of running the Preprocessor on +// a Problem and Solver::Options object. +struct PreprocessedProblem { + PreprocessedProblem() + : fixed_cost(0.0) { + } + + string error; + Solver::Options options; + LinearSolver::Options linear_solver_options; + Evaluator::Options evaluator_options; + Minimizer::Options minimizer_options; + + ProblemImpl* problem; + scoped_ptr gradient_checking_problem; + scoped_ptr reduced_program; + scoped_ptr linear_solver; + scoped_ptr logging_callback; + scoped_ptr state_updating_callback; + + shared_ptr evaluator; + shared_ptr inner_iteration_minimizer; + + vector removed_parameter_blocks; + Vector reduced_parameters; + double fixed_cost; +}; + +// Common functions used by various preprocessors. + +// If OpenMP support is not available and user has requested more than +// one threads, then set the *_num_threads options as needed to 1. +void ChangeNumThreadsIfNeeded(Solver::Options* options); + +// Extract the effective parameter vector from the preprocessed +// problem and setup bits of the Minimizer::Options object that are +// common to all Preprocessors. +void SetupCommonMinimizerOptions(PreprocessedProblem* pp); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PREPROCESSOR_H_ diff --git a/internal/ceres/reorder_program.cc b/internal/ceres/reorder_program.cc index 162bfb81c..9b40885fa 100644 --- a/internal/ceres/reorder_program.cc +++ b/internal/ceres/reorder_program.cc @@ -313,6 +313,15 @@ bool ReorderProgramForSchurTypeLinearSolver( ParameterBlockOrdering* parameter_block_ordering, Program* program, string* error) { + if (parameter_block_ordering->NumElements() != program->NumParameterBlocks()) { + *error = StringPrintf( + "The program has %d parameter blocks, but the parameter block " + "ordering has %d parameter blocks.", + program->NumParameterBlocks(), + parameter_block_ordering->NumElements()); + return false; + } + if (parameter_block_ordering->NumGroups() == 1) { // If the user supplied an parameter_block_ordering with just one // group, it is equivalent to the user supplying NULL as an @@ -340,7 +349,22 @@ bool ReorderProgramForSchurTypeLinearSolver( swap(*program->mutable_parameter_blocks(), schur_ordering); } else { // The user provided an ordering with more than one elimination - // group. Trust the user and apply the ordering. + // group. + + // Verify that the first elimination group is an independent set. + const set& first_elimination_group = + parameter_block_ordering + ->group_to_elements() + .begin() + ->second; + if (!program->IsParameterBlockSetIndependent(first_elimination_group)) { + *error = + StringPrintf("The first elimination group in the parameter block " + "ordering of size %zd is not an independent set", + first_elimination_group.size()); + return false; + } + if (!ApplyOrdering(parameter_map, *parameter_block_ordering, program, diff --git a/internal/ceres/trust_region_preprocessor.cc b/internal/ceres/trust_region_preprocessor.cc new file mode 100644 index 000000000..48b58c92c --- /dev/null +++ b/internal/ceres/trust_region_preprocessor.cc @@ -0,0 +1,354 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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 +#include +#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 { +namespace { + +ParameterBlockOrdering* CreateDefaultLinearSolverOrdering( + const Program& program) { + ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + const vector& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ordering->AddElementToGroup( + const_cast(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, 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); + + 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 block 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.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.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 diff --git a/internal/ceres/trust_region_preprocessor.h b/internal/ceres/trust_region_preprocessor.h new file mode 100644 index 000000000..ba70ff92e --- /dev/null +++ b/internal/ceres/trust_region_preprocessor.h @@ -0,0 +1,50 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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: sameragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ +#define CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ + +#include "ceres/preprocessor.h" + +namespace ceres { +namespace internal { + +class TrustRegionPreprocessor : public Preprocessor { + public: + virtual ~TrustRegionPreprocessor(); + virtual bool Preprocess(const Solver::Options& options, + ProblemImpl* problem, + PreprocessedProblem* preprocessed_problem); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRUST_REGION_PREPROCESSOR_H_ diff --git a/internal/ceres/trust_region_preprocessor_test.cc b/internal/ceres/trust_region_preprocessor_test.cc new file mode 100644 index 000000000..cfa22fe97 --- /dev/null +++ b/internal/ceres/trust_region_preprocessor_test.cc @@ -0,0 +1,399 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2014 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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 + +#include "ceres/ordered_groups.h" +#include "ceres/problem_impl.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" +#include "ceres/trust_region_preprocessor.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +TEST(TrustRegionPreprocessor, ZeroProblem) { + ProblemImpl problem; + Solver::Options options; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); +} + +TEST(TrustRegionPreprocessor, ProblemWithInvalidParameterBlock) { + ProblemImpl problem; + double x = 1.0/0.0; + problem.AddParameterBlock(&x, 1); + Solver::Options options; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_FALSE(preprocessor.Preprocess(options, &problem, &pp)); +} + +TEST(TrustRegionPreprocessor, ParameterBlockBoundsAreInvalid) { + ProblemImpl problem; + double x = 1.0; + problem.AddParameterBlock(&x, 1); + problem.SetParameterUpperBound(&x, 0, 1.0); + problem.SetParameterLowerBound(&x, 0, 2.0); + Solver::Options options; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_FALSE(preprocessor.Preprocess(options, &problem, &pp)); +} + +TEST(TrustRegionPreprocessor, ParamterBlockIsInfeasible) { + ProblemImpl problem; + double x = 3.0; + problem.AddParameterBlock(&x, 1); + problem.SetParameterUpperBound(&x, 0, 1.0); + problem.SetParameterLowerBound(&x, 0, 2.0); + problem.SetParameterBlockConstant(&x); + Solver::Options options; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_FALSE(preprocessor.Preprocess(options, &problem, &pp)); +} + +class FailingCostFunction : public SizedCostFunction<1, 1> { + public: + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + return false; + } +}; + +TEST(TrustRegionPreprocessor, RemoveParameterBlocksFailed) { + ProblemImpl problem; + double x = 3.0; + problem.AddResidualBlock(new FailingCostFunction, NULL, &x); + problem.SetParameterBlockConstant(&x); + Solver::Options options; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_FALSE(preprocessor.Preprocess(options, &problem, &pp)); +} + +TEST(TrustRegionPreprocessor, RemoveParameterBlocksSucceeds) { + ProblemImpl problem; + double x = 3.0; + problem.AddParameterBlock(&x, 1); + Solver::Options options; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); +} + +template +class DummyCostFunction : public SizedCostFunction { + public: + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + for (int i = 0; i < kNumResiduals; ++i) { + residuals[i] = kNumResiduals * kNumResiduals + i; + } + + if (jacobians == NULL) { + return true; + } + + if (jacobians[0] != NULL) { + MatrixRef j(jacobians[0], kNumResiduals, N1); + j.setOnes(); + j *= kNumResiduals * N1; + } + + if (N2 == 0) { + return true; + } + + if (jacobians[1] != NULL) { + MatrixRef j(jacobians[1], kNumResiduals, N2); + j.setOnes(); + j *= kNumResiduals * N2; + } + + if (N3 == 0) { + return true; + } + + if (jacobians[2] != NULL) { + MatrixRef j(jacobians[2], kNumResiduals, N3); + j.setOnes(); + j *= kNumResiduals * N3; + } + + return true; + } +}; + +class LinearSolverAndEvaluatorCreationTest : public ::testing::Test { + public: + virtual void SetUp() { + x_ = 1.0; + y_ = 1.0; + z_ = 1.0; + problem_.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x_, &y_); + problem_.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y_, &z_); + } + + void Run(const LinearSolverType linear_solver_type) { + Solver::Options options; + options.linear_solver_type = linear_solver_type; + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem_, &pp)); + EXPECT_EQ(pp.options.linear_solver_type, linear_solver_type); + EXPECT_EQ(pp.linear_solver_options.type, linear_solver_type); + EXPECT_EQ(pp.evaluator_options.linear_solver_type, linear_solver_type); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); + } + + protected: + ProblemImpl problem_; + double x_; + double y_; + double z_; +}; + +TEST_F(LinearSolverAndEvaluatorCreationTest, DenseQR) { + Run(DENSE_QR); +} + +TEST_F(LinearSolverAndEvaluatorCreationTest, DenseNormalCholesky) { + Run(DENSE_NORMAL_CHOLESKY); +} + +TEST_F(LinearSolverAndEvaluatorCreationTest, DenseSchur) { + Run(DENSE_SCHUR); +} + +#if defined(CERES_USE_EIGEN_SPARSE) || !defined(CERES_NO_SUITE_SPARSE) || !defined(CERES_NO_CX_SPARSE) +TEST_F(LinearSolverAndEvaluatorCreationTest, SparseNormalCholesky) { + Run(SPARSE_NORMAL_CHOLESKY); +} +#endif + +#if defined(CERES_USE_EIGEN_SPARSE) || !defined(CERES_NO_SUITE_SPARSE) || !defined(CERES_NO_CX_SPARSE) +TEST_F(LinearSolverAndEvaluatorCreationTest, SparseSchur) { + Run(SPARSE_SCHUR); +} +#endif + +TEST_F(LinearSolverAndEvaluatorCreationTest, CGNR) { + Run(CGNR); +} + +TEST_F(LinearSolverAndEvaluatorCreationTest, IterativeSchur) { + Run(ITERATIVE_SCHUR); +} + +TEST(TrustRegionPreprocessor, SchurTypeSolverWithBadOrdering) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering.reset(new ParameterBlockOrdering); + options.linear_solver_ordering->AddElementToGroup(&x, 0); + options.linear_solver_ordering->AddElementToGroup(&y, 0); + options.linear_solver_ordering->AddElementToGroup(&z, 1); + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_FALSE(preprocessor.Preprocess(options, &problem, &pp)); +} + +TEST(TrustRegionPreprocessor, SchurTypeSolverWithGoodOrdering) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering.reset(new ParameterBlockOrdering); + options.linear_solver_ordering->AddElementToGroup(&x, 0); + options.linear_solver_ordering->AddElementToGroup(&z, 0); + options.linear_solver_ordering->AddElementToGroup(&y, 1); + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); + EXPECT_EQ(pp.options.linear_solver_type, DENSE_SCHUR); + EXPECT_EQ(pp.linear_solver_options.type, DENSE_SCHUR); + EXPECT_EQ(pp.evaluator_options.linear_solver_type, DENSE_SCHUR); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); +} + +TEST(TrustRegionPreprocessor, SchurTypeSolverWithEmptyFirstEliminationGroup) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + problem.SetParameterBlockConstant(&x); + problem.SetParameterBlockConstant(&z); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering.reset(new ParameterBlockOrdering); + options.linear_solver_ordering->AddElementToGroup(&x, 0); + options.linear_solver_ordering->AddElementToGroup(&z, 0); + options.linear_solver_ordering->AddElementToGroup(&y, 1); + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); + EXPECT_EQ(pp.options.linear_solver_type, DENSE_QR); + EXPECT_EQ(pp.linear_solver_options.type, DENSE_QR); + EXPECT_EQ(pp.evaluator_options.linear_solver_type, DENSE_QR); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); +} + +TEST(TrustRegionPreprocessor, SchurTypeSolverWithEmptySecondEliminationGroup) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + problem.SetParameterBlockConstant(&y); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering.reset(new ParameterBlockOrdering); + options.linear_solver_ordering->AddElementToGroup(&x, 0); + options.linear_solver_ordering->AddElementToGroup(&z, 0); + options.linear_solver_ordering->AddElementToGroup(&y, 1); + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); + EXPECT_EQ(pp.options.linear_solver_type, DENSE_SCHUR); + EXPECT_EQ(pp.linear_solver_options.type, DENSE_SCHUR); + EXPECT_EQ(pp.evaluator_options.linear_solver_type, DENSE_SCHUR); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); +} + +TEST(TrustRegionProcessor, InnerIterationsWithOneParameterBlock) { + ProblemImpl problem; + double x = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1>, NULL, &x); + + Solver::Options options; + options.use_inner_iterations = true; + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); + EXPECT_TRUE(pp.inner_iteration_minimizer.get() == NULL); +} + +TEST(TrustRegionProcessor, InnerIterationsWithTwoParameterBlocks) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + + Solver::Options options; + options.use_inner_iterations = true; + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); + EXPECT_TRUE(pp.inner_iteration_minimizer.get() != NULL); +} + +TEST(TrustRegionProcessor, InvalidInnerIterationsOrdering) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + + Solver::Options options; + options.use_inner_iterations = true; + options.inner_iteration_ordering.reset(new ParameterBlockOrdering); + options.inner_iteration_ordering->AddElementToGroup(&x, 0); + options.inner_iteration_ordering->AddElementToGroup(&z, 0); + options.inner_iteration_ordering->AddElementToGroup(&y, 0); + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_FALSE(preprocessor.Preprocess(options, &problem, &pp)); +} + +TEST(TrustRegionProcessor, ValidInnerIterationsOrdering) { + ProblemImpl problem; + double x = 1.0; + double y = 1.0; + double z = 1.0; + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &x, &y); + problem.AddResidualBlock(new DummyCostFunction<1, 1, 1>, NULL, &y, &z); + + Solver::Options options; + options.use_inner_iterations = true; + options.inner_iteration_ordering.reset(new ParameterBlockOrdering); + options.inner_iteration_ordering->AddElementToGroup(&x, 0); + options.inner_iteration_ordering->AddElementToGroup(&z, 0); + options.inner_iteration_ordering->AddElementToGroup(&y, 1); + + TrustRegionPreprocessor preprocessor; + PreprocessedProblem pp; + EXPECT_TRUE(preprocessor.Preprocess(options, &problem, &pp)); + EXPECT_TRUE(pp.linear_solver.get() != NULL); + EXPECT_TRUE(pp.evaluator.get() != NULL); + EXPECT_TRUE(pp.inner_iteration_minimizer.get() != NULL); +} + +} // namespace internal +} // namespace ceres