Refactor Program related functions.
Move
ParameterBlocksAreFinite
IsBoundsConstrained
IsFeasible
RemoveFixedBlocks
IsParameterBlockSetIndependent
CreateJacobianBlockSparsity
from being static methods in SolverImpl to member functions
in the Program class.
Change-Id: I80fa4a429a716ea4371ad6c67864adad438e1553
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 18ff92b..a54db8a 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -261,6 +261,7 @@
CERES_TEST(partitioned_matrix_view)
CERES_TEST(polynomial)
CERES_TEST(problem)
+ CERES_TEST(program)
CERES_TEST(residual_block)
CERES_TEST(residual_block_utils)
CERES_TEST(rotation)
diff --git a/internal/ceres/program.cc b/internal/ceres/program.cc
index 9e5c51b..3a88b13 100644
--- a/internal/ceres/program.cc
+++ b/internal/ceres/program.cc
@@ -32,6 +32,7 @@
#include <map>
#include <vector>
+#include "ceres/array_utils.h"
#include "ceres/casts.h"
#include "ceres/compressed_row_sparse_matrix.h"
#include "ceres/cost_function.h"
@@ -44,6 +45,7 @@
#include "ceres/problem.h"
#include "ceres/residual_block.h"
#include "ceres/stl_util.h"
+#include "ceres/triplet_sparse_matrix.h"
namespace ceres {
namespace internal {
@@ -170,6 +172,241 @@
return true;
}
+bool Program::ParameterBlocksAreFinite(string* message) const {
+ CHECK_NOTNULL(message);
+ 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_NOTNULL(message);
+ 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;
+}
+
+bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* error) {
+ CHECK_NOTNULL(removed_parameter_blocks);
+ CHECK_NOTNULL(fixed_cost);
+ CHECK_NOTNULL(error);
+
+ scoped_array<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 (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 += 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();
}
diff --git a/internal/ceres/program.h b/internal/ceres/program.h
index 4288f60..7f2fc9d 100644
--- a/internal/ceres/program.h
+++ b/internal/ceres/program.h
@@ -31,6 +31,7 @@
#ifndef CERES_INTERNAL_PROGRAM_H_
#define CERES_INTERNAL_PROGRAM_H_
+#include <set>
#include <string>
#include <vector>
#include "ceres/internal/port.h"
@@ -41,6 +42,7 @@
class ParameterBlock;
class ProblemImpl;
class ResidualBlock;
+class TripletSparseMatrix;
// A nonlinear least squares optimization problem. This is different from the
// similarly-named "Problem" object, which offers a mutation interface for
@@ -103,6 +105,37 @@
// offsets) are correct.
bool IsValid() const;
+ bool ParameterBlocksAreFinite(string* message) const;
+
+ // Returns true if the program has any non-constant parameter blocks
+ // which have non-trivial bounds constraints.
+ bool IsBoundsConstrained() const;
+
+ // Returns false, if the program has any constant parameter blocks
+ // which are not feasible, or any variable parameter blocks which
+ // have a lower bound greater than or equal to the upper bound.
+ bool IsFeasible(string* message) const;
+
+ // Loop over each residual block and ensure that no two parameter
+ // blocks in the same residual block are part of
+ // parameter_blocks as that would violate the assumption that it
+ // is an independent set in the Hessian matrix.
+ bool IsParameterBlockSetIndependent(const set<double*>& independent_set) const;
+
+ // Create a TripletSparseMatrix which contains the zero-one
+ // structure corresponding to the block sparsity of the transpose of
+ // the Jacobian matrix.
+ //
+ // Caller owns the result.
+ TripletSparseMatrix* CreateJacobianBlockSparsityTranspose() const;
+
+ // Removes constant parameter blocks and residual blocks with no
+ // varying parameter blocks while preserving order.
+ // TODO(sameeragarwal): Update message here.
+ bool RemoveFixedBlocks(vector<double*>* removed_parameter_blocks,
+ double* fixed_cost,
+ string* message);
+
// See problem.h for what these do.
int NumParameterBlocks() const;
int NumParameters() const;
diff --git a/internal/ceres/program_test.cc b/internal/ceres/program_test.cc
new file mode 100644
index 0000000..79adfa6
--- /dev/null
+++ b/internal/ceres/program_test.cc
@@ -0,0 +1,418 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2014 Google Inc. All rights reserved.
+// http://code.google.com/p/ceres-solver/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/program.h"
+
+#include <limits>
+#include <cmath>
+#include <vector>
+#include "ceres/sized_cost_function.h"
+#include "ceres/problem_impl.h"
+#include "ceres/residual_block.h"
+#include "ceres/triplet_sparse_matrix.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+// A cost function that simply returns its argument.
+class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ residuals[0] = parameters[0][0];
+ if (jacobians != NULL && jacobians[0] != NULL) {
+ jacobians[0][0] = 1.0;
+ }
+ return true;
+ }
+};
+
+// Templated base class for the CostFunction signatures.
+template <int kNumResiduals, int N0, int N1, int N2>
+class MockCostFunctionBase : public
+SizedCostFunction<kNumResiduals, N0, N1, N2> {
+ public:
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ for (int i = 0; i < kNumResiduals; ++i) {
+ residuals[i] = kNumResiduals + N0 + N1 + N2;
+ }
+ return true;
+ }
+};
+
+class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
+class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
+class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
+
+TEST(Program, RemoveFixedBlocksNothingConstant) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+
+ Program* program = problem.mutable_program();
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ EXPECT_TRUE(program->RemoveFixedBlocks(&removed_parameter_blocks,
+ &fixed_cost,
+ &message));
+ EXPECT_EQ(program->NumParameterBlocks(), 3);
+ EXPECT_EQ(program->NumResidualBlocks(), 3);
+ EXPECT_EQ(removed_parameter_blocks.size(), 0);
+ EXPECT_EQ(fixed_cost, 0.0);
+}
+
+TEST(Program, RemoveFixedBlocksAllParameterBlocksConstant) {
+ ProblemImpl problem;
+ double x = 1.0;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.SetParameterBlockConstant(&x);
+
+ Program* program = problem.mutable_program();
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ EXPECT_TRUE(program->RemoveFixedBlocks(&removed_parameter_blocks,
+ &fixed_cost,
+ &message));
+ EXPECT_EQ(program->NumParameterBlocks(), 0);
+ EXPECT_EQ(program->NumResidualBlocks(), 0);
+ EXPECT_EQ(removed_parameter_blocks.size(), 1);
+ EXPECT_EQ(removed_parameter_blocks[0], &x);
+ EXPECT_EQ(fixed_cost, 9.0);
+}
+
+
+TEST(Program, RemoveFixedBlocksNoResidualBlocks) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ Program* program = problem.mutable_program();
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ EXPECT_TRUE(program->RemoveFixedBlocks(&removed_parameter_blocks,
+ &fixed_cost,
+ &message));
+ EXPECT_EQ(program->NumParameterBlocks(), 0);
+ EXPECT_EQ(program->NumResidualBlocks(), 0);
+ EXPECT_EQ(removed_parameter_blocks.size(), 3);
+ EXPECT_EQ(fixed_cost, 0.0);
+}
+
+TEST(Program, RemoveFixedBlocksOneParameterBlockConstant) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ Program* program = problem.mutable_program();
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ EXPECT_TRUE(program->RemoveFixedBlocks(&removed_parameter_blocks,
+ &fixed_cost,
+ &message));
+ EXPECT_EQ(program->NumParameterBlocks(), 1);
+ EXPECT_EQ(program->NumResidualBlocks(), 1);
+}
+
+TEST(Program, RemoveFixedBlocksNumEliminateBlocks) {
+ ProblemImpl problem;
+ double x;
+ double y;
+ double z;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ Program* program = problem.mutable_program();
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ EXPECT_TRUE(program->RemoveFixedBlocks(&removed_parameter_blocks,
+ &fixed_cost,
+ &message));
+ EXPECT_EQ(program->NumParameterBlocks(), 2);
+ EXPECT_EQ(program->NumResidualBlocks(), 2);
+}
+
+TEST(Program, RemoveFixedBlocksFixedCost) {
+ ProblemImpl problem;
+ double x = 1.23;
+ double y = 4.56;
+ double z = 7.89;
+
+ problem.AddParameterBlock(&x, 1);
+ problem.AddParameterBlock(&y, 1);
+ problem.AddParameterBlock(&z, 1);
+ problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x);
+ problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z);
+ problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
+ problem.SetParameterBlockConstant(&x);
+
+ Program* program = problem.mutable_program();
+
+ double expected_fixed_cost;
+ ResidualBlock *expected_removed_block = program->residual_blocks()[0];
+ scoped_array<double> scratch(
+ new double[expected_removed_block->NumScratchDoublesForEvaluate()]);
+ expected_removed_block->Evaluate(true,
+ &expected_fixed_cost,
+ NULL,
+ NULL,
+ scratch.get());
+
+ vector<double*> removed_parameter_blocks;
+ double fixed_cost = 0.0;
+ string message;
+ EXPECT_TRUE(program->RemoveFixedBlocks(&removed_parameter_blocks,
+ &fixed_cost,
+ &message));
+
+ EXPECT_EQ(program->NumParameterBlocks(), 2);
+ EXPECT_EQ(program->NumResidualBlocks(), 2);
+ EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost);
+}
+
+TEST(Program, CreateJacobianBlockSparsityTranspose) {
+ ProblemImpl problem;
+ double x[2];
+ double y[3];
+ double z;
+
+ problem.AddParameterBlock(x, 2);
+ problem.AddParameterBlock(y, 3);
+ problem.AddParameterBlock(&z, 1);
+
+ problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
+ problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
+ problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
+ problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
+ problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
+ problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
+
+ TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
+ {
+ int* rows = expected_block_sparse_jacobian.mutable_rows();
+ int* cols = expected_block_sparse_jacobian.mutable_cols();
+ double* values = expected_block_sparse_jacobian.mutable_values();
+ rows[0] = 0;
+ cols[0] = 0;
+
+ rows[1] = 2;
+ cols[1] = 1;
+ rows[2] = 0;
+ cols[2] = 1;
+
+ rows[3] = 2;
+ cols[3] = 2;
+ rows[4] = 1;
+ cols[4] = 2;
+
+ rows[5] = 2;
+ cols[5] = 3;
+ rows[6] = 1;
+ cols[6] = 3;
+
+ rows[7] = 0;
+ cols[7] = 4;
+ rows[8] = 2;
+ cols[8] = 4;
+
+ rows[9] = 2;
+ cols[9] = 5;
+ rows[10] = 1;
+ cols[10] = 5;
+
+ rows[11] = 0;
+ cols[11] = 6;
+ rows[12] = 2;
+ cols[12] = 6;
+
+ rows[13] = 1;
+ cols[13] = 7;
+ fill(values, values + 14, 1.0);
+ expected_block_sparse_jacobian.set_num_nonzeros(14);
+ }
+
+ Program* program = problem.mutable_program();
+ program->SetParameterOffsetsAndIndex();
+
+ scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
+ program->CreateJacobianBlockSparsityTranspose());
+
+ Matrix expected_dense_jacobian;
+ expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
+
+ Matrix actual_dense_jacobian;
+ actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
+ EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
+}
+
+template <int kNumResiduals, int kNumParameterBlocks>
+class NumParameterBlocksCostFunction : public CostFunction {
+ public:
+ NumParameterBlocksCostFunction() {
+ set_num_residuals(kNumResiduals);
+ for (int i = 0; i < kNumParameterBlocks; ++i) {
+ mutable_parameter_block_sizes()->push_back(1);
+ }
+ }
+
+ virtual ~NumParameterBlocksCostFunction() {
+ }
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ return true;
+ }
+};
+
+TEST(Program, ReallocationInCreateJacobianBlockSparsityTranspose) {
+ // CreateJacobianBlockSparsityTranspose starts with a conservative
+ // estimate of the size of the sparsity pattern. This test ensures
+ // that when those estimates are violated, the reallocation/resizing
+ // logic works correctly.
+
+ ProblemImpl problem;
+ double x[20];
+
+ vector<double*> parameter_blocks;
+ for (int i = 0; i < 20; ++i) {
+ problem.AddParameterBlock(x + i, 1);
+ parameter_blocks.push_back(x + i);
+ }
+
+ problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
+ NULL,
+ parameter_blocks);
+
+ TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
+ {
+ int* rows = expected_block_sparse_jacobian.mutable_rows();
+ int* cols = expected_block_sparse_jacobian.mutable_cols();
+ for (int i = 0; i < 20; ++i) {
+ rows[i] = i;
+ cols[i] = 0;
+ }
+
+ double* values = expected_block_sparse_jacobian.mutable_values();
+ fill(values, values + 20, 1.0);
+ expected_block_sparse_jacobian.set_num_nonzeros(20);
+ }
+
+ Program* program = problem.mutable_program();
+ program->SetParameterOffsetsAndIndex();
+
+ scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
+ program->CreateJacobianBlockSparsityTranspose());
+
+ Matrix expected_dense_jacobian;
+ expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
+
+ Matrix actual_dense_jacobian;
+ actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
+ EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
+}
+
+TEST(Program, ProblemHasNanParameterBlocks) {
+ ProblemImpl problem;
+ double x[2];
+ x[0] = 1.0;
+ x[1] = std::numeric_limits<double>::quiet_NaN();
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+ string error;
+ EXPECT_FALSE(problem.program().ParameterBlocksAreFinite(&error));
+ EXPECT_NE(error.find("has at least one invalid value"),
+ string::npos) << error;
+}
+
+TEST(Program, InfeasibleParameterBlock) {
+ ProblemImpl problem;
+ double x[] = {0.0, 0.0};
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+ problem.SetParameterLowerBound(x, 0, 2.0);
+ problem.SetParameterUpperBound(x, 0, 1.0);
+ string error;
+ EXPECT_FALSE(problem.program().IsFeasible(&error));
+ EXPECT_NE(error.find("infeasible bound"), string::npos) << error;
+}
+
+TEST(Program, InfeasibleConstantParameterBlock) {
+ ProblemImpl problem;
+ double x[] = {0.0, 0.0};
+ problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+ problem.SetParameterLowerBound(x, 0, 1.0);
+ problem.SetParameterUpperBound(x, 0, 2.0);
+ problem.SetParameterBlockConstant(x);
+ string error;
+ EXPECT_FALSE(problem.program().IsFeasible(&error));
+ EXPECT_NE(error.find("infeasible value"), string::npos) << error;
+}
+
+} // namespace internal
+} // namespace ceres
diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 101439c..5555135 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -106,28 +106,6 @@
summary->num_residuals_reduced = program.NumResiduals();
}
-bool ParameterBlocksAreFinite(const ProblemImpl* problem,
- string* message) {
- CHECK_NOTNULL(message);
- const Program& program = problem->program();
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
- for (int i = 0; i < parameter_blocks.size(); ++i) {
- const double* array = parameter_blocks[i]->user_state();
- const int size = parameter_blocks[i]->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 LineSearchOptionsAreValid(const Solver::Options& options,
string* message) {
// Validate values for configuration parameters supplied by user.
@@ -205,84 +183,6 @@
return true;
}
-// Returns true if the program has any non-constant parameter blocks
-// which have non-trivial bounds constraints.
-bool IsBoundsConstrained(const Program& program) {
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
- 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;
-}
-
-// Returns false, if the problem has any constant parameter blocks
-// which are not feasible, or any variable parameter blocks which have
-// a lower bound greater than or equal to the upper bound.
-bool ParameterBlocksAreFeasible(const ProblemImpl* problem, string* message) {
- CHECK_NOTNULL(message);
- const Program& program = problem->program();
- const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
- 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;
-}
-
-
} // namespace
void SolverImpl::TrustRegionMinimize(
@@ -293,7 +193,7 @@
LinearSolver* linear_solver,
Solver::Summary* summary) {
Minimizer::Options minimizer_options(options);
- minimizer_options.is_constrained = IsBoundsConstrained(*program);
+ minimizer_options.is_constrained = program->IsBoundsConstrained();
// The optimizer works on contiguous parameter vectors; allocate
// some.
@@ -470,12 +370,12 @@
return;
}
- if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) {
+ if (!original_program->ParameterBlocksAreFinite(&summary->message)) {
LOG(ERROR) << "Terminating: " << summary->message;
return;
}
- if (!ParameterBlocksAreFeasible(problem_impl, &summary->message)) {
+ if (!original_program->IsFeasible(&summary->message)) {
LOG(ERROR) << "Terminating: " << summary->message;
return;
}
@@ -683,7 +583,7 @@
return;
}
- if (IsBoundsConstrained(problem_impl->program())) {
+ if (original_program->IsBoundsConstrained()) {
summary->message = "LINE_SEARCH Minimizer does not support bounds.";
LOG(ERROR) << "Terminating: " << summary->message;
return;
@@ -711,7 +611,7 @@
summary->num_threads_given = original_options.num_threads;
summary->num_threads_used = options.num_threads;
- if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) {
+ if (original_program->ParameterBlocksAreFinite(&summary->message)) {
LOG(ERROR) << "Terminating: " << summary->message;
return;
}
@@ -1464,54 +1364,6 @@
return true;
}
-
-TripletSparseMatrix* SolverImpl::CreateJacobianBlockSparsityTranspose(
- const Program* program) {
-
- // Matrix to store the block sparsity structure of the Jacobian.
- TripletSparseMatrix* tsm =
- new TripletSparseMatrix(program->NumParameterBlocks(),
- program->NumResidualBlocks(),
- 10 * program->NumResidualBlocks());
- int num_nonzeros = 0;
- int* rows = tsm->mutable_rows();
- int* cols = tsm->mutable_cols();
- double* values = tsm->mutable_values();
-
- const vector<ResidualBlock*>& residual_blocks = program->residual_blocks();
- 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();
- }
- CHECK_LT(num_nonzeros, tsm->max_num_nonzeros());
-
- 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;
-}
-
bool SolverImpl::ReorderProgramForSchurTypeLinearSolver(
const LinearSolverType linear_solver_type,
const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type,
@@ -1579,7 +1431,7 @@
program->SetParameterOffsetsAndIndex();
// Compute a block sparse presentation of J'.
scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
+ program->CreateJacobianBlockSparsityTranspose());
SuiteSparse ss;
cholmod_sparse* block_jacobian_transpose =
@@ -1617,7 +1469,7 @@
program->SetParameterOffsetsAndIndex();
// Compute a block sparse presentation of J'.
scoped_ptr<TripletSparseMatrix> tsm_block_jacobian_transpose(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
+ program->CreateJacobianBlockSparsityTranspose());
vector<int> ordering(program->NumParameterBlocks(), 0);
vector<ParameterBlock*>& parameter_blocks =
diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h
index 0085898..e9a3bf5 100644
--- a/internal/ceres/solver_impl.h
+++ b/internal/ceres/solver_impl.h
@@ -159,14 +159,6 @@
static void AlternateLinearSolverForSchurTypeLinearSolver(
Solver::Options* options);
- // Create a TripletSparseMatrix which contains the zero-one
- // structure corresponding to the block sparsity of the transpose of
- // the Jacobian matrix.
- //
- // Caller owns the result.
- static TripletSparseMatrix* CreateJacobianBlockSparsityTranspose(
- const Program* program);
-
// Reorder the parameter blocks in program using the ordering
static bool ApplyUserOrdering(
const ProblemImpl::ParameterMap& parameter_map,
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc
index 1f15628..9e14d7e 100644
--- a/internal/ceres/solver_impl_test.cc
+++ b/internal/ceres/solver_impl_test.cc
@@ -894,209 +894,5 @@
EXPECT_EQ(options.preconditioner_type, JACOBI);
}
-TEST(SolverImpl, CreateJacobianBlockSparsityTranspose) {
- ProblemImpl problem;
- double x[2];
- double y[3];
- double z;
-
- problem.AddParameterBlock(x, 2);
- problem.AddParameterBlock(y, 3);
- problem.AddParameterBlock(&z, 1);
-
- problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 0, 0>(), NULL, x);
- problem.AddResidualBlock(new MockCostFunctionBase<3, 1, 2, 0>(), NULL, &z, x);
- problem.AddResidualBlock(new MockCostFunctionBase<4, 1, 3, 0>(), NULL, &z, y);
- problem.AddResidualBlock(new MockCostFunctionBase<5, 1, 3, 0>(), NULL, &z, y);
- problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 1, 0>(), NULL, x, &z);
- problem.AddResidualBlock(new MockCostFunctionBase<2, 1, 3, 0>(), NULL, &z, y);
- problem.AddResidualBlock(new MockCostFunctionBase<2, 2, 1, 0>(), NULL, x, &z);
- problem.AddResidualBlock(new MockCostFunctionBase<1, 3, 0, 0>(), NULL, y);
-
- TripletSparseMatrix expected_block_sparse_jacobian(3, 8, 14);
- {
- int* rows = expected_block_sparse_jacobian.mutable_rows();
- int* cols = expected_block_sparse_jacobian.mutable_cols();
- double* values = expected_block_sparse_jacobian.mutable_values();
- rows[0] = 0;
- cols[0] = 0;
-
- rows[1] = 2;
- cols[1] = 1;
- rows[2] = 0;
- cols[2] = 1;
-
- rows[3] = 2;
- cols[3] = 2;
- rows[4] = 1;
- cols[4] = 2;
-
- rows[5] = 2;
- cols[5] = 3;
- rows[6] = 1;
- cols[6] = 3;
-
- rows[7] = 0;
- cols[7] = 4;
- rows[8] = 2;
- cols[8] = 4;
-
- rows[9] = 2;
- cols[9] = 5;
- rows[10] = 1;
- cols[10] = 5;
-
- rows[11] = 0;
- cols[11] = 6;
- rows[12] = 2;
- cols[12] = 6;
-
- rows[13] = 1;
- cols[13] = 7;
- fill(values, values + 14, 1.0);
- expected_block_sparse_jacobian.set_num_nonzeros(14);
- }
-
- Program* program = problem.mutable_program();
- program->SetParameterOffsetsAndIndex();
-
- scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
-
- Matrix expected_dense_jacobian;
- expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
-
- Matrix actual_dense_jacobian;
- actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
- EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
-}
-
-template <int kNumResiduals, int kNumParameterBlocks>
-class NumParameterBlocksCostFunction : public CostFunction {
- public:
- NumParameterBlocksCostFunction() {
- set_num_residuals(kNumResiduals);
- for (int i = 0; i < kNumParameterBlocks; ++i) {
- mutable_parameter_block_sizes()->push_back(1);
- }
- }
-
- virtual ~NumParameterBlocksCostFunction() {
- }
-
- virtual bool Evaluate(double const* const* parameters,
- double* residuals,
- double** jacobians) const {
- return true;
- }
-};
-
-TEST(SolverImpl, ReallocationInCreateJacobianBlockSparsityTranspose) {
- // CreateJacobianBlockSparsityTranspose starts with a conservative
- // estimate of the size of the sparsity pattern. This test ensures
- // that when those estimates are violated, the reallocation/resizing
- // logic works correctly.
-
- ProblemImpl problem;
- double x[20];
-
- vector<double*> parameter_blocks;
- for (int i = 0; i < 20; ++i) {
- problem.AddParameterBlock(x + i, 1);
- parameter_blocks.push_back(x + i);
- }
-
- problem.AddResidualBlock(new NumParameterBlocksCostFunction<1, 20>(),
- NULL,
- parameter_blocks);
-
- TripletSparseMatrix expected_block_sparse_jacobian(20, 1, 20);
- {
- int* rows = expected_block_sparse_jacobian.mutable_rows();
- int* cols = expected_block_sparse_jacobian.mutable_cols();
- for (int i = 0; i < 20; ++i) {
- rows[i] = i;
- cols[i] = 0;
- }
-
- double* values = expected_block_sparse_jacobian.mutable_values();
- fill(values, values + 20, 1.0);
- expected_block_sparse_jacobian.set_num_nonzeros(20);
- }
-
- Program* program = problem.mutable_program();
- program->SetParameterOffsetsAndIndex();
-
- scoped_ptr<TripletSparseMatrix> actual_block_sparse_jacobian(
- SolverImpl::CreateJacobianBlockSparsityTranspose(program));
-
- Matrix expected_dense_jacobian;
- expected_block_sparse_jacobian.ToDenseMatrix(&expected_dense_jacobian);
-
- Matrix actual_dense_jacobian;
- actual_block_sparse_jacobian->ToDenseMatrix(&actual_dense_jacobian);
- EXPECT_EQ((expected_dense_jacobian - actual_dense_jacobian).norm(), 0.0);
-}
-
-TEST(SolverImpl, ProblemHasNanParameterBlocks) {
- Problem problem;
- double x[2];
- x[0] = 1.0;
- x[1] = std::numeric_limits<double>::quiet_NaN();
- problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
- Solver::Options options;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
- EXPECT_EQ(summary.termination_type, FAILURE);
- EXPECT_NE(summary.message.find("has at least one invalid value"),
- string::npos)
- << summary.message;
-}
-
-TEST(SolverImpl, BoundsConstrainedProblemWithLineSearchMinimizerDoesNotWork) {
- Problem problem;
- double x[] = {0.0, 0.0};
- problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
- problem.SetParameterUpperBound(x, 0, 1.0);
- Solver::Options options;
- options.minimizer_type = LINE_SEARCH;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
- EXPECT_EQ(summary.termination_type, FAILURE);
- EXPECT_NE(summary.message.find(
- "LINE_SEARCH Minimizer does not support bounds"),
- string::npos)
- << summary.message;
-}
-
-TEST(SolverImpl, InfeasibleParameterBlock) {
- Problem problem;
- double x[] = {0.0, 0.0};
- problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
- problem.SetParameterLowerBound(x, 0, 2.0);
- problem.SetParameterUpperBound(x, 0, 1.0);
- Solver::Options options;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
- EXPECT_EQ(summary.termination_type, FAILURE);
- EXPECT_NE(summary.message.find("infeasible bound"), string::npos)
- << summary.message;
-}
-
-TEST(SolverImpl, InfeasibleConstantParameterBlock) {
- Problem problem;
- double x[] = {0.0, 0.0};
- problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
- problem.SetParameterLowerBound(x, 0, 1.0);
- problem.SetParameterUpperBound(x, 0, 2.0);
- problem.SetParameterBlockConstant(x);
- Solver::Options options;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
- EXPECT_EQ(summary.termination_type, FAILURE);
- EXPECT_NE(summary.message.find("infeasible value"), string::npos)
- << summary.message;
-}
-
} // namespace internal
} // namespace ceres
