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@google.com (Sameer Agarwal)
30
31#include "ceres/parameter_block_ordering.h"
32
33#include "ceres/graph.h"
34#include "ceres/graph_algorithms.h"
35#include "ceres/internal/scoped_ptr.h"
36#include "ceres/map_util.h"
37#include "ceres/parameter_block.h"
38#include "ceres/program.h"
39#include "ceres/residual_block.h"
40#include "glog/logging.h"
41
42namespace ceres {
43namespace internal {
44
45int ComputeStableSchurOrdering(const Program& program,
46                         vector<ParameterBlock*>* ordering) {
47  CHECK_NOTNULL(ordering)->clear();
48
49  scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program));
50  const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
51  const HashSet<ParameterBlock*>& vertices = graph->vertices();
52  for (int i = 0; i < parameter_blocks.size(); ++i) {
53    if (vertices.count(parameter_blocks[i]) > 0) {
54      ordering->push_back(parameter_blocks[i]);
55    }
56  }
57
58  int independent_set_size = StableIndependentSetOrdering(*graph, ordering);
59
60  // Add the excluded blocks to back of the ordering vector.
61  for (int i = 0; i < parameter_blocks.size(); ++i) {
62    ParameterBlock* parameter_block = parameter_blocks[i];
63    if (parameter_block->IsConstant()) {
64      ordering->push_back(parameter_block);
65    }
66  }
67
68  return independent_set_size;
69}
70
71int ComputeSchurOrdering(const Program& program,
72                         vector<ParameterBlock*>* ordering) {
73  CHECK_NOTNULL(ordering)->clear();
74
75  scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program));
76  int independent_set_size = IndependentSetOrdering(*graph, ordering);
77  const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
78
79  // Add the excluded blocks to back of the ordering vector.
80  for (int i = 0; i < parameter_blocks.size(); ++i) {
81    ParameterBlock* parameter_block = parameter_blocks[i];
82    if (parameter_block->IsConstant()) {
83      ordering->push_back(parameter_block);
84    }
85  }
86
87  return independent_set_size;
88}
89
90void ComputeRecursiveIndependentSetOrdering(const Program& program,
91                                            ParameterBlockOrdering* ordering) {
92  CHECK_NOTNULL(ordering)->Clear();
93  const vector<ParameterBlock*> parameter_blocks = program.parameter_blocks();
94  scoped_ptr<Graph< ParameterBlock*> > graph(CreateHessianGraph(program));
95
96  int num_covered = 0;
97  int round = 0;
98  while (num_covered < parameter_blocks.size()) {
99    vector<ParameterBlock*> independent_set_ordering;
100    const int independent_set_size =
101        IndependentSetOrdering(*graph, &independent_set_ordering);
102    for (int i = 0; i < independent_set_size; ++i) {
103      ParameterBlock* parameter_block = independent_set_ordering[i];
104      ordering->AddElementToGroup(parameter_block->mutable_user_state(), round);
105      graph->RemoveVertex(parameter_block);
106    }
107    num_covered += independent_set_size;
108    ++round;
109  }
110}
111
112Graph<ParameterBlock*>*
113CreateHessianGraph(const Program& program) {
114  Graph<ParameterBlock*>* graph = CHECK_NOTNULL(new Graph<ParameterBlock*>);
115  const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
116  for (int i = 0; i < parameter_blocks.size(); ++i) {
117    ParameterBlock* parameter_block = parameter_blocks[i];
118    if (!parameter_block->IsConstant()) {
119      graph->AddVertex(parameter_block);
120    }
121  }
122
123  const vector<ResidualBlock*>& residual_blocks = program.residual_blocks();
124  for (int i = 0; i < residual_blocks.size(); ++i) {
125    const ResidualBlock* residual_block = residual_blocks[i];
126    const int num_parameter_blocks = residual_block->NumParameterBlocks();
127    ParameterBlock* const* parameter_blocks =
128        residual_block->parameter_blocks();
129    for (int j = 0; j < num_parameter_blocks; ++j) {
130      if (parameter_blocks[j]->IsConstant()) {
131        continue;
132      }
133
134      for (int k = j + 1; k < num_parameter_blocks; ++k) {
135        if (parameter_blocks[k]->IsConstant()) {
136          continue;
137        }
138
139        graph->AddEdge(parameter_blocks[j], parameter_blocks[k]);
140      }
141    }
142  }
143
144  return graph;
145}
146
147}  // namespace internal
148}  // namespace ceres
149