1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012, 2013 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#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_
32#define CERES_INTERNAL_PARAMETER_BLOCK_H_
33
34#include <cstdlib>
35#include <string>
36#include "ceres/array_utils.h"
37#include "ceres/collections_port.h"
38#include "ceres/integral_types.h"
39#include "ceres/internal/eigen.h"
40#include "ceres/internal/port.h"
41#include "ceres/internal/scoped_ptr.h"
42#include "ceres/local_parameterization.h"
43#include "ceres/stringprintf.h"
44#include "glog/logging.h"
45
46namespace ceres {
47namespace internal {
48
49class ProblemImpl;
50class ResidualBlock;
51
52// The parameter block encodes the location of the user's original value, and
53// also the "current state" of the parameter. The evaluator uses whatever is in
54// the current state of the parameter when evaluating. This is inlined since the
55// methods are performance sensitive.
56//
57// The class is not thread-safe, unless only const methods are called. The
58// parameter block may also hold a pointer to a local parameterization; the
59// parameter block does not take ownership of this pointer, so the user is
60// responsible for the proper disposal of the local parameterization.
61class ParameterBlock {
62 public:
63  // TODO(keir): Decide what data structure is best here. Should this be a set?
64  // Probably not, because sets are memory inefficient. However, if it's a
65  // vector, you can get into pathological linear performance when removing a
66  // residual block from a problem where all the residual blocks depend on one
67  // parameter; for example, shared focal length in a bundle adjustment
68  // problem. It might be worth making a custom structure that is just an array
69  // when it is small, but transitions to a hash set when it has more elements.
70  //
71  // For now, use a hash set.
72  typedef HashSet<ResidualBlock*> ResidualBlockSet;
73
74  // Create a parameter block with the user state, size, and index specified.
75  // The size is the size of the parameter block and the index is the position
76  // of the parameter block inside a Program (if any).
77  ParameterBlock(double* user_state, int size, int index) {
78    Init(user_state, size, index, NULL);
79  }
80
81  ParameterBlock(double* user_state,
82                 int size,
83                 int index,
84                 LocalParameterization* local_parameterization) {
85    Init(user_state, size, index, local_parameterization);
86  }
87
88  // The size of the parameter block.
89  int Size() const { return size_; }
90
91  // Manipulate the parameter state.
92  bool SetState(const double* x) {
93    CHECK(x != NULL)
94        << "Tried to set the state of constant parameter "
95        << "with user location " << user_state_;
96    CHECK(!is_constant_)
97        << "Tried to set the state of constant parameter "
98        << "with user location " << user_state_;
99
100    state_ = x;
101    return UpdateLocalParameterizationJacobian();
102  }
103
104  // Copy the current parameter state out to x. This is "GetState()" rather than
105  // simply "state()" since it is actively copying the data into the passed
106  // pointer.
107  void GetState(double *x) const {
108    if (x != state_) {
109      memcpy(x, state_, sizeof(*state_) * size_);
110    }
111  }
112
113  // Direct pointers to the current state.
114  const double* state() const { return state_; }
115  const double* user_state() const { return user_state_; }
116  double* mutable_user_state() { return user_state_; }
117  LocalParameterization* local_parameterization() const {
118    return local_parameterization_;
119  }
120  LocalParameterization* mutable_local_parameterization() {
121    return local_parameterization_;
122  }
123
124  // Set this parameter block to vary or not.
125  void SetConstant() { is_constant_ = true; }
126  void SetVarying() { is_constant_ = false; }
127  bool IsConstant() const { return is_constant_; }
128
129  // This parameter block's index in an array.
130  int index() const { return index_; }
131  void set_index(int index) { index_ = index; }
132
133  // This parameter offset inside a larger state vector.
134  int state_offset() const { return state_offset_; }
135  void set_state_offset(int state_offset) { state_offset_ = state_offset; }
136
137  // This parameter offset inside a larger delta vector.
138  int delta_offset() const { return delta_offset_; }
139  void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; }
140
141  // Methods relating to the parameter block's parameterization.
142
143  // The local to global jacobian. Returns NULL if there is no local
144  // parameterization for this parameter block. The returned matrix is row-major
145  // and has Size() rows and  LocalSize() columns.
146  const double* LocalParameterizationJacobian() const {
147    return local_parameterization_jacobian_.get();
148  }
149
150  int LocalSize() const {
151    return (local_parameterization_ == NULL)
152        ? size_
153        : local_parameterization_->LocalSize();
154  }
155
156  // Set the parameterization. The parameterization can be set exactly once;
157  // multiple calls to set the parameterization to different values will crash.
158  // It is an error to pass NULL for the parameterization. The parameter block
159  // does not take ownership of the parameterization.
160  void SetParameterization(LocalParameterization* new_parameterization) {
161    CHECK(new_parameterization != NULL) << "NULL parameterization invalid.";
162    CHECK(new_parameterization->GlobalSize() == size_)
163        << "Invalid parameterization for parameter block. The parameter block "
164        << "has size " << size_ << " while the parameterization has a global "
165        << "size of " << new_parameterization->GlobalSize() << ". Did you "
166        << "accidentally use the wrong parameter block or parameterization?";
167    if (new_parameterization != local_parameterization_) {
168      CHECK(local_parameterization_ == NULL)
169          << "Can't re-set the local parameterization; it leads to "
170          << "ambiguous ownership.";
171      local_parameterization_ = new_parameterization;
172      local_parameterization_jacobian_.reset(
173          new double[local_parameterization_->GlobalSize() *
174                     local_parameterization_->LocalSize()]);
175      CHECK(UpdateLocalParameterizationJacobian())
176          << "Local parameterization Jacobian computation failed for x: "
177          << ConstVectorRef(state_, Size()).transpose();
178    } else {
179      // Ignore the case that the parameterizations match.
180    }
181  }
182
183  // Generalization of the addition operation. This is the same as
184  // LocalParameterization::Plus() but uses the parameter's current state
185  // instead of operating on a passed in pointer.
186  bool Plus(const double *x, const double* delta, double* x_plus_delta) {
187    if (local_parameterization_ == NULL) {
188      VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) +
189                                       ConstVectorRef(delta,  size_);
190      return true;
191    }
192    return local_parameterization_->Plus(x, delta, x_plus_delta);
193  }
194
195  string ToString() const {
196    return StringPrintf("{ user_state=%p, state=%p, size=%d, "
197                        "constant=%d, index=%d, state_offset=%d, "
198                        "delta_offset=%d }",
199                        user_state_,
200                        state_,
201                        size_,
202                        is_constant_,
203                        index_,
204                        state_offset_,
205                        delta_offset_);
206  }
207
208  void EnableResidualBlockDependencies() {
209    CHECK(residual_blocks_.get() == NULL)
210        << "Ceres bug: There is already a residual block collection "
211        << "for parameter block: " << ToString();
212    residual_blocks_.reset(new ResidualBlockSet);
213  }
214
215  void AddResidualBlock(ResidualBlock* residual_block) {
216    CHECK(residual_blocks_.get() != NULL)
217        << "Ceres bug: The residual block collection is null for parameter "
218        << "block: " << ToString();
219    residual_blocks_->insert(residual_block);
220  }
221
222  void RemoveResidualBlock(ResidualBlock* residual_block) {
223    CHECK(residual_blocks_.get() != NULL)
224        << "Ceres bug: The residual block collection is null for parameter "
225        << "block: " << ToString();
226    CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end())
227        << "Ceres bug: Missing residual for parameter block: " << ToString();
228    residual_blocks_->erase(residual_block);
229  }
230
231  // This is only intended for iterating; perhaps this should only expose
232  // .begin() and .end().
233  ResidualBlockSet* mutable_residual_blocks() {
234    return residual_blocks_.get();
235  }
236
237 private:
238  void Init(double* user_state,
239            int size,
240            int index,
241            LocalParameterization* local_parameterization) {
242    user_state_ = user_state;
243    size_ = size;
244    index_ = index;
245    is_constant_ = false;
246    state_ = user_state_;
247
248    local_parameterization_ = NULL;
249    if (local_parameterization != NULL) {
250      SetParameterization(local_parameterization);
251    }
252
253    state_offset_ = -1;
254    delta_offset_ = -1;
255  }
256
257  bool UpdateLocalParameterizationJacobian() {
258    if (local_parameterization_ == NULL) {
259      return true;
260    }
261
262    // Update the local to global Jacobian. In some cases this is
263    // wasted effort; if this is a bottleneck, we will find a solution
264    // at that time.
265
266    const int jacobian_size = Size() * LocalSize();
267    InvalidateArray(jacobian_size,
268                    local_parameterization_jacobian_.get());
269    if (!local_parameterization_->ComputeJacobian(
270            state_,
271            local_parameterization_jacobian_.get())) {
272      LOG(WARNING) << "Local parameterization Jacobian computation failed"
273          "for x: " << ConstVectorRef(state_, Size()).transpose();
274      return false;
275    }
276
277    if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) {
278      LOG(WARNING) << "Local parameterization Jacobian computation returned"
279                   << "an invalid matrix for x: "
280                   << ConstVectorRef(state_, Size()).transpose()
281                   << "\n Jacobian matrix : "
282                   << ConstMatrixRef(local_parameterization_jacobian_.get(),
283                                     Size(),
284                                     LocalSize());
285      return false;
286    }
287    return true;
288  }
289
290  double* user_state_;
291  int size_;
292  bool is_constant_;
293  LocalParameterization* local_parameterization_;
294
295  // The "state" of the parameter. These fields are only needed while the
296  // solver is running. While at first glance using mutable is a bad idea, this
297  // ends up simplifying the internals of Ceres enough to justify the potential
298  // pitfalls of using "mutable."
299  mutable const double* state_;
300  mutable scoped_array<double> local_parameterization_jacobian_;
301
302  // The index of the parameter. This is used by various other parts of Ceres to
303  // permit switching from a ParameterBlock* to an index in another array.
304  int32 index_;
305
306  // The offset of this parameter block inside a larger state vector.
307  int32 state_offset_;
308
309  // The offset of this parameter block inside a larger delta vector.
310  int32 delta_offset_;
311
312  // If non-null, contains the residual blocks this parameter block is in.
313  scoped_ptr<ResidualBlockSet> residual_blocks_;
314
315  // Necessary so ProblemImpl can clean up the parameterizations.
316  friend class ProblemImpl;
317};
318
319}  // namespace internal
320}  // namespace ceres
321
322#endif  // CERES_INTERNAL_PARAMETER_BLOCK_H_
323