← Back to branch summary

~ubuntu-branches/ubuntu/raring/ceres-solver/raring

« back to all changes in this revision

Viewing changes to internal/ceres/program.cc

  • Committer: Package Import Robot
  • Author(s): Koichi Akabe
  • Date: 2012-06-04 07:15:43 UTC
  • Revision ID: package-import@ubuntu.com-20120604071543-zx6uthupvmtqn3k2
Tags: upstream-1.1.1
Import upstream version 1.1.1

Show diffs side-by-side

added added

removed removed

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