diff options
Diffstat (limited to 'internal/ceres/reorder_program_test.cc')
-rw-r--r-- | internal/ceres/reorder_program_test.cc | 170 |
1 files changed, 170 insertions, 0 deletions
diff --git a/internal/ceres/reorder_program_test.cc b/internal/ceres/reorder_program_test.cc new file mode 100644 index 0000000..2a0c4eb --- /dev/null +++ b/internal/ceres/reorder_program_test.cc @@ -0,0 +1,170 @@ +// 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/reorder_program.h" + +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" + +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +// 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(_, 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* linear_solver_ordering = new ParameterBlockOrdering; + linear_solver_ordering->AddElementToGroup(&x, 0); + linear_solver_ordering->AddElementToGroup(&y, 0); + linear_solver_ordering->AddElementToGroup(&z, 1); + + Solver::Options options; + options.linear_solver_type = DENSE_SCHUR; + options.linear_solver_ordering.reset(linear_solver_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 message; + EXPECT_TRUE(LexicographicallyOrderResidualBlocks( + 2, + problem.mutable_program(), + &message)); + 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(_, ApplyOrderingOrderingTooSmall) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering linear_solver_ordering; + linear_solver_ordering.AddElementToGroup(&x, 0); + linear_solver_ordering.AddElementToGroup(&y, 1); + + Program program(problem.program()); + string message; + EXPECT_FALSE(ApplyOrdering(problem.parameter_map(), + linear_solver_ordering, + &program, + &message)); +} + +TEST(_, ApplyOrderingNormal) { + ProblemImpl problem; + double x; + double y; + double z; + + problem.AddParameterBlock(&x, 1); + problem.AddParameterBlock(&y, 1); + problem.AddParameterBlock(&z, 1); + + ParameterBlockOrdering linear_solver_ordering; + linear_solver_ordering.AddElementToGroup(&x, 0); + linear_solver_ordering.AddElementToGroup(&y, 2); + linear_solver_ordering.AddElementToGroup(&z, 1); + + Program* program = problem.mutable_program(); + string message; + + EXPECT_TRUE(ApplyOrdering(problem.parameter_map(), + linear_solver_ordering, + program, + &message)); + 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); +} + +} // namespace internal +} // namespace ceres |