Home | History | Annotate | Download | only in ceres 
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2014 Google Inc. All rights reserved.
      3 // http://code.google.com/p/ceres-solver/
      4 //
      5 // Redistribution and use in source and binary forms, with or without
      6 // modification, are permitted provided that the following conditions are met:
      7 //
      8 // * Redistributions of source code must retain the above copyright notice,
      9 //   this list of conditions and the following disclaimer.
     10 // * Redistributions in binary form must reproduce the above copyright notice,
     11 //   this list of conditions and the following disclaimer in the documentation
     12 //   and/or other materials provided with the distribution.
     13 // * Neither the name of Google Inc. nor the names of its contributors may be
     14 //   used to endorse or promote products derived from this software without
     15 //   specific prior written permission.
     16 //
     17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
     18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
     19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
     20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
     21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     27 // POSSIBILITY OF SUCH DAMAGE.
     28 //
     29 // Author: sameeragarwal (at) google.com (Sameer Agarwal)
     30 
     31 #include "ceres/reorder_program.h"
     32 
     33 #include "ceres/parameter_block.h"
     34 #include "ceres/problem_impl.h"
     35 #include "ceres/program.h"
     36 #include "ceres/sized_cost_function.h"
     37 #include "ceres/solver.h"
     38 
     39 #include "gtest/gtest.h"
     40 
     41 namespace ceres {
     42 namespace internal {
     43 
     44 // Templated base class for the CostFunction signatures.
     45 template <int kNumResiduals, int N0, int N1, int N2>
     46 class MockCostFunctionBase : public
     47 SizedCostFunction<kNumResiduals, N0, N1, N2> {
     48  public:
     49   virtual bool Evaluate(double const* const* parameters,
     50                         double* residuals,
     51                         double** jacobians) const {
     52     // Do nothing. This is never called.
     53     return true;
     54   }
     55 };
     56 
     57 class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
     58 class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
     59 class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
     60 
     61 TEST(_, ReorderResidualBlockNormalFunction) {
     62   ProblemImpl problem;
     63   double x;
     64   double y;
     65   double z;
     66 
     67   problem.AddParameterBlock(&x, 1);
     68   problem.AddParameterBlock(&y, 1);
     69   problem.AddParameterBlock(&z, 1);
     70 
     71   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x);
     72   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x);
     73   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y);
     74   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z);
     75   problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y);
     76   problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y);
     77 
     78   ParameterBlockOrdering* linear_solver_ordering = new ParameterBlockOrdering;
     79   linear_solver_ordering->AddElementToGroup(&x, 0);
     80   linear_solver_ordering->AddElementToGroup(&y, 0);
     81   linear_solver_ordering->AddElementToGroup(&z, 1);
     82 
     83   Solver::Options options;
     84   options.linear_solver_type = DENSE_SCHUR;
     85   options.linear_solver_ordering.reset(linear_solver_ordering);
     86 
     87   const vector<ResidualBlock*>& residual_blocks =
     88       problem.program().residual_blocks();
     89 
     90   vector<ResidualBlock*> expected_residual_blocks;
     91 
     92   // This is a bit fragile, but it serves the purpose. We know the
     93   // bucketing algorithm that the reordering function uses, so we
     94   // expect the order for residual blocks for each e_block to be
     95   // filled in reverse.
     96   expected_residual_blocks.push_back(residual_blocks[4]);
     97   expected_residual_blocks.push_back(residual_blocks[1]);
     98   expected_residual_blocks.push_back(residual_blocks[0]);
     99   expected_residual_blocks.push_back(residual_blocks[5]);
    100   expected_residual_blocks.push_back(residual_blocks[2]);
    101   expected_residual_blocks.push_back(residual_blocks[3]);
    102 
    103   Program* program = problem.mutable_program();
    104   program->SetParameterOffsetsAndIndex();
    105 
    106   string message;
    107   EXPECT_TRUE(LexicographicallyOrderResidualBlocks(
    108                   2,
    109                   problem.mutable_program(),
    110                   &message));
    111   EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size());
    112   for (int i = 0; i < expected_residual_blocks.size(); ++i) {
    113     EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]);
    114   }
    115 }
    116 
    117 TEST(_, ApplyOrderingOrderingTooSmall) {
    118   ProblemImpl problem;
    119   double x;
    120   double y;
    121   double z;
    122 
    123   problem.AddParameterBlock(&x, 1);
    124   problem.AddParameterBlock(&y, 1);
    125   problem.AddParameterBlock(&z, 1);
    126 
    127   ParameterBlockOrdering linear_solver_ordering;
    128   linear_solver_ordering.AddElementToGroup(&x, 0);
    129   linear_solver_ordering.AddElementToGroup(&y, 1);
    130 
    131   Program program(problem.program());
    132   string message;
    133   EXPECT_FALSE(ApplyOrdering(problem.parameter_map(),
    134                              linear_solver_ordering,
    135                              &program,
    136                              &message));
    137 }
    138 
    139 TEST(_, ApplyOrderingNormal) {
    140   ProblemImpl problem;
    141   double x;
    142   double y;
    143   double z;
    144 
    145   problem.AddParameterBlock(&x, 1);
    146   problem.AddParameterBlock(&y, 1);
    147   problem.AddParameterBlock(&z, 1);
    148 
    149   ParameterBlockOrdering linear_solver_ordering;
    150   linear_solver_ordering.AddElementToGroup(&x, 0);
    151   linear_solver_ordering.AddElementToGroup(&y, 2);
    152   linear_solver_ordering.AddElementToGroup(&z, 1);
    153 
    154   Program* program = problem.mutable_program();
    155   string message;
    156 
    157   EXPECT_TRUE(ApplyOrdering(problem.parameter_map(),
    158                             linear_solver_ordering,
    159                             program,
    160                             &message));
    161   const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
    162 
    163   EXPECT_EQ(parameter_blocks.size(), 3);
    164   EXPECT_EQ(parameter_blocks[0]->user_state(), &x);
    165   EXPECT_EQ(parameter_blocks[1]->user_state(), &z);
    166   EXPECT_EQ(parameter_blocks[2]->user_state(), &y);
    167 }
    168 
    169 }  // namespace internal
    170 }  // namespace ceres
    171