internal/ceres/parameter_block_ordering_test.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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@google.com (Sameer Agarwal)
  30
  31 #include "ceres/parameter_block_ordering.h"
  32
  33 #include <cstddef>
  34 #include <vector>
  35 #include "gtest/gtest.h"
  36 #include "ceres/collections_port.h"
  37 #include "ceres/graph.h"
  38 #include "ceres/problem_impl.h"
  39 #include "ceres/program.h"
  40 #include "ceres/stl_util.h"
  41 #include "ceres/cost_function.h"
  42 #include "ceres/internal/scoped_ptr.h"
  43 #include "ceres/sized_cost_function.h"
  44
  45 namespace ceres {
  46 namespace internal {
  47
  48 using std::vector;
  49
  50 typedef Graph<ParameterBlock*> HessianGraph;
  51 typedef HashSet<ParameterBlock*> VertexSet;
  52
  53 template <int M, int N1 = 0, int N2 = 0, int N3 = 0>
  54 class DummyCostFunction: public SizedCostFunction<M, N1, N2, N3> {
  55   virtual bool Evaluate(double const* const* parameters,
  56                         double* residuals,
  57                         double** jacobians) const {
  58     return true;
  59   }
  60 };
  61
  62 class SchurOrderingTest : public ::testing::Test {
  63  protected :
  64   virtual void SetUp() {
  65     // The explicit calls to AddParameterBlock are necessary because
  66     // the below tests depend on the specific numbering of the
  67     // parameter blocks.
  68     problem_.AddParameterBlock(x_, 3);
  69     problem_.AddParameterBlock(y_, 4);
  70     problem_.AddParameterBlock(z_, 5);
  71     problem_.AddParameterBlock(w_, 6);
  72
  73     problem_.AddResidualBlock(new DummyCostFunction<2, 3>, NULL, x_);
  74     problem_.AddResidualBlock(new DummyCostFunction<6, 5, 4>, NULL, z_, y_);
  75     problem_.AddResidualBlock(new DummyCostFunction<3, 3, 5>, NULL, x_, z_);
  76     problem_.AddResidualBlock(new DummyCostFunction<7, 5, 3>, NULL, z_, x_);
  77     problem_.AddResidualBlock(new DummyCostFunction<1, 5, 3, 6>, NULL,
  78                               z_, x_, w_);
  79   }
  80
  81   ProblemImpl problem_;
  82   double x_[3], y_[4], z_[5], w_[6];
  83 };
  84
  85 TEST_F(SchurOrderingTest, NoFixed) {
  86   const Program& program = problem_.program();
  87   const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
  88   scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
  89
  90   const VertexSet& vertices = graph->vertices();
  91   EXPECT_EQ(vertices.size(), 4);
  92
  93   for (int i = 0; i < 4; ++i) {
  94     EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end());
  95   }
  96
  97   {
  98     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[0]);
  99     EXPECT_EQ(neighbors.size(), 2);
 100     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
 101     EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
 102   }
 103
 104   {
 105     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]);
 106     EXPECT_EQ(neighbors.size(), 1);
 107     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
 108   }
 109
 110   {
 111     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]);
 112     EXPECT_EQ(neighbors.size(), 3);
 113     EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end());
 114     EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end());
 115     EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
 116   }
 117
 118   {
 119     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]);
 120     EXPECT_EQ(neighbors.size(), 2);
 121     EXPECT_TRUE(neighbors.find(parameter_blocks[0]) != neighbors.end());
 122     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
 123   }
 124 }
 125
 126 TEST_F(SchurOrderingTest, AllFixed) {
 127   problem_.SetParameterBlockConstant(x_);
 128   problem_.SetParameterBlockConstant(y_);
 129   problem_.SetParameterBlockConstant(z_);
 130   problem_.SetParameterBlockConstant(w_);
 131
 132   const Program& program = problem_.program();
 133   scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
 134   EXPECT_EQ(graph->vertices().size(), 0);
 135 }
 136
 137 TEST_F(SchurOrderingTest, OneFixed) {
 138   problem_.SetParameterBlockConstant(x_);
 139
 140   const Program& program = problem_.program();
 141   const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
 142   scoped_ptr<HessianGraph> graph(CreateHessianGraph(program));
 143
 144   const VertexSet& vertices = graph->vertices();
 145
 146   EXPECT_EQ(vertices.size(), 3);
 147   EXPECT_TRUE(vertices.find(parameter_blocks[0]) == vertices.end());
 148
 149   for (int i = 1; i < 3; ++i) {
 150     EXPECT_TRUE(vertices.find(parameter_blocks[i]) != vertices.end());
 151   }
 152
 153   {
 154     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[1]);
 155     EXPECT_EQ(neighbors.size(), 1);
 156     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
 157   }
 158
 159   {
 160     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[2]);
 161     EXPECT_EQ(neighbors.size(), 2);
 162     EXPECT_TRUE(neighbors.find(parameter_blocks[1]) != neighbors.end());
 163     EXPECT_TRUE(neighbors.find(parameter_blocks[3]) != neighbors.end());
 164   }
 165
 166   {
 167     const VertexSet& neighbors = graph->Neighbors(parameter_blocks[3]);
 168     EXPECT_EQ(neighbors.size(), 1);
 169     EXPECT_TRUE(neighbors.find(parameter_blocks[2]) != neighbors.end());
 170   }
 171
 172   // The constant parameter block is at the end.
 173   vector<ParameterBlock*> ordering;
 174   ComputeSchurOrdering(program, &ordering);
 175   EXPECT_EQ(ordering.back(), parameter_blocks[0]);
 176 }
 177
 178 }  // namespace internal
 179 }  // namespace ceres