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: keir@google.com (Keir Mierle)
30
31#include "ceres/program.h"
32
33#include <map>
34#include <vector>
35#include "ceres/casts.h"
36#include "ceres/compressed_row_sparse_matrix.h"
37#include "ceres/cost_function.h"
38#include "ceres/evaluator.h"
39#include "ceres/internal/port.h"
40#include "ceres/local_parameterization.h"
41#include "ceres/loss_function.h"
42#include "ceres/map_util.h"
43#include "ceres/parameter_block.h"
44#include "ceres/problem.h"
45#include "ceres/residual_block.h"
46#include "ceres/stl_util.h"
47
48namespace ceres {
49namespace internal {
50
51Program::Program() {}
52
53Program::Program(const Program& program)
54    : parameter_blocks_(program.parameter_blocks_),
55      residual_blocks_(program.residual_blocks_) {
56}
57
58const vector<ParameterBlock*>& Program::parameter_blocks() const {
59  return parameter_blocks_;
60}
61
62const vector<ResidualBlock*>& Program::residual_blocks() const {
63  return residual_blocks_;
64}
65
66vector<ParameterBlock*>* Program::mutable_parameter_blocks() {
67  return &parameter_blocks_;
68}
69
70vector<ResidualBlock*>* Program::mutable_residual_blocks() {
71  return &residual_blocks_;
72}
73
74bool Program::StateVectorToParameterBlocks(const double *state) {
75  for (int i = 0; i < parameter_blocks_.size(); ++i) {
76    if (!parameter_blocks_[i]->IsConstant() &&
77        !parameter_blocks_[i]->SetState(state)) {
78      return false;
79    }
80    state += parameter_blocks_[i]->Size();
81  }
82  return true;
83}
84
85void Program::ParameterBlocksToStateVector(double *state) const {
86  for (int i = 0; i < parameter_blocks_.size(); ++i) {
87    parameter_blocks_[i]->GetState(state);
88    state += parameter_blocks_[i]->Size();
89  }
90}
91
92void Program::CopyParameterBlockStateToUserState() {
93  for (int i = 0; i < parameter_blocks_.size(); ++i) {
94    parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state());
95  }
96}
97
98bool Program::SetParameterBlockStatePtrsToUserStatePtrs() {
99  for (int i = 0; i < parameter_blocks_.size(); ++i) {
100    if (!parameter_blocks_[i]->IsConstant() &&
101        !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
102      return false;
103    }
104  }
105  return true;
106}
107
108bool Program::Plus(const double* state,
109                   const double* delta,
110                   double* state_plus_delta) const {
111  for (int i = 0; i < parameter_blocks_.size(); ++i) {
112    if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) {
113      return false;
114    }
115    state += parameter_blocks_[i]->Size();
116    delta += parameter_blocks_[i]->LocalSize();
117    state_plus_delta += parameter_blocks_[i]->Size();
118  }
119  return true;
120}
121
122void Program::SetParameterOffsetsAndIndex() {
123  // Set positions for all parameters appearing as arguments to residuals to one
124  // past the end of the parameter block array.
125  for (int i = 0; i < residual_blocks_.size(); ++i) {
126    ResidualBlock* residual_block = residual_blocks_[i];
127    for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
128      residual_block->parameter_blocks()[j]->set_index(-1);
129    }
130  }
131  // For parameters that appear in the program, set their position and offset.
132  int state_offset = 0;
133  int delta_offset = 0;
134  for (int i = 0; i < parameter_blocks_.size(); ++i) {
135    parameter_blocks_[i]->set_index(i);
136    parameter_blocks_[i]->set_state_offset(state_offset);
137    parameter_blocks_[i]->set_delta_offset(delta_offset);
138    state_offset += parameter_blocks_[i]->Size();
139    delta_offset += parameter_blocks_[i]->LocalSize();
140  }
141}
142
143int Program::NumResidualBlocks() const {
144  return residual_blocks_.size();
145}
146
147int Program::NumParameterBlocks() const {
148  return parameter_blocks_.size();
149}
150
151int Program::NumResiduals() const {
152  int num_residuals = 0;
153  for (int i = 0; i < residual_blocks_.size(); ++i) {
154    num_residuals += residual_blocks_[i]->NumResiduals();
155  }
156  return num_residuals;
157}
158
159int Program::NumParameters() const {
160  int num_parameters = 0;
161  for (int i = 0; i < parameter_blocks_.size(); ++i) {
162    num_parameters += parameter_blocks_[i]->Size();
163  }
164  return num_parameters;
165}
166
167int Program::NumEffectiveParameters() const {
168  int num_parameters = 0;
169  for (int i = 0; i < parameter_blocks_.size(); ++i) {
170    num_parameters += parameter_blocks_[i]->LocalSize();
171  }
172  return num_parameters;
173}
174
175int Program::MaxScratchDoublesNeededForEvaluate() const {
176  // Compute the scratch space needed for evaluate.
177  int max_scratch_bytes_for_evaluate = 0;
178  for (int i = 0; i < residual_blocks_.size(); ++i) {
179    max_scratch_bytes_for_evaluate =
180        max(max_scratch_bytes_for_evaluate,
181            residual_blocks_[i]->NumScratchDoublesForEvaluate());
182  }
183  return max_scratch_bytes_for_evaluate;
184}
185
186int Program::MaxDerivativesPerResidualBlock() const {
187  int max_derivatives = 0;
188  for (int i = 0; i < residual_blocks_.size(); ++i) {
189    int derivatives = 0;
190    ResidualBlock* residual_block = residual_blocks_[i];
191    int num_parameters = residual_block->NumParameterBlocks();
192    for (int j = 0; j < num_parameters; ++j) {
193      derivatives += residual_block->NumResiduals() *
194                     residual_block->parameter_blocks()[j]->LocalSize();
195    }
196    max_derivatives = max(max_derivatives, derivatives);
197  }
198  return max_derivatives;
199}
200
201int Program::MaxParametersPerResidualBlock() const {
202  int max_parameters = 0;
203  for (int i = 0; i < residual_blocks_.size(); ++i) {
204    max_parameters = max(max_parameters,
205                         residual_blocks_[i]->NumParameterBlocks());
206  }
207  return max_parameters;
208}
209
210int Program::MaxResidualsPerResidualBlock() const {
211  int max_residuals = 0;
212  for (int i = 0; i < residual_blocks_.size(); ++i) {
213    max_residuals = max(max_residuals,
214                        residual_blocks_[i]->NumResiduals());
215  }
216  return max_residuals;
217}
218
219string Program::ToString() const {
220  string ret = "Program dump\n";
221  ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks());
222  ret += StringPrintf("Number of parameters: %d\n", NumParameters());
223  ret += "Parameters:\n";
224  for (int i = 0; i < parameter_blocks_.size(); ++i) {
225    ret += StringPrintf("%d: %s\n",
226                        i, parameter_blocks_[i]->ToString().c_str());
227  }
228  return ret;
229}
230
231}  // namespace internal
232}  // namespace ceres
233