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@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
41namespace ceres {
42namespace internal {
43
44// Templated base class for the CostFunction signatures.
45template <int kNumResiduals, int N0, int N1, int N2>
46class MockCostFunctionBase : public
47SizedCostFunction<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
57class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {};
58class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {};
59class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {};
60
61TEST(_, 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
117TEST(_, 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
139TEST(_, 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