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