Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc
new file mode 100644
index 0000000..f1cd1bb
--- /dev/null
+++ b/internal/ceres/program.cc
@@ -0,0 +1,526 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: keir@google.com (Keir Mierle)
+
+#include "ceres/program.h"
+
+#include <algorithm>
+#include <map>
+#include <memory>
+#include <vector>
+
+#include "ceres/array_utils.h"
+#include "ceres/casts.h"
+#include "ceres/compressed_row_sparse_matrix.h"
+#include "ceres/cost_function.h"
+#include "ceres/evaluator.h"
+#include "ceres/internal/port.h"
+#include "ceres/local_parameterization.h"
+#include "ceres/loss_function.h"
+#include "ceres/map_util.h"
+#include "ceres/parameter_block.h"
+#include "ceres/problem.h"
+#include "ceres/residual_block.h"
+#include "ceres/stl_util.h"
+#include "ceres/triplet_sparse_matrix.h"
+
+namespace ceres {
+namespace internal {
+
+using std::max;
+using std::set;
+using std::string;
+using std::vector;
+
+Program::Program() {}
+
+Program::Program(const Program& program)
+ : parameter_blocks_(program.parameter_blocks_),
+ residual_blocks_(program.residual_blocks_) {
+}
+
+const vector<ParameterBlock*>& Program::parameter_blocks() const {
+ return parameter_blocks_;
+}
+
+const vector<ResidualBlock*>& Program::residual_blocks() const {
+ return residual_blocks_;
+}
+
+vector<ParameterBlock*>* Program::mutable_parameter_blocks() {
+ return ¶meter_blocks_;
+}
+
+vector<ResidualBlock*>* Program::mutable_residual_blocks() {
+ return &residual_blocks_;
+}
+
+bool Program::StateVectorToParameterBlocks(const double *state) {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ if (!parameter_blocks_[i]->IsConstant() &&
+ !parameter_blocks_[i]->SetState(state)) {
+ return false;
+ }
+ state += parameter_blocks_[i]->Size();
+ }
+ return true;
+}
+
+void Program::ParameterBlocksToStateVector(double *state) const {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ parameter_blocks_[i]->GetState(state);
+ state += parameter_blocks_[i]->Size();
+ }
+}
+
+void Program::CopyParameterBlockStateToUserState() {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state());
+ }
+}
+
+bool Program::SetParameterBlockStatePtrsToUserStatePtrs() {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ if (!parameter_blocks_[i]->IsConstant() &&
+ !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool Program::Plus(const double* state,
+ const double* delta,
+ double* state_plus_delta) const {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) {
+ return false;
+ }
+ state += parameter_blocks_[i]->Size();
+ delta += parameter_blocks_[i]->LocalSize();
+ state_plus_delta += parameter_blocks_[i]->Size();
+ }
+ return true;
+}
+
+void Program::SetParameterOffsetsAndIndex() {
+ // Set positions for all parameters appearing as arguments to residuals to one
+ // past the end of the parameter block array.
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ ResidualBlock* residual_block = residual_blocks_[i];
+ for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
+ residual_block->parameter_blocks()[j]->set_index(-1);
+ }
+ }
+ // 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();
+ }
+}
+
+bool Program::IsValid() const {
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ const ResidualBlock* residual_block = residual_blocks_[i];
+ if (residual_block->index() != i) {
+ LOG(WARNING) << "Residual block: " << i
+ << " has incorrect index: " << residual_block->index();
+ return false;
+ }
+ }
+
+ int state_offset = 0;
+ int delta_offset = 0;
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ if (parameter_block->index() != i ||
+ parameter_block->state_offset() != state_offset ||
+ parameter_block->delta_offset() != delta_offset) {
+ LOG(WARNING) << "Parameter block: " << i
+ << "has incorrect indexing information: "
+ << parameter_block->ToString();
+ return false;
+ }
+
+ state_offset += parameter_blocks_[i]->Size();
+ delta_offset += parameter_blocks_[i]->LocalSize();
+ }
+
+ return true;
+}
+
+bool Program::ParameterBlocksAreFinite(string* message) const {
+ CHECK(message != nullptr);
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ const double* array = parameter_block->user_state();
+ const int size = parameter_block->Size();
+ const int invalid_index = FindInvalidValue(size, array);
+ if (invalid_index != size) {
+ *message = StringPrintf(
+ "ParameterBlock: %p with size %d has at least one invalid value.\n"
+ "First invalid value is at index: %d.\n"
+ "Parameter block values: ",
+ array, size, invalid_index);
+ AppendArrayToString(size, array, message);
+ return false;
+ }
+ }
+ return true;
+}
+
+bool Program::IsBoundsConstrained() const {
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ if (parameter_block->IsConstant()) {
+ continue;
+ }
+ const int size = parameter_block->Size();
+ for (int j = 0; j < size; ++j) {
+ const double lower_bound = parameter_block->LowerBoundForParameter(j);
+ const double upper_bound = parameter_block->UpperBoundForParameter(j);
+ if (lower_bound > -std::numeric_limits<double>::max() ||
+ upper_bound < std::numeric_limits<double>::max()) {
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+bool Program::IsFeasible(string* message) const {
+ CHECK(message != nullptr);
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ const ParameterBlock* parameter_block = parameter_blocks_[i];
+ const double* parameters = parameter_block->user_state();
+ const int size = parameter_block->Size();
+ if (parameter_block->IsConstant()) {
+ // Constant parameter blocks must start in the feasible region
+ // to ultimately produce a feasible solution, since Ceres cannot
+ // change them.
+ for (int j = 0; j < size; ++j) {
+ const double lower_bound = parameter_block->LowerBoundForParameter(j);
+ const double upper_bound = parameter_block->UpperBoundForParameter(j);
+ if (parameters[j] < lower_bound || parameters[j] > upper_bound) {
+ *message = StringPrintf(
+ "ParameterBlock: %p with size %d has at least one infeasible "
+ "value."
+ "\nFirst infeasible value is at index: %d."
+ "\nLower bound: %e, value: %e, upper bound: %e"
+ "\nParameter block values: ",
+ parameters, size, j, lower_bound, parameters[j], upper_bound);
+ AppendArrayToString(size, parameters, message);
+ return false;
+ }
+ }
+ } else {
+ // Variable parameter blocks must have non-empty feasible
+ // regions, otherwise there is no way to produce a feasible
+ // solution.
+ for (int j = 0; j < size; ++j) {
+ const double lower_bound = parameter_block->LowerBoundForParameter(j);
+ const double upper_bound = parameter_block->UpperBoundForParameter(j);
+ if (lower_bound >= upper_bound) {
+ *message = StringPrintf(
+ "ParameterBlock: %p with size %d has at least one infeasible "
+ "bound."
+ "\nFirst infeasible bound is at index: %d."
+ "\nLower bound: %e, upper bound: %e"
+ "\nParameter block values: ",
+ parameters, size, j, lower_bound, upper_bound);
+ AppendArrayToString(size, parameters, message);
+ return false;
+ }
+ }
+ }
+ }
+
+ return true;
+}
+
+Program* Program::CreateReducedProgram(
+ vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* error) const {
+ CHECK(removed_parameter_blocks != nullptr);
+ CHECK(fixed_cost != nullptr);
+ CHECK(error != nullptr);
+
+ std::unique_ptr<Program> reduced_program(new Program(*this));
+ if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks,
+ fixed_cost,
+ error)) {
+ return NULL;
+ }
+
+ reduced_program->SetParameterOffsetsAndIndex();
+ return reduced_program.release();
+}
+
+bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* error) {
+ CHECK(removed_parameter_blocks != nullptr);
+ CHECK(fixed_cost != nullptr);
+ CHECK(error != nullptr);
+
+ std::unique_ptr<double[]> residual_block_evaluate_scratch;
+ residual_block_evaluate_scratch.reset(
+ new double[MaxScratchDoublesNeededForEvaluate()]);
+ *fixed_cost = 0.0;
+
+ // Mark all the parameters as unused. Abuse the index member of the
+ // parameter blocks for the marking.
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ parameter_blocks_[i]->set_index(-1);
+ }
+
+ // Filter out residual that have all-constant parameters, and mark
+ // all the parameter blocks that appear in residuals.
+ int num_active_residual_blocks = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ ResidualBlock* residual_block = residual_blocks_[i];
+ int num_parameter_blocks = residual_block->NumParameterBlocks();
+
+ // Determine if the residual block is fixed, and also mark varying
+ // parameters that appear in the residual block.
+ bool all_constant = true;
+ for (int k = 0; k < num_parameter_blocks; k++) {
+ ParameterBlock* parameter_block = residual_block->parameter_blocks()[k];
+ if (!parameter_block->IsConstant()) {
+ all_constant = false;
+ parameter_block->set_index(1);
+ }
+ }
+
+ if (!all_constant) {
+ residual_blocks_[num_active_residual_blocks++] = residual_block;
+ continue;
+ }
+
+ // The residual is constant and will be removed, so its cost is
+ // added to the variable fixed_cost.
+ double cost = 0.0;
+ if (!residual_block->Evaluate(true,
+ &cost,
+ NULL,
+ NULL,
+ residual_block_evaluate_scratch.get())) {
+ *error = StringPrintf("Evaluation of the residual %d failed during "
+ "removal of fixed residual blocks.", i);
+ return false;
+ }
+ *fixed_cost += cost;
+ }
+ residual_blocks_.resize(num_active_residual_blocks);
+
+ // Filter out unused or fixed parameter blocks.
+ int num_active_parameter_blocks = 0;
+ removed_parameter_blocks->clear();
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ ParameterBlock* parameter_block = parameter_blocks_[i];
+ if (parameter_block->index() == -1) {
+ removed_parameter_blocks->push_back(
+ parameter_block->mutable_user_state());
+ } else {
+ parameter_blocks_[num_active_parameter_blocks++] = parameter_block;
+ }
+ }
+ parameter_blocks_.resize(num_active_parameter_blocks);
+
+ if (!(((NumResidualBlocks() == 0) &&
+ (NumParameterBlocks() == 0)) ||
+ ((NumResidualBlocks() != 0) &&
+ (NumParameterBlocks() != 0)))) {
+ *error = "Congratulations, you found a bug in Ceres. Please report it.";
+ return false;
+ }
+
+ return true;
+}
+
+bool Program::IsParameterBlockSetIndependent(
+ const set<double*>& independent_set) const {
+ // 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 (const ResidualBlock* residual_block : residual_blocks_) {
+ ParameterBlock* const* parameter_blocks =
+ residual_block->parameter_blocks();
+ const int num_parameter_blocks = residual_block->NumParameterBlocks();
+ int count = 0;
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ count += independent_set.count(
+ parameter_blocks[i]->mutable_user_state());
+ }
+ if (count > 1) {
+ return false;
+ }
+ }
+ return true;
+}
+
+TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const {
+ // Matrix to store the block sparsity structure of the Jacobian.
+ TripletSparseMatrix* tsm =
+ new TripletSparseMatrix(NumParameterBlocks(),
+ NumResidualBlocks(),
+ 10 * NumResidualBlocks());
+ int num_nonzeros = 0;
+ int* rows = tsm->mutable_rows();
+ int* cols = tsm->mutable_cols();
+ double* values = tsm->mutable_values();
+
+ for (int c = 0; c < residual_blocks_.size(); ++c) {
+ const ResidualBlock* residual_block = residual_blocks_[c];
+ const int num_parameter_blocks = residual_block->NumParameterBlocks();
+ ParameterBlock* const* parameter_blocks =
+ residual_block->parameter_blocks();
+
+ for (int j = 0; j < num_parameter_blocks; ++j) {
+ if (parameter_blocks[j]->IsConstant()) {
+ continue;
+ }
+
+ // Re-size the matrix if needed.
+ if (num_nonzeros >= tsm->max_num_nonzeros()) {
+ tsm->set_num_nonzeros(num_nonzeros);
+ tsm->Reserve(2 * num_nonzeros);
+ rows = tsm->mutable_rows();
+ cols = tsm->mutable_cols();
+ values = tsm->mutable_values();
+ }
+
+ const int r = parameter_blocks[j]->index();
+ rows[num_nonzeros] = r;
+ cols[num_nonzeros] = c;
+ values[num_nonzeros] = 1.0;
+ ++num_nonzeros;
+ }
+ }
+
+ tsm->set_num_nonzeros(num_nonzeros);
+ return tsm;
+}
+
+int Program::NumResidualBlocks() const {
+ return residual_blocks_.size();
+}
+
+int Program::NumParameterBlocks() const {
+ return parameter_blocks_.size();
+}
+
+int Program::NumResiduals() const {
+ int num_residuals = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ num_residuals += residual_blocks_[i]->NumResiduals();
+ }
+ return num_residuals;
+}
+
+int Program::NumParameters() const {
+ int num_parameters = 0;
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ num_parameters += parameter_blocks_[i]->Size();
+ }
+ return num_parameters;
+}
+
+int Program::NumEffectiveParameters() const {
+ int num_parameters = 0;
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ num_parameters += parameter_blocks_[i]->LocalSize();
+ }
+ return num_parameters;
+}
+
+int Program::MaxScratchDoublesNeededForEvaluate() const {
+ // Compute the scratch space needed for evaluate.
+ int max_scratch_bytes_for_evaluate = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ max_scratch_bytes_for_evaluate =
+ max(max_scratch_bytes_for_evaluate,
+ residual_blocks_[i]->NumScratchDoublesForEvaluate());
+ }
+ return max_scratch_bytes_for_evaluate;
+}
+
+int Program::MaxDerivativesPerResidualBlock() const {
+ int max_derivatives = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ int derivatives = 0;
+ ResidualBlock* residual_block = residual_blocks_[i];
+ int num_parameters = residual_block->NumParameterBlocks();
+ for (int j = 0; j < num_parameters; ++j) {
+ derivatives += residual_block->NumResiduals() *
+ residual_block->parameter_blocks()[j]->LocalSize();
+ }
+ max_derivatives = max(max_derivatives, derivatives);
+ }
+ return max_derivatives;
+}
+
+int Program::MaxParametersPerResidualBlock() const {
+ int max_parameters = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ max_parameters = max(max_parameters,
+ residual_blocks_[i]->NumParameterBlocks());
+ }
+ return max_parameters;
+}
+
+int Program::MaxResidualsPerResidualBlock() const {
+ int max_residuals = 0;
+ for (int i = 0; i < residual_blocks_.size(); ++i) {
+ max_residuals = max(max_residuals, residual_blocks_[i]->NumResiduals());
+ }
+ return max_residuals;
+}
+
+string Program::ToString() const {
+ string ret = "Program dump\n";
+ ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks());
+ ret += StringPrintf("Number of parameters: %d\n", NumParameters());
+ ret += "Parameters:\n";
+ for (int i = 0; i < parameter_blocks_.size(); ++i) {
+ ret += StringPrintf("%d: %s\n",
+ i, parameter_blocks_[i]->ToString().c_str());
+ }
+ return ret;
+}
+
+} // namespace internal
+} // namespace ceres