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