Generalization of the inner iterations algorithm. Add automatic recursive independent set decomposition. Clean up the naming and the API for inner iterations. Change-Id: I3d7d6babb9756842d7367e14b7279d2df98fb724
diff --git a/docs/changes.tex b/docs/changes.tex index 90e8089..ef899d6 100644 --- a/docs/changes.tex +++ b/docs/changes.tex
@@ -30,10 +30,10 @@ options.linear_solver_type = ceres::DENSE_SCHUR; options.ordering = new ceres::Ordering; for (int i = 0; i < num_points; ++i) { - options.ordering.AddParameterToGroup(my_points[i], 0); + options.ordering.AddElementToGroup(my_points[i], 0); } for (int i = 0; i < num_cameras; ++i) { - options.ordering.AddParameterToGroup(my_cameras[i], 1); + options.ordering.AddElementToGroup(my_cameras[i], 1); } \end{minted} \subsection{New Features}
diff --git a/docs/solving.tex b/docs/solving.tex index 2630fa1..4fe8356 100644 --- a/docs/solving.tex +++ b/docs/solving.tex
@@ -170,13 +170,20 @@ on $a_1$ and $a_2$ (if they are two different parameter block) is that they do not co-occur in a residual block. +This idea can be further generalized, by not just optimizing $(a_1, +a_2)$, but decomposing the graph corresponding to the Hessian matrix's +sparsity structure into a collection of non-overlapping independent sets +and optimizing each of them. + Setting \texttt{Solver::Options::use\_inner\_iterations} to true -enables (and optionally setting -\texttt{Solver::Options::parameter\_blocks\_for\_inner\_iterations} +enables the use of this non-linear generalization of Ruhe \& Wedin's Algorithm II. This version of Ceres has a higher iteration complexity, but also displays better convergence behavior per iteration. +Setting \texttt{Solver::Options::num\_threads} to the maximum number +possible is highly recommended. + \subsection{Non-monotonic Steps} \label{sec:non-monotonic} Note that the basic trust-region algorithm described in @@ -543,19 +550,22 @@ on each Newton/Trust region step using a coordinate descent algorithm. For more details, see the discussion in ~\ref{sec:inner} -\item{\texttt{parameter\_blocks\_for\_inner\_iterations} } The set of - parameter blocks that should be used for coordinate descent when - doing the inner iterations. The set of parameter blocks should form - an independent set in the Hessian of the optimization problem, i.e., - no two parameter blocks in this list should co-occur in the same - residual block. +\item{\texttt{inner\_iteration\_ordering} (\texttt{NULL})} If + \texttt{Solver::Options::inner\_iterations} is true, then the user + has two choices. - If this vector is left empty and \texttt{use\_inner\_iterations} is - set to true, Ceres will use a heuristic to choose a set of parameter - blocks for you. +\begin{enumerate} +\item Let the solver heuristically decide which parameter blocks to + optimize in each inner iteration. To do this, set + \texttt{inner\_iteration\_ordering} to {\texttt{NULL}}. -\item{\texttt{ordering} (NULL)} An instance of the ordering object - informs the solver about the desired order in which parameter +\item Specify a collection of of ordered independent sets. The lower + numbered groups are optimized before the higher number groups during + the inner optimization phase. Each group must be an independent set. +\end{enumerate} + +\item{\texttt{ordering} (\texttt{NULL})} An instance of the ordering + object informs the solver about the desired order in which parameter blocks should be eliminated by the linear solvers. See section~\ref{sec:ordering} for more details.
diff --git a/examples/bundle_adjuster.cc b/examples/bundle_adjuster.cc index 16a9c1e..b24962a 100644 --- a/examples/bundle_adjuster.cc +++ b/examples/bundle_adjuster.cc
@@ -74,8 +74,8 @@ DEFINE_bool(inner_iterations, false, "Use inner iterations to non-linearly " "refine each successful trust region step."); -DEFINE_string(blocks_for_inner_iterations, "cameras", "Options are: " - "automatic, cameras, points"); +DEFINE_string(blocks_for_inner_iterations, "automatic", "Options are: " + "automatic, cameras, points, cameras,points, points,cameras"); DEFINE_string(linear_solver, "sparse_schur", "Options are: " "sparse_schur, dense_schur, iterative_schur, sparse_normal_cholesky, " @@ -145,13 +145,33 @@ if (options->use_inner_iterations) { if (FLAGS_blocks_for_inner_iterations == "cameras") { LOG(INFO) << "Camera blocks for inner iterations"; + options->inner_iteration_ordering = new ParameterBlockOrdering; for (int i = 0; i < num_cameras; ++i) { - options->parameter_blocks_for_inner_iterations.push_back(cameras + camera_block_size * i); + options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0); } } else if (FLAGS_blocks_for_inner_iterations == "points") { LOG(INFO) << "Point blocks for inner iterations"; + options->inner_iteration_ordering = new ParameterBlockOrdering; for (int i = 0; i < num_points; ++i) { - options->parameter_blocks_for_inner_iterations.push_back(points + point_block_size * i); + options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0); + } + } else if (FLAGS_blocks_for_inner_iterations == "cameras,points") { + LOG(INFO) << "Camera followed by point blocks for inner iterations"; + options->inner_iteration_ordering = new ParameterBlockOrdering; + for (int i = 0; i < num_cameras; ++i) { + options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 0); + } + for (int i = 0; i < num_points; ++i) { + options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 1); + } + } else if (FLAGS_blocks_for_inner_iterations == "points,cameras") { + LOG(INFO) << "Point followed by camera blocks for inner iterations"; + options->inner_iteration_ordering = new ParameterBlockOrdering; + for (int i = 0; i < num_cameras; ++i) { + options->inner_iteration_ordering->AddElementToGroup(cameras + camera_block_size * i, 1); + } + for (int i = 0; i < num_points; ++i) { + options->inner_iteration_ordering->AddElementToGroup(points + point_block_size * i, 0); } } else if (FLAGS_blocks_for_inner_iterations == "automatic") { LOG(INFO) << "Choosing automatic blocks for inner iterations";
diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h index d9210b3..6699ef3 100644 --- a/include/ceres/ordered_groups.h +++ b/include/ceres/ordered_groups.h
@@ -34,7 +34,6 @@ #include <map> #include <set> #include "ceres/collections_port.h" -#include "glog/logging.h" namespace ceres { @@ -80,6 +79,11 @@ return true; } + void Clear() { + group_to_elements_.clear(); + element_to_group_.clear(); + } + // Remove the element, no matter what group it is in. If the element // is not a member of any group, calling this method will result in // a crash. @@ -102,6 +106,27 @@ return true; } + // Reverse the order of the groups in place. + void Reverse() { + typename map<int, set<T> >::reverse_iterator it = + group_to_elements_.rbegin(); + map<int, set<T> > new_group_to_elements; + new_group_to_elements[it->first] = it->second; + + int new_group_id = it->first + 1; + for (++it; it != group_to_elements_.rend(); ++it) { + for (typename set<T>::const_iterator element_it = it->second.begin(); + element_it != it->second.end(); + ++element_it) { + element_to_group_[*element_it] = new_group_id; + } + new_group_to_elements[new_group_id] = it->second; + new_group_id++; + } + + group_to_elements_.swap(new_group_to_elements); + } + // Return the group id for the element. If the element is not a // member of any group, return -1. int GroupId(const T element) const { @@ -134,7 +159,7 @@ return group_to_elements_.size(); } - const map<int, set<T> > group_to_elements() const { + const map<int, set<T> >& group_to_elements() const { return group_to_elements_; }
diff --git a/include/ceres/solver.h b/include/ceres/solver.h index e7b6b32..eb882ab 100644 --- a/include/ceres/solver.h +++ b/include/ceres/solver.h
@@ -98,6 +98,7 @@ #endif ordering = NULL; use_inner_iterations = false; + inner_iteration_ordering = NULL; linear_solver_min_num_iterations = 1; linear_solver_max_num_iterations = 500; eta = 1e-1; @@ -296,8 +297,25 @@ // the parameter blocks into two groups, one for the points and one // for the cameras, where the group containing the points has an id // smaller than the group containing cameras. + // + // Once assigned, Solver::Options owns this pointer and will + // deallocate the memory when destroyed. ParameterBlockOrdering* ordering; + // By virtue of the modeling layer in Ceres being block oriented, + // all the matrices used by Ceres are also block oriented. When + // doing sparse direct factorization of these matrices (for + // SPARSE_NORMAL_CHOLESKY, SPARSE_SCHUR and ITERATIVE in + // conjunction with CLUSTER_TRIDIAGONAL AND CLUSTER_JACOBI + // preconditioners), the fill-reducing ordering algorithms can + // either be run on the block or the scalar form of these matrices. + // Running it on the block form exposes more of the super-nodal + // structure of the matrix to the factorization routines. Setting + // this parameter to true runs the ordering algorithms in block + // form. Currently this option only makes sense with + // sparse_linear_algebra_library = SUITE_SPARSE. + bool use_block_amd; + // Some non-linear least squares problems have additional // structure in the way the parameter blocks interact that it is // beneficial to modify the way the trust region step is computed. @@ -345,38 +363,29 @@ // optimization problems will do. The only constraint on a_1 and // a_2 is that they do not co-occur in any residual block. // + // This idea can be further generalized, by not just optimizing + // (a_1, a_2), but decomposing the graph corresponding to the + // Hessian matrix's sparsity structure in a collection of + // non-overlapping independent sets and optimizing each of them. + // // Setting "use_inner_iterations" to true enables the use of this // non-linear generalization of Ruhe & Wedin's Algorithm II. This // version of Ceres has a higher iteration complexity, but also - // displays better convergence behaviour per iteration. + // displays better convergence behaviour per iteration. Setting + // Solver::Options::num_threads to the maximum number possible is + // highly recommended. bool use_inner_iterations; // If inner_iterations is true, then the user has two choices. // - // 1. Provide a list of parameter blocks, which should be subject - // to inner iterations. The only requirement on the set of - // parameter blocks is that they form an independent set in the - // Hessian matrix, much like the first elimination group in - // Solver::Options::ordering. + // 1. Let the solver heuristically decide which parameter blocks + // to optimize in each inner iteration. To do this leave + // Solver::Options::inner_iteration_ordering untouched. // - // 2. The second is to leave it empty, in which case, Ceres will - // use a heuristic to automatically choose a set of parameter - // blocks. - vector<double*> parameter_blocks_for_inner_iterations; - - // By virtue of the modeling layer in Ceres being block oriented, - // all the matrices used by Ceres are also block oriented. When - // doing sparse direct factorization of these matrices (for - // SPARSE_NORMAL_CHOLESKY, SPARSE_SCHUR and ITERATIVE in - // conjunction with CLUSTER_TRIDIAGONAL AND CLUSTER_JACOBI - // preconditioners), the fill-reducing ordering algorithms can - // either be run on the block or the scalar form of these matrices. - // Running it on the block form exposes more of the super-nodal - // structure of the matrix to the factorization routines. Setting - // this parameter to true runs the ordering algorithms in block - // form. Currently this option only makes sense with - // sparse_linear_algebra_library = SUITE_SPARSE. - bool use_block_amd; + // 2. Specify a collection of of ordered independent sets. Where + // the lower numbered groups are optimized before the higher + // number groups. Each group must be an independent set. + ParameterBlockOrdering* inner_iteration_ordering; // Minimum number of iterations for which the linear solver should // run, even if the convergence criterion is satisfied.
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 3d7d496..cc2d2ea 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -55,7 +55,7 @@ file.cc gradient_checking_cost_function.cc implicit_schur_complement.cc - inner_iteration_minimizer.cc + coordinate_descent_minimizer.cc iterative_schur_complement_solver.cc levenberg_marquardt_strategy.cc linear_least_squares_problems.cc @@ -64,6 +64,7 @@ local_parameterization.cc loss_function.cc normal_prior.cc + parameter_block_ordering.cc partitioned_matrix_view.cc polynomial_solver.cc problem.cc @@ -74,7 +75,6 @@ runtime_numeric_diff_cost_function.cc schur_complement_solver.cc schur_eliminator.cc - schur_ordering.cc scratch_evaluate_preparer.cc solver.cc solver_impl.cc @@ -235,6 +235,7 @@ CERES_TEST(numeric_diff_cost_function) CERES_TEST(ordered_groups) CERES_TEST(parameter_block) + CERES_TEST(parameter_block_ordering) CERES_TEST(partitioned_matrix_view) CERES_TEST(polynomial_solver) CERES_TEST(problem) @@ -244,7 +245,6 @@ CERES_TEST(runtime_numeric_diff_cost_function) CERES_TEST(schur_complement_solver) CERES_TEST(schur_eliminator) - CERES_TEST(schur_ordering) CERES_TEST(solver_impl) IF (${SUITESPARSE_FOUND})
diff --git a/internal/ceres/coordinate_descent_minimizer.cc b/internal/ceres/coordinate_descent_minimizer.cc new file mode 100644 index 0000000..370b004 --- /dev/null +++ b/internal/ceres/coordinate_descent_minimizer.cc
@@ -0,0 +1,215 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 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/coordinate_descent_minimizer.h" + +#include <numeric> +#include <vector> +#include "ceres/evaluator.h" +#include "ceres/linear_solver.h" +#include "ceres/minimizer.h" +#include "ceres/ordered_groups.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/solver.h" +#include "ceres/solver_impl.h" +#include "ceres/trust_region_minimizer.h" +#include "ceres/trust_region_strategy.h" + +namespace ceres { +namespace internal { + +CoordinateDescentMinimizer::~CoordinateDescentMinimizer() { +} + +bool CoordinateDescentMinimizer::Init( + const Program& program, + const ProblemImpl::ParameterMap& parameter_map, + const ParameterBlockOrdering& ordering, + string* error) { + parameter_blocks_.clear(); + independent_set_offsets_.clear(); + independent_set_offsets_.push_back(0); + + // Serialize the OrderedGroups into a vector of parameter block + // offsets for parallel access. + map<ParameterBlock*, int> parameter_block_index; + map<int, set<double*> > group_to_elements = ordering.group_to_elements(); + for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + it != group_to_elements.end(); + ++it) { + for (set<double*>::const_iterator ptr_it = it->second.begin(); + ptr_it != it->second.end(); + ++ptr_it) { + parameter_blocks_.push_back(parameter_map.find(*ptr_it)->second); + parameter_block_index[parameter_blocks_.back()] = + parameter_blocks_.size() - 1; + } + independent_set_offsets_.push_back( + independent_set_offsets_.back() + it->second.size()); + } + + // The ordering does not have to contain all parameter blocks, so + // assign zero offsets/empty independent sets to these parameter + // blocks. + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + if (!ordering.IsMember(parameter_blocks[i]->mutable_user_state())) { + parameter_blocks_.push_back(parameter_blocks[i]); + independent_set_offsets_.push_back(independent_set_offsets_.back()); + } + } + + // Compute the set of residual blocks that depend on each parameter + // block. + residual_blocks_.resize(parameter_block_index.size()); + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + const map<ParameterBlock*, int>::const_iterator it = + parameter_block_index.find(parameter_block); + if (it != parameter_block_index.end()) { + residual_blocks_[it->second].push_back(residual_block); + } + } + } + + LinearSolver::Options linear_solver_options; + linear_solver_options.type = DENSE_QR; + linear_solver_.reset(LinearSolver::Create(linear_solver_options)); + CHECK_NOTNULL(linear_solver_.get()); + + evaluator_options_.linear_solver_type = DENSE_QR; + evaluator_options_.num_eliminate_blocks = 0; + evaluator_options_.num_threads = 1; + + return true; +} + +void CoordinateDescentMinimizer::Minimize( + const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + // Set the state and mark all parameter blocks constant. + for (int i = 0; i < parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks_[i]; + parameter_block->SetState(parameters + parameter_block->state_offset()); + parameter_block->SetConstant(); + } + + for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) { + // No point paying the price for an OpemMP call if the set if of + // size zero. + if (independent_set_offsets_[i] == independent_set_offsets_[i + 1]) { + continue; + } + + // The parameter blocks in each independent set can be optimized + // in parallel, since they do not co-occur in any residual block. +#pragma omp parallel for num_threads(options.num_threads) + for (int j = independent_set_offsets_[i]; + j < independent_set_offsets_[i + 1]; + ++j) { + + ParameterBlock* parameter_block = parameter_blocks_[j]; + const int old_index = parameter_block->index(); + const int old_delta_offset = parameter_block->delta_offset(); + parameter_block->SetVarying(); + parameter_block->set_index(0); + parameter_block->set_delta_offset(0); + + Program inner_program; + inner_program.mutable_parameter_blocks()->push_back(parameter_block); + *inner_program.mutable_residual_blocks() = residual_blocks_[j]; + + // TODO(sameeragarwal): Better error handling. Right now we + // assume that this is not going to lead to problems of any + // sort. Basically we should be checking for numerical failure + // of some sort. + // + // On the other hand, if the optimization is a failure, that in + // some ways is fine, since it won't change the parameters and + // we are fine. + Solver::Summary inner_summary; + Solve(&inner_program, + parameters + parameter_block->state_offset(), + &inner_summary); + + parameter_block->set_index(old_index); + parameter_block->set_delta_offset(old_delta_offset); + parameter_block->SetState(parameters + parameter_block->state_offset()); + parameter_block->SetConstant(); + } + } + + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->SetVarying(); + } +} + +// Solve the optimization problem for one parameter block. +void CoordinateDescentMinimizer::Solve(Program* program, + double* parameter, + Solver::Summary* summary) { + *summary = Solver::Summary(); + summary->initial_cost = 0.0; + summary->fixed_cost = 0.0; + summary->final_cost = 0.0; + string error; + + scoped_ptr<Evaluator> evaluator( + Evaluator::Create(evaluator_options_, program, &error)); + CHECK_NOTNULL(evaluator.get()); + + scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); + CHECK_NOTNULL(jacobian.get()); + + TrustRegionStrategy::Options trust_region_strategy_options; + trust_region_strategy_options.linear_solver = linear_solver_.get(); + scoped_ptr<TrustRegionStrategy>trust_region_strategy( + TrustRegionStrategy::Create(trust_region_strategy_options)); + CHECK_NOTNULL(trust_region_strategy.get()); + + Minimizer::Options minimizer_options; + minimizer_options.evaluator = evaluator.get(); + minimizer_options.jacobian = jacobian.get(); + minimizer_options.trust_region_strategy = trust_region_strategy.get(); + + TrustRegionMinimizer minimizer; + minimizer.Minimize(minimizer_options, parameter, summary); +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/inner_iteration_minimizer.h b/internal/ceres/coordinate_descent_minimizer.h similarity index 63% rename from internal/ceres/inner_iteration_minimizer.h rename to internal/ceres/coordinate_descent_minimizer.h index b7c7461..d7ff4db 100644 --- a/internal/ceres/inner_iteration_minimizer.h +++ b/internal/ceres/coordinate_descent_minimizer.h
@@ -28,60 +28,59 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include <vector> -#include "ceres/internal/scoped_ptr.h" +#ifndef CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ +#define CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_ + +#include "ceres/evaluator.h" #include "ceres/minimizer.h" #include "ceres/problem_impl.h" +#include "ceres/program.h" #include "ceres/solver.h" -#include "ceres/trust_region_strategy.h" -#include "ceres/evaluator.h" -#include "ceres/linear_solver.h" namespace ceres { namespace internal { -class Program; - -// The InnerIterationMinimizer performs coordinate descent on a user -// specified set of parameter blocks. The user can either specify the -// set of parameter blocks for coordinate descent or have the -// minimizer choose on its own. +// Given a Program, and a ParameterBlockOrdering which partitions +// (non-exhaustively) the Hessian matrix into independent sets, +// perform coordinate descent on the parameter blocks in the +// ordering. The independent set structure allows for all parameter +// blocks in the same independent set to be optimized in parallel, and +// the order of the independent set determines the order in which the +// parameter block groups are optimized. // -// This Minimizer when used in combination with the -// TrustRegionMinimizer is used to implement a non-linear -// generalization of Ruhe & Wedin's Algorithm II for separable -// non-linear least squares problems. -class InnerIterationMinimizer : public Minimizer { +// The minimizer assumes that none of the parameter blocks in the +// program are constant. +class CoordinateDescentMinimizer : public Minimizer { public: - // Initialize the minimizer. The return value indicates success or - // failure, and the error string contains a description of the - // error. - // - // The parameter blocks for inner iterations must form an - // independent set in the Hessian for the optimization problem. - // - // If this vector is empty, the minimizer will attempt to find a set - // of parameter blocks to optimize. bool Init(const Program& program, const ProblemImpl::ParameterMap& parameter_map, - const vector<double*>& parameter_blocks_for_inner_iterations, + const ParameterBlockOrdering& ordering, string* error); // Minimizer interface. - virtual ~InnerIterationMinimizer(); + virtual ~CoordinateDescentMinimizer(); virtual void Minimize(const Minimizer::Options& options, double* parameters, Solver::Summary* summary); private: - void MinimalSolve(Program* program, double* parameters, Solver::Summary* summary); - void ComputeResidualBlockOffsets(const int num_eliminate_blocks); + void Solve(Program* program, + double* parameters, + Solver::Summary* summary); - scoped_ptr<Program> program_; - vector<int> residual_block_offsets_; + vector<ParameterBlock*> parameter_blocks_; + vector<vector<ResidualBlock*> > residual_blocks_; + // The optimization is performed in rounds. In each round all the + // parameter blocks that form one independent set are optimized in + // parallel. This array, marks the boundaries of the independent + // sets in parameter_blocks_. + vector<int> independent_set_offsets_; + Evaluator::Options evaluator_options_; scoped_ptr<LinearSolver> linear_solver_; }; } // namespace internal } // namespace ceres + +#endif // CERES_INTERNAL_COORDINATE_DESCENT_MINIMIZER_H_
diff --git a/internal/ceres/graph.h b/internal/ceres/graph.h index 2c0f6d2..e080489 100644 --- a/internal/ceres/graph.h +++ b/internal/ceres/graph.h
@@ -65,6 +65,28 @@ AddVertex(vertex, 1.0); } + bool RemoveVertex(const Vertex& vertex) { + if (vertices_.find(vertex) == vertices_.end()) { + return false; + } + + vertices_.erase(vertex); + vertex_weights_.erase(vertex); + const HashSet<Vertex>& sinks = edges_[vertex]; + for (typename HashSet<Vertex>::const_iterator it = sinks.begin(); + it != sinks.end(); ++it) { + if (vertex < *it) { + edge_weights_.erase(make_pair(vertex, *it)); + } else { + edge_weights_.erase(make_pair(*it, vertex)); + } + edges_[*it].erase(vertex); + } + + edges_.erase(vertex); + return true; + } + // Add a weighted edge between the vertex1 and vertex2. Calling // AddEdge on a pair of vertices which do not exist in the graph yet // will result in undefined behavior.
diff --git a/internal/ceres/inner_iteration_minimizer.cc b/internal/ceres/inner_iteration_minimizer.cc deleted file mode 100644 index 36748ef..0000000 --- a/internal/ceres/inner_iteration_minimizer.cc +++ /dev/null
@@ -1,256 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2012 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/inner_iteration_minimizer.h" - -#include <numeric> -#include <vector> -#include "ceres/evaluator.h" -#include "ceres/linear_solver.h" -#include "ceres/minimizer.h" -#include "ceres/ordered_groups.h" -#include "ceres/parameter_block.h" -#include "ceres/problem_impl.h" -#include "ceres/program.h" -#include "ceres/residual_block.h" -#include "ceres/schur_ordering.h" -#include "ceres/solver.h" -#include "ceres/solver_impl.h" -#include "ceres/trust_region_minimizer.h" -#include "ceres/trust_region_strategy.h" - -namespace ceres { -namespace internal { - -InnerIterationMinimizer::~InnerIterationMinimizer() { -} - -bool InnerIterationMinimizer::Init(const Program& outer_program, - const ProblemImpl::ParameterMap& parameter_map, - const vector<double*>& parameter_blocks_for_inner_iterations, - string* error) { - program_.reset(new Program(outer_program)); - - ParameterBlockOrdering ordering; - int num_inner_iteration_parameter_blocks = 0; - - if (parameter_blocks_for_inner_iterations.size() == 0) { - // The user wishes for the solver to determine a set of parameter - // blocks to descend on. - // - // For now use approximate maximum independent set computed by - // ComputeSchurOrdering code. Though going forward, we want use - // the smallest maximal independent set, rather than the largest. - // - // TODO(sameeragarwal): Smallest maximal independent set instead - // of the approximate maximum independent set. - vector<ParameterBlock*> parameter_block_ordering; - num_inner_iteration_parameter_blocks = - ComputeSchurOrdering(*program_, ¶meter_block_ordering); - // Decompose the Schur ordering into elimination group 0 and 1, 0 - // is the one used for inner iterations. - for (int i = 0; i < parameter_block_ordering.size(); ++i) { - double* ptr = parameter_block_ordering[i]->mutable_user_state(); - if (i < num_inner_iteration_parameter_blocks) { - ordering.AddElementToGroup(ptr, 0); - } else { - ordering.AddElementToGroup(ptr, 1); - } - } - } else { - const vector<ParameterBlock*> parameter_blocks = program_->parameter_blocks(); - set<double*> parameter_block_ptrs(parameter_blocks_for_inner_iterations.begin(), - parameter_blocks_for_inner_iterations.end()); - num_inner_iteration_parameter_blocks = 0; - // Divide the set of parameter blocks into two groups. Group 0 is - // the set of parameter blocks specified by the user, and the rest - // in group 1. - for (int i = 0; i < parameter_blocks.size(); ++i) { - double* ptr = parameter_blocks[i]->mutable_user_state(); - if (parameter_block_ptrs.count(ptr) != 0) { - ordering.AddElementToGroup(ptr, 0); - } else { - ordering.AddElementToGroup(ptr, 1); - } - } - - num_inner_iteration_parameter_blocks = ordering.GroupSize(0); - if (num_inner_iteration_parameter_blocks > 0) { - const map<int, set<double*> >& group_to_elements = - ordering.group_to_elements(); - if (!SolverImpl::IsParameterBlockSetIndependent( - group_to_elements.begin()->second, - program_->residual_blocks())) { - *error = "The user provided parameter_blocks_for_inner_iterations " - "does not form an independent set"; - return false; - } - } - } - - if (!SolverImpl::ApplyUserOrdering(parameter_map, - &ordering, - program_.get(), - error)) { - return false; - } - - program_->SetParameterOffsetsAndIndex(); - - if (!SolverImpl::LexicographicallyOrderResidualBlocks( - num_inner_iteration_parameter_blocks, - program_.get(), - error)) { - return false; - } - - ComputeResidualBlockOffsets(num_inner_iteration_parameter_blocks); - - const_cast<Program*>(&outer_program)->SetParameterOffsetsAndIndex(); - - LinearSolver::Options linear_solver_options; - linear_solver_options.type = DENSE_QR; - linear_solver_.reset(LinearSolver::Create(linear_solver_options)); - CHECK_NOTNULL(linear_solver_.get()); - - evaluator_options_.linear_solver_type = DENSE_QR; - evaluator_options_.num_eliminate_blocks = 0; - evaluator_options_.num_threads = 1; - - return true; -} - -void InnerIterationMinimizer::Minimize( - const Minimizer::Options& options, - double* parameters, - Solver::Summary* summary) { - const vector<ParameterBlock*>& parameter_blocks = program_->parameter_blocks(); - const vector<ResidualBlock*>& residual_blocks = program_->residual_blocks(); - - const int num_inner_iteration_parameter_blocks = residual_block_offsets_.size() - 1; - - for (int i = 0; i < parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = parameter_blocks[i]; - parameter_block->SetState(parameters + parameter_block->state_offset()); - if (i >= num_inner_iteration_parameter_blocks) { - parameter_block->SetConstant(); - } - } - -#pragma omp parallel for num_threads(options.num_threads) - for (int i = 0; i < num_inner_iteration_parameter_blocks; ++i) { - Solver::Summary inner_summary; - ParameterBlock* parameter_block = parameter_blocks[i]; - const int old_index = parameter_block->index(); - const int old_delta_offset = parameter_block->delta_offset(); - - parameter_block->set_index(0); - parameter_block->set_delta_offset(0); - - Program inner_program; - inner_program.mutable_parameter_blocks()->push_back(parameter_block); - - // This works, because we have already ordered the residual blocks - // so that the residual blocks for each parameter block being - // optimized over are contiguously located in the residual_blocks - // vector. - copy(residual_blocks.begin() + residual_block_offsets_[i], - residual_blocks.begin() + residual_block_offsets_[i + 1], - back_inserter(*inner_program.mutable_residual_blocks())); - - MinimalSolve(&inner_program, - parameters + parameter_block->state_offset(), - &inner_summary); - - parameter_block->set_index(old_index); - parameter_block->set_delta_offset(old_delta_offset); - } - - for (int i = num_inner_iteration_parameter_blocks; i < parameter_blocks.size(); ++i) { - parameter_blocks[i]->SetVarying(); - } -} - -void InnerIterationMinimizer::MinimalSolve(Program* program, - double* parameters, - Solver::Summary* summary) { - - *summary = Solver::Summary(); - summary->initial_cost = 0.0; - summary->fixed_cost = 0.0; - summary->final_cost = 0.0; - string error; - - scoped_ptr<Evaluator> evaluator(Evaluator::Create(evaluator_options_, program, &error)); - CHECK_NOTNULL(evaluator.get()); - - scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); - CHECK_NOTNULL(jacobian.get()); - - TrustRegionStrategy::Options trust_region_strategy_options; - trust_region_strategy_options.linear_solver = linear_solver_.get(); - scoped_ptr<TrustRegionStrategy>trust_region_strategy( - TrustRegionStrategy::Create(trust_region_strategy_options)); - CHECK_NOTNULL(trust_region_strategy.get()); - - Minimizer::Options minimizer_options; - minimizer_options.evaluator = evaluator.get(); - minimizer_options.jacobian = jacobian.get(); - minimizer_options.trust_region_strategy = trust_region_strategy.get(); - - TrustRegionMinimizer minimizer; - minimizer.Minimize(minimizer_options, parameters, summary); -} - - -void InnerIterationMinimizer::ComputeResidualBlockOffsets( - const int num_eliminate_blocks) { - vector<int> counts(num_eliminate_blocks, 0); - const vector<ResidualBlock*>& residual_blocks = program_->residual_blocks(); - for (int i = 0; i < residual_blocks.size(); ++i) { - ResidualBlock* residual_block = residual_blocks[i]; - const int num_parameter_blocks = residual_block->NumParameterBlocks(); - for (int j = 0; j < num_parameter_blocks; ++j) { - ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; - if (!parameter_block->IsConstant() && - parameter_block->index() < num_eliminate_blocks) { - counts[parameter_block->index()] += 1; - } - } - } - - residual_block_offsets_.resize(num_eliminate_blocks + 1); - residual_block_offsets_[0] = 0; - partial_sum(counts.begin(), counts.end(), residual_block_offsets_.begin() + 1); -} - - -} // namespace internal -} // namespace ceres
diff --git a/internal/ceres/ordered_groups_test.cc b/internal/ceres/ordered_groups_test.cc index 214d032..700e788 100644 --- a/internal/ceres/ordered_groups_test.cc +++ b/internal/ceres/ordered_groups_test.cc
@@ -133,5 +133,31 @@ EXPECT_EQ(ordering.GroupId(x + 2), 5); } +TEST(OrderedGroup, ReverseOrdering) { + ParameterBlockOrdering ordering; + double x[3]; + ordering.AddElementToGroup(x, 1); + ordering.AddElementToGroup(x + 1, 2); + ordering.AddElementToGroup(x + 2, 2); + + EXPECT_EQ(ordering.NumGroups(), 2); + EXPECT_EQ(ordering.NumElements(), 3); + EXPECT_EQ(ordering.GroupSize(1), 1); + EXPECT_EQ(ordering.GroupSize(2), 2); + EXPECT_EQ(ordering.GroupId(x), 1); + EXPECT_EQ(ordering.GroupId(x + 1), 2); + EXPECT_EQ(ordering.GroupId(x + 2), 2); + + ordering.Reverse(); + + EXPECT_EQ(ordering.NumGroups(), 2); + EXPECT_EQ(ordering.NumElements(), 3); + EXPECT_EQ(ordering.GroupSize(3), 1); + EXPECT_EQ(ordering.GroupSize(2), 2); + EXPECT_EQ(ordering.GroupId(x), 3); + EXPECT_EQ(ordering.GroupId(x + 1), 2); + EXPECT_EQ(ordering.GroupId(x + 2), 2); +} + } // namespace internal } // namespace ceres
diff --git a/internal/ceres/schur_ordering.cc b/internal/ceres/parameter_block_ordering.cc similarity index 77% rename from internal/ceres/schur_ordering.cc rename to internal/ceres/parameter_block_ordering.cc index 1cdff4e..e8f626f 100644 --- a/internal/ceres/schur_ordering.cc +++ b/internal/ceres/parameter_block_ordering.cc
@@ -28,7 +28,7 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include "ceres/schur_ordering.h" +#include "ceres/parameter_block_ordering.h" #include "ceres/graph.h" #include "ceres/graph_algorithms.h" @@ -46,8 +46,7 @@ vector<ParameterBlock*>* ordering) { CHECK_NOTNULL(ordering)->clear(); - scoped_ptr<Graph< ParameterBlock*> > graph( - CHECK_NOTNULL(CreateHessianGraph(program))); + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); int independent_set_size = IndependentSetOrdering(*graph, ordering); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); @@ -62,9 +61,31 @@ return independent_set_size; } +void ComputeRecursiveIndependentSetOrdering(const Program& program, + ParameterBlockOrdering* ordering) { + CHECK_NOTNULL(ordering)->Clear(); + const vector<ParameterBlock*> parameter_blocks = program.parameter_blocks(); + scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program)); + + int num_covered = 0; + int round = 0; + while (num_covered < parameter_blocks.size()) { + vector<ParameterBlock*> independent_set_ordering; + const int independent_set_size = + IndependentSetOrdering(*graph, &independent_set_ordering); + for (int i = 0; i < independent_set_size; ++i) { + ParameterBlock* parameter_block = independent_set_ordering[i]; + ordering->AddElementToGroup(parameter_block->mutable_user_state(), round); + graph->RemoveVertex(parameter_block); + } + num_covered += independent_set_size; + ++round; + } +} + Graph<ParameterBlock*>* CreateHessianGraph(const Program& program) { - Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>; + Graph<ParameterBlock*>* graph = CHECK_NOTNULL(new Graph<ParameterBlock*>); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); for (int i = 0; i < parameter_blocks.size(); ++i) { ParameterBlock* parameter_block = parameter_blocks[i];
diff --git a/internal/ceres/schur_ordering.h b/internal/ceres/parameter_block_ordering.h similarity index 84% rename from internal/ceres/schur_ordering.h rename to internal/ceres/parameter_block_ordering.h index 1f9a4ff..a5277a4 100644 --- a/internal/ceres/schur_ordering.h +++ b/internal/ceres/parameter_block_ordering.h
@@ -27,14 +27,12 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: sameeragarwal@google.com (Sameer Agarwal) -// -// Compute a parameter block ordering for use with the Schur -// complement based algorithms. -#ifndef CERES_INTERNAL_SCHUR_ORDERING_H_ -#define CERES_INTERNAL_SCHUR_ORDERING_H_ +#ifndef CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ +#define CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_ #include <vector> +#include "ceres/ordered_groups.h" #include "ceres/graph.h" #include "ceres/types.h" @@ -60,6 +58,12 @@ int ComputeSchurOrdering(const Program& program, vector<ParameterBlock* >* ordering); +// Use an approximate independent set ordering to decompose the +// parameter blocks of a problem in a sequence of independent +// sets. The ordering covers all the non-constant parameter blocks in +// the program. +void ComputeRecursiveIndependentSetOrdering(const Program& program, + ParameterBlockOrdering* ordering); // Builds a graph on the parameter blocks of a Problem, whose // structure reflects the sparsity structure of the Hessian. Each @@ -71,4 +75,4 @@ } // namespace internal } // namespace ceres -#endif // CERES_INTERNAL_SCHUR_ORDERING_H_ +#endif // CERES_INTERNAL_PARAMETER_BLOCK_ORDERING_H_
diff --git a/internal/ceres/schur_ordering_test.cc b/internal/ceres/parameter_block_ordering_test.cc similarity index 99% rename from internal/ceres/schur_ordering_test.cc rename to internal/ceres/parameter_block_ordering_test.cc index bd74ebb..bc497d0 100644 --- a/internal/ceres/schur_ordering_test.cc +++ b/internal/ceres/parameter_block_ordering_test.cc
@@ -28,7 +28,7 @@ // // Author: sameeragarwal@google.com (Sameer Agarwal) -#include "ceres/schur_ordering.h" +#include "ceres/parameter_block_ordering.h" #include <cstddef> #include <vector>
diff --git a/internal/ceres/solver.cc b/internal/ceres/solver.cc index 5a84cfc..a6c804b 100644 --- a/internal/ceres/solver.cc +++ b/internal/ceres/solver.cc
@@ -41,6 +41,11 @@ namespace ceres { +Solver::Options::~Options() { + delete ordering; + delete inner_iteration_ordering; +} + Solver::~Solver() {} void Solver::Solve(const Solver::Options& options,
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index c052aeb..062577e 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc
@@ -33,33 +33,26 @@ #include <cstdio> #include <iostream> // NOLINT #include <numeric> +#include "ceres/coordinate_descent_minimizer.h" #include "ceres/evaluator.h" #include "ceres/gradient_checking_cost_function.h" #include "ceres/iteration_callback.h" -#include "ceres/inner_iteration_minimizer.h" #include "ceres/levenberg_marquardt_strategy.h" #include "ceres/linear_solver.h" #include "ceres/map_util.h" #include "ceres/minimizer.h" #include "ceres/ordered_groups.h" #include "ceres/parameter_block.h" +#include "ceres/parameter_block_ordering.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" #include "ceres/program.h" #include "ceres/residual_block.h" -#include "ceres/schur_ordering.h" #include "ceres/stringprintf.h" #include "ceres/trust_region_minimizer.h" #include "ceres/wall_time.h" namespace ceres { - -Solver::Options::~Options() { - if (ordering != NULL) { - delete ordering; - } -} - namespace internal { namespace { @@ -151,7 +144,7 @@ void SolverImpl::Minimize(const Solver::Options& options, Program* program, - InnerIterationMinimizer* inner_iteration_minimizer, + CoordinateDescentMinimizer* inner_iteration_minimizer, Evaluator* evaluator, LinearSolver* linear_solver, double* parameters, @@ -212,6 +205,10 @@ Solver::Summary* summary) { double solver_start_time = WallTimeInSeconds(); Solver::Options options(original_options); + + options.ordering = NULL; + options.inner_iteration_ordering = NULL; + Program* original_program = original_problem_impl->mutable_program(); ProblemImpl* problem_impl = original_problem_impl; // Reset the summary object to its default values. @@ -226,14 +223,14 @@ if (options.num_threads > 1) { LOG(WARNING) << "OpenMP support is not compiled into this binary; " - << "only options.num_threads=1 is supported. Switching" + << "only options.num_threads=1 is supported. Switching " << "to single threaded mode."; options.num_threads = 1; } if (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" + << "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; } @@ -350,14 +347,51 @@ return; } - scoped_ptr<InnerIterationMinimizer> inner_iteration_minimizer; + scoped_ptr<CoordinateDescentMinimizer> inner_iteration_minimizer; + // TODO(sameeragarwal): Detect when the problem just contains one + // parameter block and the inner iterations are redundant. if (options.use_inner_iterations) { - inner_iteration_minimizer.reset(new InnerIterationMinimizer); - if (!inner_iteration_minimizer->Init( - *reduced_program, - problem_impl->parameter_map(), - options.parameter_blocks_for_inner_iterations, - &summary->error)) { + inner_iteration_minimizer.reset(new CoordinateDescentMinimizer); + + scoped_ptr<ParameterBlockOrdering> inner_iteration_ordering; + ParameterBlockOrdering* ordering_ptr = NULL; + if (original_options.inner_iteration_ordering == NULL) { + // Find a recursive decomposition of the Hessian matrix as a set + // of independent sets of decreasing size and invert it. This + // seems to work better in practice, i.e., Cameras before + // points. + inner_iteration_ordering.reset(new ParameterBlockOrdering); + ComputeRecursiveIndependentSetOrdering(*reduced_program, + inner_iteration_ordering.get()); + inner_iteration_ordering->Reverse(); + ordering_ptr = inner_iteration_ordering.get(); + } else { + const map<int, set<double*> >& group_to_elements = + original_options.inner_iteration_ordering->group_to_elements(); + + // Iterate over each group and verify that it is an independent + // set. + map<int, set<double*> >::const_iterator it = group_to_elements.begin(); + for ( ;it != group_to_elements.end(); ++it) { + if (!IsParameterBlockSetIndependent( + it->second, + reduced_program->residual_blocks())) { + summary->error = + StringPrintf("The user-provided " + "parameter_blocks_for_inner_iterations does not " + "form an independent set. Group Id: %d", it->first); + LOG(ERROR) << summary->error; + return; + } + } + ordering_ptr = original_options.inner_iteration_ordering; + } + + if (!inner_iteration_minimizer->Init(*reduced_program, + problem_impl->parameter_map(), + *ordering_ptr, + &summary->error)) { + LOG(ERROR) << summary->error; return; } } @@ -424,7 +458,6 @@ const Program& program = problem_impl->program(); const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); - const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); for (vector<ParameterBlock*>::const_iterator it = parameter_blocks.begin(); it != parameter_blocks.end(); ++it) { @@ -437,6 +470,7 @@ if (IsSchurType(options.linear_solver_type) && options.ordering->NumGroups() > 1) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); const set<double*>& e_blocks = options.ordering->group_to_elements().begin()->second; if (!IsParameterBlockSetIndependent(e_blocks, residual_blocks)) { @@ -449,8 +483,8 @@ return true; } -bool SolverImpl::IsParameterBlockSetIndependent(const set<double*> parameter_block_ptrs, - const vector<ResidualBlock*> residual_blocks) { +bool SolverImpl::IsParameterBlockSetIndependent(const set<double*>& parameter_block_ptrs, + const vector<ResidualBlock*>& residual_blocks) { // Loop over each residual block and ensure that no two parameter // blocks in the same residual block are part of // parameter_block_ptrs as that would violate the assumption that it @@ -462,9 +496,8 @@ const int num_parameter_blocks = (*it)->NumParameterBlocks(); int count = 0; for (int i = 0; i < num_parameter_blocks; ++i) { - count += - parameter_block_ptrs.count(const_cast<double*>( - parameter_blocks[i]->user_state())); + count += parameter_block_ptrs.count( + parameter_blocks[i]->mutable_user_state()); } if (count > 1) { return false; @@ -592,8 +625,7 @@ // fit. For Schur type solvers, this means that the user wishes for // Ceres to identify the e_blocks, which we do by computing a // maximal independent set. - if (original_num_groups == 1 && - IsSchurType(options->linear_solver_type)) { + if (original_num_groups == 1 && IsSchurType(options->linear_solver_type)) { vector<ParameterBlock*> schur_ordering; const int num_eliminate_blocks = ComputeSchurOrdering(*original_program, &schur_ordering);
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h index 140de95..dd40e41 100644 --- a/internal/ceres/solver_impl.h +++ b/internal/ceres/solver_impl.h
@@ -35,15 +35,15 @@ #include <vector> #include "ceres/internal/port.h" #include "ceres/ordered_groups.h" -#include "ceres/solver.h" #include "ceres/problem_impl.h" +#include "ceres/solver.h" namespace ceres { namespace internal { +class CoordinateDescentMinimizer; class Evaluator; class LinearSolver; -class InnerIterationMinimizer; class Program; class SolverImpl { @@ -86,7 +86,7 @@ // Reorder the residuals for program, if necessary, so that the // residuals involving e block (i.e., the first num_eliminate_block // parameter blocks) occur together. This is a necessary condition - // for the Schur eliminator as well as the InnerIterationMinimizer. + // for the Schur eliminator. static bool LexicographicallyOrderResidualBlocks( const int num_eliminate_blocks, Program* program, @@ -102,7 +102,7 @@ // Run the minimization for the given evaluator and configuration. static void Minimize(const Solver::Options &options, Program* program, - InnerIterationMinimizer* inner_iteration_minimizer, + CoordinateDescentMinimizer* inner_iteration_minimizer, Evaluator* evaluator, LinearSolver* linear_solver, double* parameters, @@ -124,8 +124,8 @@ string* error); static bool IsParameterBlockSetIndependent( - const set<double*> parameter_block_ptrs, - const vector<ResidualBlock*> residual_blocks); + const set<double*>& parameter_block_ptrs, + const vector<ResidualBlock*>& residual_blocks); }; } // namespace internal
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc index b838b19..db2641d 100644 --- a/internal/ceres/trust_region_minimizer.cc +++ b/internal/ceres/trust_region_minimizer.cc
@@ -340,11 +340,30 @@ LOG(WARNING) << "Step failed to evaluate. " << "Treating it as step with infinite cost"; new_cost = numeric_limits<double>::max(); - } else if (new_cost < cost && options.inner_iteration_minimizer != NULL) { - Solver::Summary inner_iteration_summary; - options.inner_iteration_minimizer->Minimize(options, - x_plus_delta.data(), - &inner_iteration_summary); + } else { + // Check if performing an inner iteration will make it better. + if (options.inner_iteration_minimizer != NULL) { + const double x_plus_delta_cost = new_cost; + Vector inner_iteration_x = x_plus_delta; + Solver::Summary inner_iteration_summary; + options.inner_iteration_minimizer->Minimize(options, + inner_iteration_x.data(), + &inner_iteration_summary); + if(!evaluator->Evaluate(inner_iteration_x.data(), + &new_cost, + NULL, NULL, NULL)) { + VLOG(2) << "Inner iteration failed."; + new_cost = x_plus_delta_cost; + } else { + x_plus_delta = inner_iteration_x; + // Bost the model_cost_change, since the inner iteration + // improvements are not accounted for by the trust region. + model_cost_change += x_plus_delta_cost - new_cost; + VLOG(2) << "Inner iteration succeeded; current cost: " << cost + << " x_plus_delta_cost: " << x_plus_delta_cost + << " new_cost: " << new_cost; + } + } } iteration_summary.step_norm = (x - x_plus_delta).norm();
diff --git a/jni/Android.mk b/jni/Android.mk index 98e16f5..0654580 100644 --- a/jni/Android.mk +++ b/jni/Android.mk
@@ -105,6 +105,7 @@ $(CERES_SRC_PATH)/compressed_row_sparse_matrix.cc \ $(CERES_SRC_PATH)/conditioned_cost_function.cc \ $(CERES_SRC_PATH)/conjugate_gradients_solver.cc \ + $(CERES_SRC_PATH)/coordinate_descent_minimizer.cc \ $(CERES_SRC_PATH)/corrector.cc \ $(CERES_SRC_PATH)/dense_normal_cholesky_solver.cc \ $(CERES_SRC_PATH)/dense_qr_solver.cc \ @@ -124,6 +125,7 @@ $(CERES_SRC_PATH)/loss_function.cc \ $(CERES_SRC_PATH)/normal_prior.cc \ $(CERES_SRC_PATH)/ordering.cc \ + $(CERES_SRC_PATH)/parmeter_block_ordering.cc \ $(CERES_SRC_PATH)/partitioned_matrix_view.cc \ $(CERES_SRC_PATH)/polynomial_solver.cc \ $(CERES_SRC_PATH)/problem.cc \ @@ -134,7 +136,6 @@ $(CERES_SRC_PATH)/runtime_numeric_diff_cost_function.cc \ $(CERES_SRC_PATH)/schur_complement_solver.cc \ $(CERES_SRC_PATH)/schur_eliminator.cc \ - $(CERES_SRC_PATH)/schur_ordering.cc \ $(CERES_SRC_PATH)/scratch_evaluate_preparer.cc \ $(CERES_SRC_PATH)/solver.cc \ $(CERES_SRC_PATH)/solver_impl.cc \