← Back to branch summary

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

« back to all changes in this revision

Viewing changes to internal/ceres/parameter_block.h

  • 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/parameter_block.h
 
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
#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_
 
32
#define CERES_INTERNAL_PARAMETER_BLOCK_H_
 
33
 
 
34
#include <cstdlib>
 
35
#include <glog/logging.h>
 
36
#include "ceres/array_utils.h"
 
37
#include "ceres/integral_types.h"
 
38
#include "ceres/internal/eigen.h"
 
39
#include "ceres/internal/port.h"
 
40
#include "ceres/internal/scoped_ptr.h"
 
41
#include "ceres/local_parameterization.h"
 
42
 
 
43
namespace ceres {
 
44
namespace internal {
 
45
 
 
46
class ProblemImpl;
 
47
 
 
48
// The parameter block encodes the location of the user's original value, and
 
49
// also the "current state" of the parameter. The evaluator uses whatever is in
 
50
// the current state of the parameter when evaluating. This is inlined since the
 
51
// methods are performance sensitive.
 
52
//
 
53
// The class is not thread-safe, unless only const methods are called. The
 
54
// parameter block may also hold a pointer to a local parameterization; the
 
55
// parameter block does not take ownership of this pointer, so the user is
 
56
// responsible for the proper disposal of the local parameterization.
 
57
class ParameterBlock {
 
58
 public:
 
59
  ParameterBlock(double* user_state, int size) {
 
60
    Init(user_state, size, NULL);
 
61
  }
 
62
  ParameterBlock(double* user_state,
 
63
                 int size,
 
64
                 LocalParameterization* local_parameterization) {
 
65
    Init(user_state, size, local_parameterization);
 
66
  }
 
67
 
 
68
  // The size of the parameter block.
 
69
  int Size() const { return size_; }
 
70
 
 
71
  // Manipulate the parameter state.
 
72
  bool SetState(const double* x) {
 
73
    CHECK(x != NULL)
 
74
        << "Tried to set the state of constant parameter "
 
75
        << "with user location " << user_state_;
 
76
    CHECK(!is_constant_)
 
77
        << "Tried to set the state of constant parameter "
 
78
        << "with user location " << user_state_;
 
79
 
 
80
    state_ = x;
 
81
    return UpdateLocalParameterizationJacobian();
 
82
  }
 
83
 
 
84
  // Copy the current parameter state out to x. This is "GetState()" rather than
 
85
  // simply "state()" since it is actively copying the data into the passed
 
86
  // pointer.
 
87
  void GetState(double *x) const {
 
88
    if (x != state_) {
 
89
      memcpy(x, state_, sizeof(*state_) * size_);
 
90
    }
 
91
  }
 
92
 
 
93
  // Direct pointers to the current state.
 
94
  const double* state() const { return state_; }
 
95
  const double* user_state() const { return user_state_; }
 
96
  double* mutable_user_state() { return user_state_; }
 
97
  LocalParameterization* local_parameterization() const {
 
98
    return local_parameterization_;
 
99
  }
 
100
  LocalParameterization* mutable_local_parameterization() {
 
101
    return local_parameterization_;
 
102
  }
 
103
 
 
104
  // Set this parameter block to vary or not.
 
105
  void SetConstant() { is_constant_ = true; }
 
106
  void SetVarying() { is_constant_ = false; }
 
107
  bool IsConstant() const { return is_constant_; }
 
108
 
 
109
  // This parameter block's index in an array.
 
110
  int index() const { return index_; }
 
111
  void set_index(int index) { index_ = index; }
 
112
 
 
113
  // This parameter offset inside a larger state vector.
 
114
  int state_offset() const { return state_offset_; }
 
115
  void set_state_offset(int state_offset) { state_offset_ = state_offset; }
 
116
 
 
117
  // This parameter offset inside a larger delta vector.
 
118
  int delta_offset() const { return delta_offset_; }
 
119
  void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; }
 
120
 
 
121
  // Methods relating to the parameter block's parameterization.
 
122
 
 
123
  // The local to global jacobian. Returns NULL if there is no local
 
124
  // parameterization for this parameter block. The returned matrix is row-major
 
125
  // and has Size() rows and  LocalSize() columns.
 
126
  const double* LocalParameterizationJacobian() const {
 
127
    return local_parameterization_jacobian_.get();
 
128
  }
 
129
 
 
130
  int LocalSize() const {
 
131
    return (local_parameterization_ == NULL)
 
132
        ? size_
 
133
        : local_parameterization_->LocalSize();
 
134
  }
 
135
 
 
136
  // Set the parameterization. The parameterization can be set exactly once;
 
137
  // multiple calls to set the parameterization to different values will crash.
 
138
  // It is an error to pass NULL for the parameterization. The parameter block
 
139
  // does not take ownership of the parameterization.
 
140
  void SetParameterization(LocalParameterization* new_parameterization) {
 
141
    CHECK(new_parameterization != NULL) << "NULL parameterization invalid.";
 
142
    CHECK(new_parameterization->GlobalSize() == size_)
 
143
        << "Invalid parameterization for parameter block. The parameter block "
 
144
        << "has size " << size_ << " while the parameterization has a global "
 
145
        << "size of " << new_parameterization->GlobalSize() << ". Did you "
 
146
        << "accidentally use the wrong parameter block or parameterization?";
 
147
    if (new_parameterization != local_parameterization_) {
 
148
      CHECK(local_parameterization_ == NULL)
 
149
          << "Can't re-set the local parameterization; it leads to "
 
150
          << "ambiguous ownership.";
 
151
      local_parameterization_ = new_parameterization;
 
152
      local_parameterization_jacobian_.reset(
 
153
          new double[local_parameterization_->GlobalSize() *
 
154
                     local_parameterization_->LocalSize()]);
 
155
      CHECK(UpdateLocalParameterizationJacobian())
 
156
          "Local parameterization Jacobian computation failed"
 
157
          "for x: " << ConstVectorRef(state_, Size()).transpose();
 
158
    } else {
 
159
      // Ignore the case that the parameterizations match.
 
160
    }
 
161
  }
 
162
 
 
163
  // Generalization of the addition operation. This is the same as
 
164
  // LocalParameterization::Plus() but uses the parameter's current state
 
165
  // instead of operating on a passed in pointer.
 
166
  bool Plus(const double *x, const double* delta, double* x_plus_delta) {
 
167
    if (local_parameterization_ == NULL) {
 
168
      VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) +
 
169
                                       ConstVectorRef(delta,  size_);
 
170
      return true;
 
171
    }
 
172
    return local_parameterization_->Plus(x, delta, x_plus_delta);
 
173
  }
 
174
 
 
175
 private:
 
176
  void Init(double* user_state,
 
177
            int size,
 
178
            LocalParameterization* local_parameterization) {
 
179
    user_state_ = user_state;
 
180
    size_ = size;
 
181
    is_constant_ = false;
 
182
    state_ = user_state_;
 
183
 
 
184
    local_parameterization_ = NULL;
 
185
    if (local_parameterization != NULL) {
 
186
      SetParameterization(local_parameterization);
 
187
    }
 
188
 
 
189
    index_ = -1;
 
190
    state_offset_ = -1;
 
191
    delta_offset_ = -1;
 
192
  }
 
193
 
 
194
  bool UpdateLocalParameterizationJacobian() {
 
195
    if (local_parameterization_ == NULL) {
 
196
      return true;
 
197
    }
 
198
 
 
199
    // Update the local to global Jacobian. In some cases this is
 
200
    // wasted effort; if this is a bottleneck, we will find a solution
 
201
    // at that time.
 
202
 
 
203
    const int jacobian_size = Size() * LocalSize();
 
204
    InvalidateArray(jacobian_size,
 
205
                    local_parameterization_jacobian_.get());
 
206
    if (!local_parameterization_->ComputeJacobian(
 
207
            state_,
 
208
            local_parameterization_jacobian_.get())) {
 
209
      LOG(WARNING) << "Local parameterization Jacobian computation failed"
 
210
          "for x: " << ConstVectorRef(state_, Size()).transpose();
 
211
      return false;
 
212
    }
 
213
 
 
214
    if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) {
 
215
      LOG(WARNING) << "Local parameterization Jacobian computation returned"
 
216
                   << "an invalid matrix for x: "
 
217
                   << ConstVectorRef(state_, Size()).transpose()
 
218
                   << "\n Jacobian matrix : "
 
219
                   << ConstMatrixRef(local_parameterization_jacobian_.get(),
 
220
                                     Size(),
 
221
                                     LocalSize());
 
222
      return false;
 
223
    }
 
224
    return true;
 
225
  }
 
226
 
 
227
  double* user_state_;
 
228
  int size_;
 
229
  bool is_constant_;
 
230
  LocalParameterization* local_parameterization_;
 
231
 
 
232
  // The "state" of the parameter. These fields are only needed while the
 
233
  // solver is running. While at first glance using mutable is a bad idea, this
 
234
  // ends up simplifying the internals of Ceres enough to justify the potential
 
235
  // pitfalls of using "mutable."
 
236
  mutable const double* state_;
 
237
  mutable scoped_array<double> local_parameterization_jacobian_;
 
238
 
 
239
  // The index of the parameter. This is used by various other parts of Ceres to
 
240
  // permit switching from a ParameterBlock* to an index in another array.
 
241
  int32 index_;
 
242
 
 
243
  // The offset of this parameter block inside a larger state vector.
 
244
  int32 state_offset_;
 
245
 
 
246
  // The offset of this parameter block inside a larger delta vector.
 
247
  int32 delta_offset_;
 
248
 
 
249
  // Necessary so ProblemImpl can clean up the parameterizations.
 
250
  friend class ProblemImpl;
 
251
};
 
252
 
 
253
}  // namespace internal
 
254
}  // namespace ceres
 
255
 
 
256
#endif  // CERES_INTERNAL_PARAMETER_BLOCK_H_