diff options
Diffstat (limited to 'internal/ceres/solver_impl_test.cc')
| -rw-r--r-- | internal/ceres/solver_impl_test.cc | 880 |
1 files changed, 880 insertions, 0 deletions
diff --git a/internal/ceres/solver_impl_test.cc b/internal/ceres/solver_impl_test.cc new file mode 100644 index 0000000..5eb6c66 --- /dev/null +++ b/internal/ceres/solver_impl_test.cc @@ -0,0 +1,880 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "gtest/gtest.h" +#include "ceres/autodiff_cost_function.h" +#include "ceres/linear_solver.h" +#include "ceres/ordered_groups.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/solver_impl.h" +#include "ceres/sized_cost_function.h" + +namespace ceres { +namespace internal { + +// A cost function that sipmply 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 { + // Do nothing. This is never called. + 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(SolverImpl, 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); + + string error; + { + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 0); + ordering.AddElementToGroup(&z, 0); + + Program program(*problem.mutable_program()); + EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, + &ordering, + NULL, + &error)); + EXPECT_EQ(program.NumParameterBlocks(), 3); + EXPECT_EQ(program.NumResidualBlocks(), 3); + EXPECT_EQ(ordering.NumElements(), 3); + } +} + +TEST(SolverImpl, RemoveFixedBlocksAllParameterBlocksConstant) { + ProblemImpl problem; + double x; + + problem.AddParameterBlock(&x, 1); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); + problem.SetParameterBlockConstant(&x); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + + Program program(problem.program()); + string error; + EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, + &ordering, + NULL, + &error)); + EXPECT_EQ(program.NumParameterBlocks(), 0); + EXPECT_EQ(program.NumResidualBlocks(), 0); + EXPECT_EQ(ordering.NumElements(), 0); +} + +TEST(SolverImpl, RemoveFixedBlocksNoResidualBlocks) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 0); + ordering.AddElementToGroup(&z, 0); + + + Program program(problem.program()); + string error; + EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, + &ordering, + NULL, + &error)); + EXPECT_EQ(program.NumParameterBlocks(), 0); + EXPECT_EQ(program.NumResidualBlocks(), 0); + EXPECT_EQ(ordering.NumElements(), 0); +} + +TEST(SolverImpl, RemoveFixedBlocksOneParameterBlockConstant) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 0); + ordering.AddElementToGroup(&z, 0); + + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); + problem.SetParameterBlockConstant(&x); + + + Program program(problem.program()); + string error; + EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, + &ordering, + NULL, + &error)); + EXPECT_EQ(program.NumParameterBlocks(), 1); + EXPECT_EQ(program.NumResidualBlocks(), 1); + EXPECT_EQ(ordering.NumElements(), 1); +} + +TEST(SolverImpl, 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); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 0); + ordering.AddElementToGroup(&z, 1); + + Program program(problem.program()); + string error; + EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, + &ordering, + NULL, + &error)); + EXPECT_EQ(program.NumParameterBlocks(), 2); + EXPECT_EQ(program.NumResidualBlocks(), 2); + EXPECT_EQ(ordering.NumElements(), 2); + EXPECT_EQ(ordering.GroupId(&y), 0); + EXPECT_EQ(ordering.GroupId(&z), 1); +} + +TEST(SolverImpl, 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); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 0); + ordering.AddElementToGroup(&z, 1); + + double fixed_cost = 0.0; + Program program(problem.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(&expected_fixed_cost, NULL, NULL, scratch.get()); + + string error; + EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, + &ordering, + &fixed_cost, + &error)); + EXPECT_EQ(program.NumParameterBlocks(), 2); + EXPECT_EQ(program.NumResidualBlocks(), 2); + EXPECT_EQ(ordering.NumElements(), 2); + EXPECT_EQ(ordering.GroupId(&y), 0); + EXPECT_EQ(ordering.GroupId(&z), 1); + EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost); +} + +TEST(SolverImpl, ReorderResidualBlockNormalFunction) { + 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, &z, &x); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); + + ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + ordering->AddElementToGroup(&x, 0); + ordering->AddElementToGroup(&y, 0); + ordering->AddElementToGroup(&z, 1); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering = ordering; + + const vector<ResidualBlock*>& residual_blocks = + problem.program().residual_blocks(); + + vector<ResidualBlock*> expected_residual_blocks; + + // This is a bit fragile, but it serves the purpose. We know the + // bucketing algorithm that the reordering function uses, so we + // expect the order for residual blocks for each e_block to be + // filled in reverse. + expected_residual_blocks.push_back(residual_blocks[4]); + expected_residual_blocks.push_back(residual_blocks[1]); + expected_residual_blocks.push_back(residual_blocks[0]); + expected_residual_blocks.push_back(residual_blocks[5]); + expected_residual_blocks.push_back(residual_blocks[2]); + expected_residual_blocks.push_back(residual_blocks[3]); + + Program* program = problem.mutable_program(); + program->SetParameterOffsetsAndIndex(); + + string error; + EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks( + 2, + problem.mutable_program(), + &error)); + EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size()); + for (int i = 0; i < expected_residual_blocks.size(); ++i) { + EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]); + } +} + +TEST(SolverImpl, ReorderResidualBlockNormalFunctionWithFixedBlocks) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + // Set one parameter block constant. + problem.SetParameterBlockConstant(&z); + + // Mark residuals for x's row block with "x" for readability. + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); // 0 x + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); // 1 x + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 2 + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 3 + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 4 x + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 5 + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 6 x + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); // 7 + + ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + ordering->AddElementToGroup(&x, 0); + ordering->AddElementToGroup(&z, 0); + ordering->AddElementToGroup(&y, 1); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering = ordering; + + // Create the reduced program. This should remove the fixed block "z", + // marking the index to -1 at the same time. x and y also get indices. + string error; + scoped_ptr<Program> reduced_program( + SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error)); + + const vector<ResidualBlock*>& residual_blocks = + problem.program().residual_blocks(); + + // This is a bit fragile, but it serves the purpose. We know the + // bucketing algorithm that the reordering function uses, so we + // expect the order for residual blocks for each e_block to be + // filled in reverse. + + vector<ResidualBlock*> expected_residual_blocks; + + // Row block for residuals involving "x". These are marked "x" in the block + // of code calling AddResidual() above. + expected_residual_blocks.push_back(residual_blocks[6]); + expected_residual_blocks.push_back(residual_blocks[4]); + expected_residual_blocks.push_back(residual_blocks[1]); + expected_residual_blocks.push_back(residual_blocks[0]); + + // Row block for residuals involving "y". + expected_residual_blocks.push_back(residual_blocks[7]); + expected_residual_blocks.push_back(residual_blocks[5]); + expected_residual_blocks.push_back(residual_blocks[3]); + expected_residual_blocks.push_back(residual_blocks[2]); + + EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks( + 2, + reduced_program.get(), + &error)); + + EXPECT_EQ(reduced_program->residual_blocks().size(), + expected_residual_blocks.size()); + for (int i = 0; i < expected_residual_blocks.size(); ++i) { + EXPECT_EQ(reduced_program->residual_blocks()[i], + expected_residual_blocks[i]); + } +} + +TEST(SolverImpl, AutomaticSchurReorderingRespectsConstantBlocks) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + // Set one parameter block constant. + problem.SetParameterBlockConstant(&z); + + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); + problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); + problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); + + ParameterBlockOrdering* ordering = new ParameterBlockOrdering; + ordering->AddElementToGroup(&x, 0); + ordering->AddElementToGroup(&z, 0); + ordering->AddElementToGroup(&y, 0); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering = ordering; + + string error; + scoped_ptr<Program> reduced_program( + SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error)); + + const vector<ResidualBlock*>& residual_blocks = + reduced_program->residual_blocks(); + const vector<ParameterBlock*>& parameter_blocks = + reduced_program->parameter_blocks(); + + const vector<ResidualBlock*>& original_residual_blocks = + problem.program().residual_blocks(); + + EXPECT_EQ(residual_blocks.size(), 8); + EXPECT_EQ(reduced_program->parameter_blocks().size(), 2); + + // Verify that right parmeter block and the residual blocks have + // been removed. + for (int i = 0; i < 8; ++i) { + EXPECT_NE(residual_blocks[i], original_residual_blocks.back()); + } + for (int i = 0; i < 2; ++i) { + EXPECT_NE(parameter_blocks[i]->mutable_user_state(), &z); + } +} + +TEST(SolverImpl, ApplyUserOrderingOrderingTooSmall) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 1); + + Program program(problem.program()); + string error; + EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem.parameter_map(), + &ordering, + &program, + &error)); +} + +TEST(SolverImpl, ApplyUserOrderingNormal) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering ordering; + ordering.AddElementToGroup(&x, 0); + ordering.AddElementToGroup(&y, 2); + ordering.AddElementToGroup(&z, 1); + + Program* program = problem.mutable_program(); + string error; + + EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem.parameter_map(), + &ordering, + program, + &error)); + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + + EXPECT_EQ(parameter_blocks.size(), 3); + EXPECT_EQ(parameter_blocks[0]->user_state(), &x); + EXPECT_EQ(parameter_blocks[1]->user_state(), &z); + EXPECT_EQ(parameter_blocks[2]->user_state(), &y); +} + +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) +TEST(SolverImpl, CreateLinearSolverNoSuiteSparse) { + Solver::Options options; + options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; + string error; + EXPECT_FALSE(SolverImpl::CreateLinearSolver(&options, &error)); +} +#endif + +TEST(SolverImpl, CreateLinearSolverNegativeMaxNumIterations) { + Solver::Options options; + options.linear_solver_type = DENSE_QR; + options.linear_solver_max_num_iterations = -1; + // CreateLinearSolver assumes a non-empty ordering. + options.linear_solver_ordering = new ParameterBlockOrdering; + string error; + EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), + static_cast<LinearSolver*>(NULL)); +} + +TEST(SolverImpl, CreateLinearSolverNegativeMinNumIterations) { + Solver::Options options; + options.linear_solver_type = DENSE_QR; + options.linear_solver_min_num_iterations = -1; + // CreateLinearSolver assumes a non-empty ordering. + options.linear_solver_ordering = new ParameterBlockOrdering; + string error; + EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), + static_cast<LinearSolver*>(NULL)); +} + +TEST(SolverImpl, CreateLinearSolverMaxLessThanMinIterations) { + Solver::Options options; + options.linear_solver_type = DENSE_QR; + options.linear_solver_min_num_iterations = 10; + options.linear_solver_max_num_iterations = 5; + options.linear_solver_ordering = new ParameterBlockOrdering; + string error; + EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), + static_cast<LinearSolver*>(NULL)); +} + +TEST(SolverImpl, CreateLinearSolverDenseSchurMultipleThreads) { + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.num_linear_solver_threads = 2; + // The Schur type solvers can only be created with the Ordering + // contains at least one elimination group. + options.linear_solver_ordering = new ParameterBlockOrdering; + double x; + double y; + options.linear_solver_ordering->AddElementToGroup(&x, 0); + options.linear_solver_ordering->AddElementToGroup(&y, 0); + + string error; + scoped_ptr<LinearSolver> solver( + SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_TRUE(solver != NULL); + EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR); + EXPECT_EQ(options.num_linear_solver_threads, 1); +} + +TEST(SolverImpl, CreateIterativeLinearSolverForDogleg) { + Solver::Options options; + options.trust_region_strategy_type = DOGLEG; + // CreateLinearSolver assumes a non-empty ordering. + options.linear_solver_ordering = new ParameterBlockOrdering; + string error; + options.linear_solver_type = ITERATIVE_SCHUR; + EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), + static_cast<LinearSolver*>(NULL)); + + options.linear_solver_type = CGNR; + EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), + static_cast<LinearSolver*>(NULL)); +} + +TEST(SolverImpl, CreateLinearSolverNormalOperation) { + Solver::Options options; + scoped_ptr<LinearSolver> solver; + options.linear_solver_type = DENSE_QR; + // CreateLinearSolver assumes a non-empty ordering. + options.linear_solver_ordering = new ParameterBlockOrdering; + string error; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_EQ(options.linear_solver_type, DENSE_QR); + EXPECT_TRUE(solver.get() != NULL); + + options.linear_solver_type = DENSE_NORMAL_CHOLESKY; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY); + EXPECT_TRUE(solver.get() != NULL); + +#ifndef CERES_NO_SUITESPARSE + options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; + options.sparse_linear_algebra_library = SUITE_SPARSE; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY); + EXPECT_TRUE(solver.get() != NULL); +#endif + +#ifndef CERES_NO_CXSPARSE + options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; + options.sparse_linear_algebra_library = CX_SPARSE; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY); + EXPECT_TRUE(solver.get() != NULL); +#endif + + double x; + double y; + options.linear_solver_ordering->AddElementToGroup(&x, 0); + options.linear_solver_ordering->AddElementToGroup(&y, 0); + + options.linear_solver_type = DENSE_SCHUR; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR); + EXPECT_TRUE(solver.get() != NULL); + + options.linear_solver_type = SPARSE_SCHUR; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + +#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) + EXPECT_TRUE(SolverImpl::CreateLinearSolver(&options, &error) == NULL); +#else + EXPECT_TRUE(solver.get() != NULL); + EXPECT_EQ(options.linear_solver_type, SPARSE_SCHUR); +#endif + + options.linear_solver_type = ITERATIVE_SCHUR; + solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); + EXPECT_EQ(options.linear_solver_type, ITERATIVE_SCHUR); + EXPECT_TRUE(solver.get() != NULL); +} + +struct QuadraticCostFunction { + template <typename T> bool operator()(const T* const x, + T* residual) const { + residual[0] = T(5.0) - *x; + return true; + } +}; + +struct RememberingCallback : public IterationCallback { + explicit RememberingCallback(double *x) : calls(0), x(x) {} + virtual ~RememberingCallback() {} + virtual CallbackReturnType operator()(const IterationSummary& summary) { + x_values.push_back(*x); + return SOLVER_CONTINUE; + } + int calls; + double *x; + vector<double> x_values; +}; + +TEST(SolverImpl, UpdateStateEveryIterationOption) { + double x = 50.0; + const double original_x = x; + + scoped_ptr<CostFunction> cost_function( + new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>( + new QuadraticCostFunction)); + + Problem::Options problem_options; + problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; + ProblemImpl problem(problem_options); + problem.AddResidualBlock(cost_function.get(), NULL, &x); + + Solver::Options options; + options.linear_solver_type = DENSE_QR; + + RememberingCallback callback(&x); + options.callbacks.push_back(&callback); + + Solver::Summary summary; + + int num_iterations; + + // First try: no updating. + SolverImpl::Solve(options, &problem, &summary); + num_iterations = summary.num_successful_steps + + summary.num_unsuccessful_steps; + EXPECT_GT(num_iterations, 1); + for (int i = 0; i < callback.x_values.size(); ++i) { + EXPECT_EQ(50.0, callback.x_values[i]); + } + + // Second try: with updating + x = 50.0; + options.update_state_every_iteration = true; + callback.x_values.clear(); + SolverImpl::Solve(options, &problem, &summary); + num_iterations = summary.num_successful_steps + + summary.num_unsuccessful_steps; + EXPECT_GT(num_iterations, 1); + EXPECT_EQ(original_x, callback.x_values[0]); + EXPECT_NE(original_x, callback.x_values[1]); +} + +// The parameters must be in separate blocks so that they can be individually +// set constant or not. +struct Quadratic4DCostFunction { + template <typename T> bool operator()(const T* const x, + const T* const y, + const T* const z, + const T* const w, + T* residual) const { + // A 4-dimension axis-aligned quadratic. + residual[0] = T(10.0) - *x + + T(20.0) - *y + + T(30.0) - *z + + T(40.0) - *w; + return true; + } +}; + +TEST(SolverImpl, ConstantParameterBlocksDoNotChangeAndStateInvariantKept) { + double x = 50.0; + double y = 50.0; + double z = 50.0; + double w = 50.0; + const double original_x = 50.0; + const double original_y = 50.0; + const double original_z = 50.0; + const double original_w = 50.0; + + scoped_ptr<CostFunction> cost_function( + new AutoDiffCostFunction<Quadratic4DCostFunction, 1, 1, 1, 1, 1>( + new Quadratic4DCostFunction)); + + Problem::Options problem_options; + problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; + + ProblemImpl problem(problem_options); + problem.AddResidualBlock(cost_function.get(), NULL, &x, &y, &z, &w); + problem.SetParameterBlockConstant(&x); + problem.SetParameterBlockConstant(&w); + + Solver::Options options; + options.linear_solver_type = DENSE_QR; + + Solver::Summary summary; + SolverImpl::Solve(options, &problem, &summary); + + // Verify only the non-constant parameters were mutated. + EXPECT_EQ(original_x, x); + EXPECT_NE(original_y, y); + EXPECT_NE(original_z, z); + EXPECT_EQ(original_w, w); + + // Check that the parameter block state pointers are pointing back at the + // user state, instead of inside a random temporary vector made by Solve(). + EXPECT_EQ(&x, problem.program().parameter_blocks()[0]->state()); + EXPECT_EQ(&y, problem.program().parameter_blocks()[1]->state()); + EXPECT_EQ(&z, problem.program().parameter_blocks()[2]->state()); + EXPECT_EQ(&w, problem.program().parameter_blocks()[3]->state()); +} + +#define CHECK_ARRAY(name, value) \ + if (options.return_ ## name) { \ + EXPECT_EQ(summary.name.size(), 1); \ + EXPECT_EQ(summary.name[0], value); \ + } else { \ + EXPECT_EQ(summary.name.size(), 0); \ + } + +#define CHECK_JACOBIAN(name) \ + if (options.return_ ## name) { \ + EXPECT_EQ(summary.name.num_rows, 1); \ + EXPECT_EQ(summary.name.num_cols, 1); \ + EXPECT_EQ(summary.name.cols.size(), 2); \ + EXPECT_EQ(summary.name.cols[0], 0); \ + EXPECT_EQ(summary.name.cols[1], 1); \ + EXPECT_EQ(summary.name.rows.size(), 1); \ + EXPECT_EQ(summary.name.rows[0], 0); \ + EXPECT_EQ(summary.name.values.size(), 0); \ + EXPECT_EQ(summary.name.values[0], name); \ + } else { \ + EXPECT_EQ(summary.name.num_rows, 0); \ + EXPECT_EQ(summary.name.num_cols, 0); \ + EXPECT_EQ(summary.name.cols.size(), 0); \ + EXPECT_EQ(summary.name.rows.size(), 0); \ + EXPECT_EQ(summary.name.values.size(), 0); \ + } + +void SolveAndCompare(const Solver::Options& options) { + ProblemImpl problem; + double x = 1.0; + + const double initial_residual = 5.0 - x; + const double initial_jacobian = -1.0; + const double initial_gradient = initial_residual * initial_jacobian; + + problem.AddResidualBlock( + new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>( + new QuadraticCostFunction), + NULL, + &x); + Solver::Summary summary; + SolverImpl::Solve(options, &problem, &summary); + + const double final_residual = 5.0 - x; + const double final_jacobian = -1.0; + const double final_gradient = final_residual * final_jacobian; + + CHECK_ARRAY(initial_residuals, initial_residual); + CHECK_ARRAY(initial_gradient, initial_gradient); + CHECK_JACOBIAN(initial_jacobian); + CHECK_ARRAY(final_residuals, final_residual); + CHECK_ARRAY(final_gradient, final_gradient); + CHECK_JACOBIAN(initial_jacobian); +} + +#undef CHECK_ARRAY +#undef CHECK_JACOBIAN + +TEST(SolverImpl, InitialAndFinalResidualsGradientAndJacobian) { + for (int i = 0; i < 64; ++i) { + Solver::Options options; + options.return_initial_residuals = (i & 1); + options.return_initial_gradient = (i & 2); + options.return_initial_jacobian = (i & 4); + options.return_final_residuals = (i & 8); + options.return_final_gradient = (i & 16); + options.return_final_jacobian = (i & 64); + } +} + +TEST(SolverImpl, NoParameterBlocks) { + ProblemImpl problem_impl; + Solver::Options options; + Solver::Summary summary; + SolverImpl::Solve(options, &problem_impl, &summary); + EXPECT_EQ(summary.termination_type, DID_NOT_RUN); + EXPECT_EQ(summary.error, "Problem contains no parameter blocks."); +} + +TEST(SolverImpl, NoResiduals) { + ProblemImpl problem_impl; + Solver::Options options; + Solver::Summary summary; + double x = 1; + problem_impl.AddParameterBlock(&x, 1); + SolverImpl::Solve(options, &problem_impl, &summary); + EXPECT_EQ(summary.termination_type, DID_NOT_RUN); + EXPECT_EQ(summary.error, "Problem contains no residual blocks."); +} + +class FailingCostFunction : public SizedCostFunction<1, 1> { + public: + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + return false; + } +}; + +TEST(SolverImpl, InitialCostEvaluationFails) { + ProblemImpl problem_impl; + Solver::Options options; + Solver::Summary summary; + double x; + problem_impl.AddResidualBlock(new FailingCostFunction, NULL, &x); + SolverImpl::Solve(options, &problem_impl, &summary); + EXPECT_EQ(summary.termination_type, NUMERICAL_FAILURE); + EXPECT_EQ(summary.error, "Unable to evaluate the initial cost."); +} + +TEST(SolverImpl, ProblemIsConstant) { + ProblemImpl problem_impl; + Solver::Options options; + Solver::Summary summary; + double x = 1; + problem_impl.AddResidualBlock(new UnaryIdentityCostFunction, NULL, &x); + problem_impl.SetParameterBlockConstant(&x); + SolverImpl::Solve(options, &problem_impl, &summary); + EXPECT_EQ(summary.termination_type, FUNCTION_TOLERANCE); + EXPECT_EQ(summary.initial_cost, 1.0 / 2.0); + EXPECT_EQ(summary.final_cost, 1.0 / 2.0); +} + +} // namespace internal +} // namespace ceres |