An implementation of Ruhe & Wedin's Algorithm II.
A non-linear generalization of Ruhe & Wedin's algorithm
for separable non-linear least squares problem. It is implemented
as coordinate descent on an independent subset of the parameter
blocks at the end of every successful Newton step. The resulting
algorithm has much improved convergence at the cost of some
execution time.
Change-Id: I8fdc5edbd0ba1e702c9658b98041b2c2ae705402
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 862b7cf..e094f6a 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -55,6 +55,7 @@
file.cc
gradient_checking_cost_function.cc
implicit_schur_complement.cc
+ inner_iteration_minimizer.cc
iterative_schur_complement_solver.cc
levenberg_marquardt_strategy.cc
linear_least_squares_problems.cc
diff --git a/internal/ceres/inner_iteration_minimizer.cc b/internal/ceres/inner_iteration_minimizer.cc
new file mode 100644
index 0000000..6a9b419
--- /dev/null
+++ b/internal/ceres/inner_iteration_minimizer.cc
@@ -0,0 +1,256 @@
+// 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/ordering.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));
+
+ Ordering 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.AddParameterBlockToGroup(ptr, 0);
+ } else {
+ ordering.AddParameterBlockToGroup(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.AddParameterBlockToGroup(ptr, 0);
+ } else {
+ ordering.AddParameterBlockToGroup(ptr, 1);
+ }
+ }
+
+ num_inner_iteration_parameter_blocks = ordering.GroupSize(0);
+ if (num_inner_iteration_parameter_blocks > 0) {
+ const map<int, set<double*> >& group_id_to_parameter_blocks =
+ ordering.group_id_to_parameter_blocks();
+ if (!SolverImpl::IsParameterBlockSetIndependent(
+ group_id_to_parameter_blocks.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/inner_iteration_minimizer.h b/internal/ceres/inner_iteration_minimizer.h
new file mode 100644
index 0000000..b7c7461
--- /dev/null
+++ b/internal/ceres/inner_iteration_minimizer.h
@@ -0,0 +1,87 @@
+// 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 <vector>
+#include "ceres/internal/scoped_ptr.h"
+#include "ceres/minimizer.h"
+#include "ceres/problem_impl.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.
+//
+// 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 {
+ 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,
+ string* error);
+
+ // Minimizer interface.
+ virtual ~InnerIterationMinimizer();
+ 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);
+
+ scoped_ptr<Program> program_;
+ vector<int> residual_block_offsets_;
+ Evaluator::Options evaluator_options_;
+ scoped_ptr<LinearSolver> linear_solver_;
+};
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/minimizer.h b/internal/ceres/minimizer.h
index 667d80a..22c10f0 100644
--- a/internal/ceres/minimizer.h
+++ b/internal/ceres/minimizer.h
@@ -59,6 +59,7 @@
}
void Init(const Solver::Options& options) {
+ num_threads = options.num_threads;
max_num_iterations = options.max_num_iterations;
max_solver_time_in_seconds = options.max_solver_time_in_seconds;
max_step_solver_retries = 5;
@@ -81,6 +82,7 @@
trust_region_strategy = NULL;
jacobian = NULL;
callbacks = options.callbacks;
+ inner_iteration_minimizer = NULL;
}
int max_num_iterations;
@@ -91,6 +93,7 @@
// mu at each retry. This leads to stronger and stronger
// regularization making the linear least squares problem better
// conditioned at each retry.
+ int num_threads;
int max_step_solver_retries;
double gradient_tolerance;
double parameter_tolerance;
@@ -126,6 +129,8 @@
// and will remain constant for the life time of the
// optimization. The Options struct does not own this pointer.
SparseMatrix* jacobian;
+
+ Minimizer* inner_iteration_minimizer;
};
virtual ~Minimizer() {}
diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h
index dd69cd6..f20805c 100644
--- a/internal/ceres/parameter_block.h
+++ b/internal/ceres/parameter_block.h
@@ -112,6 +112,10 @@
int index() const { return index_; }
void set_index(int index) { index_ = index; }
+ // This parameter offset inside a larger state vector.
+ int state_offset() const { return state_offset_; }
+ void set_state_offset(int state_offset) { state_offset_ = state_offset; }
+
// This parameter offset inside a larger delta vector.
int delta_offset() const { return delta_offset_; }
void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; }
@@ -172,13 +176,14 @@
string ToString() const {
return StringPrintf("{ user_state=%p, state=%p, size=%d, "
- "constant=%d, index=%d, "
+ "constant=%d, index=%d, state_offset=%d, "
"delta_offset=%d }",
user_state_,
state_,
size_,
is_constant_,
index_,
+ state_offset_,
delta_offset_);
}
@@ -197,6 +202,7 @@
}
index_ = -1;
+ state_offset_ = -1;
delta_offset_ = -1;
}
@@ -249,6 +255,9 @@
// permit switching from a ParameterBlock* to an index in another array.
int32 index_;
+ // The offset of this parameter block inside a larger state vector.
+ int32 state_offset_;
+
// The offset of this parameter block inside a larger delta vector.
int32 delta_offset_;
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc
index 9b20dca..4afe1b5 100644
--- a/internal/ceres/problem_test.cc
+++ b/internal/ceres/problem_test.cc
@@ -275,6 +275,7 @@
EXPECT_EQ(12, problem.NumParameters());
// Add a parameter that has a local parameterization.
+ w[0] = 1.0; w[1] = 0.0; w[2] = 0.0; w[3] = 0.0;
problem.AddParameterBlock(w, 4, new QuaternionParameterization);
EXPECT_EQ(4, problem.NumParameterBlocks());
EXPECT_EQ(16, problem.NumParameters());
diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc
index 99f6af9..82d76d3 100644
--- a/internal/ceres/program.cc
+++ b/internal/ceres/program.cc
@@ -129,10 +129,13 @@
}
}
// For parameters that appear in the program, set their position and offset.
+ int state_offset = 0;
int delta_offset = 0;
for (int i = 0; i < parameter_blocks_.size(); ++i) {
parameter_blocks_[i]->set_index(i);
+ parameter_blocks_[i]->set_state_offset(state_offset);
parameter_blocks_[i]->set_delta_offset(delta_offset);
+ state_offset += parameter_blocks_[i]->Size();
delta_offset += parameter_blocks_[i]->LocalSize();
}
}
diff --git a/internal/ceres/program_evaluator.h b/internal/ceres/program_evaluator.h
index 6c48e7d..1fb44e1 100644
--- a/internal/ceres/program_evaluator.h
+++ b/internal/ceres/program_evaluator.h
@@ -129,7 +129,7 @@
if (residuals != NULL) {
VectorRef(residuals, program_->NumResiduals()).setZero();
- }
+ }
if (jacobian != NULL) {
jacobian->SetZero();
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index c45c194..4f914fa 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -36,6 +36,7 @@
#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"
@@ -150,6 +151,7 @@
void SolverImpl::Minimize(const Solver::Options& options,
Program* program,
+ InnerIterationMinimizer* inner_iteration_minimizer,
Evaluator* evaluator,
LinearSolver* linear_solver,
double* parameters,
@@ -182,6 +184,7 @@
minimizer_options.evaluator = evaluator;
scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
minimizer_options.jacobian = jacobian.get();
+ minimizer_options.inner_iteration_minimizer = inner_iteration_minimizer;
TrustRegionStrategy::Options trust_region_strategy_options;
trust_region_strategy_options.linear_solver = linear_solver;
@@ -214,6 +217,11 @@
// Reset the summary object to its default values.
*CHECK_NOTNULL(summary) = Solver::Summary();
+ if (options.lsqp_iterations_to_dump.size() > 0) {
+ LOG(WARNING) << "Dumping linear least squares problems to disk is"
+ " currently broken. Ignoring Solver::Options::lsqp_iterations_to_dump";
+ }
+
#ifndef CERES_USE_OPENMP
if (options.num_threads > 1) {
LOG(WARNING)
@@ -323,16 +331,35 @@
return;
}
- if (!MaybeReorderResidualBlocks(options,
- reduced_program.get(),
- &summary->error)) {
+ // Only Schur types require the lexicographic reordering.
+ if (IsSchurType(options.linear_solver_type)) {
+ const int num_eliminate_blocks =
+ options.ordering->group_id_to_parameter_blocks().begin()->second.size();
+ if (!LexicographicallyOrderResidualBlocks(num_eliminate_blocks,
+ reduced_program.get(),
+ &summary->error)) {
+ return;
+ }
+ }
+
+ scoped_ptr<Evaluator> evaluator(CreateEvaluator(options,
+ problem_impl->parameter_map(),
+ reduced_program.get(),
+ &summary->error));
+ if (evaluator == NULL) {
return;
}
- scoped_ptr<Evaluator> evaluator(
- CreateEvaluator(options, reduced_program.get(), &summary->error));
- if (evaluator == NULL) {
- return;
+ scoped_ptr<InnerIterationMinimizer> inner_iteration_minimizer;
+ 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)) {
+ return;
+ }
}
// The optimizer works on contiguous parameter vectors; allocate some.
@@ -348,6 +375,7 @@
// Run the optimization.
Minimize(options,
reduced_program.get(),
+ inner_iteration_minimizer.get(),
evaluator.get(),
linear_solver.get(),
parameters.data(),
@@ -412,33 +440,41 @@
options.ordering->NumGroups() > 1) {
const set<double*>& e_blocks =
options.ordering->group_id_to_parameter_blocks().begin()->second;
-
- // Loop over each residual block and ensure that no two parameter
- // blocks in the same residual block are part of the first
- // elimination group, as that would violate the assumption that it
- // is an independent set in the Hessian matrix.
- for (vector<ResidualBlock*>::const_iterator it = residual_blocks.begin();
- it != residual_blocks.end();
- ++it) {
- ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks();
- const int num_parameter_blocks = (*it)->NumParameterBlocks();
- int count = 0;
- for (int i = 0; i < num_parameter_blocks; ++i) {
- count += e_blocks.count(const_cast<double*>(
- parameter_blocks[i]->user_state()));
- }
-
- if (count > 1) {
- *error = "The user requested the use of a Schur type solver. "
- "But the first elimination group in the ordering is not an "
- "independent set.";
- return false;
- }
+ if (!IsParameterBlockSetIndependent(e_blocks, residual_blocks)) {
+ *error = "The user requested the use of a Schur type solver. "
+ "But the first elimination group in the ordering is not an "
+ "independent set.";
+ return false;
}
}
return true;
}
+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
+ // is an independent set in the Hessian matrix.
+ for (vector<ResidualBlock*>::const_iterator it = residual_blocks.begin();
+ it != residual_blocks.end();
+ ++it) {
+ ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks();
+ 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()));
+ }
+ if (count > 1) {
+ return false;
+ }
+ }
+ return true;
+}
+
+
// Strips varying parameters and residuals, maintaining order, and updating
// num_eliminate_blocks.
bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program,
@@ -572,8 +608,10 @@
}
}
- if (!ApplyUserOrdering(
- *problem_impl, ordering, transformed_program.get(), error)) {
+ if (!ApplyUserOrdering(problem_impl->parameter_map(),
+ ordering,
+ transformed_program.get(),
+ error)) {
return NULL;
}
@@ -744,7 +782,7 @@
return LinearSolver::Create(linear_solver_options);
}
-bool SolverImpl::ApplyUserOrdering(const ProblemImpl& problem_impl,
+bool SolverImpl::ApplyUserOrdering(const ProblemImpl::ParameterMap& parameter_map,
const Ordering* ordering,
Program* program,
string* error) {
@@ -761,8 +799,6 @@
program->mutable_parameter_blocks();
parameter_blocks->clear();
- const ProblemImpl::ParameterMap& parameter_map =
- problem_impl.parameter_map();
const map<int, set<double*> >& groups =
ordering->group_id_to_parameter_blocks();
@@ -810,16 +846,12 @@
// Reorder the residuals for program, if necessary, so that the residuals
// involving each E block occur together. This is a necessary condition for the
// Schur eliminator, which works on these "row blocks" in the jacobian.
-bool SolverImpl::MaybeReorderResidualBlocks(const Solver::Options& options,
- Program* program,
- string* error) {
- // Only Schur types require the lexicographic reordering.
- if (!IsSchurType(options.linear_solver_type)) {
- return true;
- }
-
- const int num_eliminate_blocks =
- options.ordering->group_id_to_parameter_blocks().begin()->second.size();
+bool SolverImpl::LexicographicallyOrderResidualBlocks(const int num_eliminate_blocks,
+ Program* program,
+ string* error) {
+ CHECK_GE(num_eliminate_blocks, 1)
+ << "Congratulations, you found a Ceres bug! Please report this error "
+ << "to the developers.";
// Create a histogram of the number of residuals for each E block. There is an
// extra bucket at the end to catch all non-eliminated F blocks.
@@ -894,16 +926,16 @@
}
Evaluator* SolverImpl::CreateEvaluator(const Solver::Options& options,
+ const ProblemImpl::ParameterMap& parameter_map,
Program* program,
string* error) {
Evaluator::Options evaluator_options;
evaluator_options.linear_solver_type = options.linear_solver_type;
-
evaluator_options.num_eliminate_blocks =
(options.ordering->NumGroups() > 0 &&
IsSchurType(options.linear_solver_type))
- ? options.ordering->group_id_to_parameter_blocks().begin()->second.size()
- : 0;
+ ? options.ordering->group_id_to_parameter_blocks().begin()->second.size()
+ : 0;
evaluator_options.num_threads = options.num_threads;
return Evaluator::Create(evaluator_options, program, error);
}
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h
index 076a371..eb81696 100644
--- a/internal/ceres/solver_impl.h
+++ b/internal/ceres/solver_impl.h
@@ -35,6 +35,7 @@
#include <vector>
#include "ceres/internal/port.h"
#include "ceres/solver.h"
+#include "ceres/problem_impl.h"
namespace ceres {
class Ordering;
@@ -43,7 +44,7 @@
class Evaluator;
class LinearSolver;
-class ProblemImpl;
+class InnerIterationMinimizer;
class Program;
class SolverImpl {
@@ -58,6 +59,7 @@
// and residuals eliminated, and in the case of automatic schur
// ordering, has the E blocks first in the resulting program, with
// options.num_eliminate_blocks set appropriately.
+ //
// If fixed_cost is not NULL, the residual blocks that are removed
// are evaluated and the sum of their cost is returned in fixed_cost.
static Program* CreateReducedProgram(Solver::Options* options,
@@ -73,31 +75,35 @@
static LinearSolver* CreateLinearSolver(Solver::Options* options,
string* error);
- // Reorder the parameter blocks in program using the vector
- // ordering. A return value of true indicates success and false
- // indicates an error was encountered whose cause is logged to
- // LOG(ERROR).
- static bool ApplyUserOrdering(const ProblemImpl& problem_impl,
+ // Reorder the parameter blocks in program using the ordering. A
+ // return value of true indicates success and false indicates an
+ // error was encountered whose cause is logged to LOG(ERROR).
+ static bool ApplyUserOrdering(const ProblemImpl::ParameterMap& parameter_map,
const Ordering* ordering,
Program* program,
string* error);
+
// Reorder the residuals for program, if necessary, so that the
- // residuals involving each E block occur together. This is a
- // necessary condition for the Schur eliminator, which works on
- // these "row blocks" in the jacobian.
- static bool MaybeReorderResidualBlocks(const Solver::Options& options,
- Program* program,
- string* error);
+ // 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.
+ static bool LexicographicallyOrderResidualBlocks(
+ const int num_eliminate_blocks,
+ Program* program,
+ string* error);
// Create the appropriate evaluator for the transformed program.
- static Evaluator* CreateEvaluator(const Solver::Options& options,
- Program* program,
- string* error);
+ static Evaluator* CreateEvaluator(
+ const Solver::Options& options,
+ const ProblemImpl::ParameterMap& parameter_map,
+ Program* program,
+ string* error);
// Run the minimization for the given evaluator and configuration.
static void Minimize(const Solver::Options &options,
Program* program,
+ InnerIterationMinimizer* inner_iteration_minimizer,
Evaluator* evaluator,
LinearSolver* linear_solver,
double* parameters,
@@ -106,9 +112,9 @@
// Remove the fixed or unused parameter blocks and residuals
// depending only on fixed parameters from the problem. Also updates
// num_eliminate_blocks, since removed parameters changes the point
- // at which the eliminated blocks is valid.
- // If fixed_cost is not NULL, the residual blocks that are removed
- // are evaluated and the sum of their cost is returned in fixed_cost.
+ // at which the eliminated blocks is valid. If fixed_cost is not
+ // NULL, the residual blocks that are removed are evaluated and the
+ // sum of their cost is returned in fixed_cost.
static bool RemoveFixedBlocksFromProgram(Program* program,
Ordering* ordering,
double* fixed_cost,
@@ -117,6 +123,10 @@
static bool IsOrderingValid(const Solver::Options& options,
const ProblemImpl* problem_impl,
string* error);
+
+ static bool IsParameterBlockSetIndependent(
+ const set<double*> parameter_block_ptrs,
+ const vector<ResidualBlock*> residual_blocks);
};
} // namespace internal
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc
index a19fca0..e612f8f 100644
--- a/internal/ceres/solver_impl_test.cc
+++ b/internal/ceres/solver_impl_test.cc
@@ -282,9 +282,10 @@
options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
string error;
- EXPECT_TRUE(SolverImpl::MaybeReorderResidualBlocks(options,
- problem.mutable_program(),
- &error));
+ EXPECT_FALSE(SolverImpl::LexicographicallyOrderResidualBlocks(
+ 0,
+ problem.mutable_program(),
+ &error));
EXPECT_EQ(current_residual_blocks.size(), residual_blocks.size());
for (int i = 0; i < current_residual_blocks.size(); ++i) {
EXPECT_EQ(current_residual_blocks[i], residual_blocks[i]);
@@ -337,9 +338,10 @@
program->SetParameterOffsetsAndIndex();
string error;
- EXPECT_TRUE(SolverImpl::MaybeReorderResidualBlocks(options,
- problem.mutable_program(),
- &error));
+ EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks(
+ 2,
+ problem.mutable_program(),
+ &error));
EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());
for (int i = 0; i < expected_residual_blocks.size(); ++i) {
EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);
@@ -407,9 +409,10 @@
expected_residual_blocks.push_back(residual_blocks[3]);
expected_residual_blocks.push_back(residual_blocks[2]);
- EXPECT_TRUE(SolverImpl::MaybeReorderResidualBlocks(options,
- reduced_program.get(),
- &error));
+ EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks(
+ 2,
+ reduced_program.get(),
+ &error));
EXPECT_EQ(reduced_program->residual_blocks().size(),
expected_residual_blocks.size());
@@ -435,7 +438,7 @@
Program program(problem.program());
string error;
- EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem,
+ EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem.parameter_map(),
&ordering,
&program,
&error));
@@ -459,7 +462,7 @@
Program* program = problem.mutable_program();
string error;
- EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem,
+ EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem.parameter_map(),
&ordering,
program,
&error));
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index dd49f9e..b838b19 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -45,6 +45,7 @@
#include "ceres/internal/scoped_ptr.h"
#include "ceres/linear_least_squares_problems.h"
#include "ceres/sparse_matrix.h"
+#include "ceres/stringprintf.h"
#include "ceres/trust_region_strategy.h"
#include "ceres/types.h"
#include "ceres/wall_time.h"
@@ -102,8 +103,6 @@
//
// Both of these indicate that this is the wrong place for this
// code, and going forward this should needs fixing/refactoring.
- LOG(WARNING) << "Dumping linear least squares problem to disk is "
- << "currently broken.";
return true;
}
@@ -142,7 +141,6 @@
iteration_summary.iteration = 0;
iteration_summary.step_is_valid = false;
iteration_summary.step_is_successful = false;
- iteration_summary.cost = summary->initial_cost;
iteration_summary.cost_change = 0.0;
iteration_summary.gradient_max_norm = 0.0;
iteration_summary.step_norm = 0.0;
@@ -162,6 +160,8 @@
return;
}
+ iteration_summary.cost = cost + summary->fixed_cost;
+
int num_consecutive_nonmonotonic_steps = 0;
double minimum_cost = cost;
double reference_cost = cost;
@@ -294,10 +294,12 @@
if (++num_consecutive_invalid_steps >=
options_.max_num_consecutive_invalid_steps) {
summary->termination_type = NUMERICAL_FAILURE;
- LOG(WARNING) << "Terminating. Number of successive invalid steps more "
- << "than "
- << "Solver::Options::max_num_consecutive_invalid_steps: "
- << options_.max_num_consecutive_invalid_steps;
+ summary->error = StringPrintf(
+ "Terminating. Number of successive invalid steps more "
+ "than Solver::Options::max_num_consecutive_invalid_steps: %d",
+ options_.max_num_consecutive_invalid_steps);
+
+ LOG(WARNING) << summary->error;
return;
}
@@ -306,7 +308,7 @@
// as an unsuccessful iteration. Since the various callbacks are
// still executed, we are going to fill the iteration summary
// with data that assumes a step of length zero and no progress.
- iteration_summary.cost = cost;
+ iteration_summary.cost = cost + summary->fixed_cost;
iteration_summary.cost_change = 0.0;
iteration_summary.gradient_max_norm =
summary->iterations.back().gradient_max_norm;
@@ -319,7 +321,33 @@
// Undo the Jacobian column scaling.
delta = (trust_region_step.array() * scale.array()).matrix();
- iteration_summary.step_norm = delta.norm();
+ if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+ summary->termination_type = NUMERICAL_FAILURE;
+ summary->error =
+ "Terminating. Failed to compute Plus(x, delta, x_plus_delta).";
+
+ LOG(WARNING) << summary->error;
+ return;
+ }
+
+ // Try this step.
+ double new_cost = numeric_limits<double>::max();
+ if (!evaluator->Evaluate(x_plus_delta.data(),
+ &new_cost,
+ NULL, NULL, NULL)) {
+ // If the evaluation of the new cost fails, treat it as a step
+ // with high cost.
+ 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);
+ }
+
+ iteration_summary.step_norm = (x - x_plus_delta).norm();
// Convergence based on parameter_tolerance.
const double step_size_tolerance = options_.parameter_tolerance *
@@ -334,25 +362,6 @@
return;
}
- if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
- summary->termination_type = NUMERICAL_FAILURE;
- LOG(WARNING) << "Terminating. Failed to compute "
- << "Plus(x, delta, x_plus_delta).";
- return;
- }
-
- // Try this step.
- double new_cost;
- if (!evaluator->Evaluate(x_plus_delta.data(),
- &new_cost,
- NULL, NULL, NULL)) {
- // If the evaluation of the new cost fails, treat it as a step
- // with high cost.
- LOG(WARNING) << "Step failed to evaluate. "
- << "Treating it as step with infinite cost";
- new_cost = numeric_limits<double>::max();
- }
-
VLOG(2) << "old cost: " << cost << " new cost: " << new_cost;
iteration_summary.cost_change = cost - new_cost;
const double absolute_function_tolerance =
@@ -420,7 +429,8 @@
NULL,
jacobian)) {
summary->termination_type = NUMERICAL_FAILURE;
- LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed.";
+ summary->error = "Terminating: Residual and Jacobian evaluation failed.";
+ LOG(WARNING) << summary->error;
return;
}
