internal/ceres/parameter_block.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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 <algorithm>
  35 #include <cstdlib>
  36 #include <limits>
  37 #include <string>
  38 #include "ceres/array_utils.h"
  39 #include "ceres/collections_port.h"
  40 #include "ceres/integral_types.h"
  41 #include "ceres/internal/eigen.h"
  42 #include "ceres/internal/port.h"
  43 #include "ceres/internal/scoped_ptr.h"
  44 #include "ceres/local_parameterization.h"
  45 #include "ceres/stringprintf.h"
  46 #include "glog/logging.h"
  47
  48 namespace ceres {
  49 namespace internal {
  50
  51 class ProblemImpl;
  52 class ResidualBlock;
  53
  54 // The parameter block encodes the location of the user's original value, and
  55 // also the "current state" of the parameter. The evaluator uses whatever is in
  56 // the current state of the parameter when evaluating. This is inlined since the
  57 // methods are performance sensitive.
  58 //
  59 // The class is not thread-safe, unless only const methods are called. The
  60 // parameter block may also hold a pointer to a local parameterization; the
  61 // parameter block does not take ownership of this pointer, so the user is
  62 // responsible for the proper disposal of the local parameterization.
  63 class ParameterBlock {
  64  public:
  65   // TODO(keir): Decide what data structure is best here. Should this be a set?
  66   // Probably not, because sets are memory inefficient. However, if it's a
  67   // vector, you can get into pathological linear performance when removing a
  68   // residual block from a problem where all the residual blocks depend on one
  69   // parameter; for example, shared focal length in a bundle adjustment
  70   // problem. It might be worth making a custom structure that is just an array
  71   // when it is small, but transitions to a hash set when it has more elements.
  72   //
  73   // For now, use a hash set.
  74   typedef HashSet<ResidualBlock*> ResidualBlockSet;
  75
  76   // Create a parameter block with the user state, size, and index specified.
  77   // The size is the size of the parameter block and the index is the position
  78   // of the parameter block inside a Program (if any).
  79   ParameterBlock(double* user_state, int size, int index) {
  80     Init(user_state, size, index, NULL);
  81   }
  82
  83   ParameterBlock(double* user_state,
  84                  int size,
  85                  int index,
  86                  LocalParameterization* local_parameterization) {
  87     Init(user_state, size, index, local_parameterization);
  88   }
  89
  90   // The size of the parameter block.
  91   int Size() const { return size_; }
  92
  93   // Manipulate the parameter state.
  94   bool SetState(const double* x) {
  95     CHECK(x != NULL)
  96         << "Tried to set the state of constant parameter "
  97         << "with user location " << user_state_;
  98     CHECK(!is_constant_)
  99         << "Tried to set the state of constant parameter "
 100         << "with user location " << user_state_;
 101
 102     state_ = x;
 103     return UpdateLocalParameterizationJacobian();
 104   }
 105
 106   // Copy the current parameter state out to x. This is "GetState()" rather than
 107   // simply "state()" since it is actively copying the data into the passed
 108   // pointer.
 109   void GetState(double *x) const {
 110     if (x != state_) {
 111       memcpy(x, state_, sizeof(*state_) * size_);
 112     }
 113   }
 114
 115   // Direct pointers to the current state.
 116   const double* state() const { return state_; }
 117   const double* user_state() const { return user_state_; }
 118   double* mutable_user_state() { return user_state_; }
 119   LocalParameterization* local_parameterization() const {
 120     return local_parameterization_;
 121   }
 122   LocalParameterization* mutable_local_parameterization() {
 123     return local_parameterization_;
 124   }
 125
 126   // Set this parameter block to vary or not.
 127   void SetConstant() { is_constant_ = true; }
 128   void SetVarying() { is_constant_ = false; }
 129   bool IsConstant() const { return is_constant_; }
 130
 131   // This parameter block's index in an array.
 132   int index() const { return index_; }
 133   void set_index(int index) { index_ = index; }
 134
 135   // This parameter offset inside a larger state vector.
 136   int state_offset() const { return state_offset_; }
 137   void set_state_offset(int state_offset) { state_offset_ = state_offset; }
 138
 139   // This parameter offset inside a larger delta vector.
 140   int delta_offset() const { return delta_offset_; }
 141   void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; }
 142
 143   // Methods relating to the parameter block's parameterization.
 144
 145   // The local to global jacobian. Returns NULL if there is no local
 146   // parameterization for this parameter block. The returned matrix is row-major
 147   // and has Size() rows and  LocalSize() columns.
 148   const double* LocalParameterizationJacobian() const {
 149     return local_parameterization_jacobian_.get();
 150   }
 151
 152   int LocalSize() const {
 153     return (local_parameterization_ == NULL)
 154         ? size_
 155         : local_parameterization_->LocalSize();
 156   }
 157
 158   // Set the parameterization. The parameterization can be set exactly once;
 159   // multiple calls to set the parameterization to different values will crash.
 160   // It is an error to pass NULL for the parameterization. The parameter block
 161   // does not take ownership of the parameterization.
 162   void SetParameterization(LocalParameterization* new_parameterization) {
 163     CHECK(new_parameterization != NULL) << "NULL parameterization invalid.";
 164     // Nothing to do if the new parameterization is the same as the
 165     // old parameterization.
 166     if (new_parameterization == local_parameterization_) {
 167       return;
 168     }
 169
 170     CHECK(local_parameterization_ == NULL)
 171         << "Can't re-set the local parameterization; it leads to "
 172         << "ambiguous ownership. Current local parameterization is: "
 173         << local_parameterization_;
 174
 175     CHECK(new_parameterization->GlobalSize() == size_)
 176         << "Invalid parameterization for parameter block. The parameter block "
 177         << "has size " << size_ << " while the parameterization has a global "
 178         << "size of " << new_parameterization->GlobalSize() << ". Did you "
 179         << "accidentally use the wrong parameter block or parameterization?";
 180
 181     CHECK_GT(new_parameterization->LocalSize(), 0)
 182         << "Invalid parameterization. Parameterizations must have a positive "
 183         << "dimensional tangent space.";
 184
 185     local_parameterization_ = new_parameterization;
 186     local_parameterization_jacobian_.reset(
 187         new double[local_parameterization_->GlobalSize() *
 188                    local_parameterization_->LocalSize()]);
 189     CHECK(UpdateLocalParameterizationJacobian())
 190         << "Local parameterization Jacobian computation failed for x: "
 191         << ConstVectorRef(state_, Size()).transpose();
 192   }
 193
 194   void SetUpperBound(int index, double upper_bound) {
 195     CHECK_LT(index, size_);
 196
 197     if (upper_bounds_.get() == NULL) {
 198       upper_bounds_.reset(new double[size_]);
 199       std::fill(upper_bounds_.get(),
 200                 upper_bounds_.get() + size_,
 201                 std::numeric_limits<double>::max());
 202     }
 203
 204     upper_bounds_[index] = upper_bound;
 205   }
 206
 207   void SetLowerBound(int index, double lower_bound) {
 208     CHECK_LT(index, size_);
 209
 210     if (lower_bounds_.get() == NULL) {
 211       lower_bounds_.reset(new double[size_]);
 212       std::fill(lower_bounds_.get(),
 213                 lower_bounds_.get() + size_,
 214                 -std::numeric_limits<double>::max());
 215     }
 216
 217     lower_bounds_[index] = lower_bound;
 218   }
 219
 220   // Generalization of the addition operation. This is the same as
 221   // LocalParameterization::Plus() followed by projection onto the
 222   // hyper cube implied by the bounds constraints.
 223   bool Plus(const double *x, const double* delta, double* x_plus_delta) {
 224     if (local_parameterization_ != NULL) {
 225       if (!local_parameterization_->Plus(x, delta, x_plus_delta)) {
 226         return false;
 227       }
 228     } else {
 229       VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) +
 230                                        ConstVectorRef(delta,  size_);
 231     }
 232
 233     // Project onto the box constraints.
 234     if (lower_bounds_.get() != NULL) {
 235       for (int i = 0; i < size_; ++i) {
 236         x_plus_delta[i] = std::max(x_plus_delta[i], lower_bounds_[i]);
 237       }
 238     }
 239
 240     if (upper_bounds_.get() != NULL) {
 241       for (int i = 0; i < size_; ++i) {
 242         x_plus_delta[i] = std::min(x_plus_delta[i], upper_bounds_[i]);
 243       }
 244     }
 245
 246     return true;
 247   }
 248
 249   std::string ToString() const {
 250     return StringPrintf("{ this=%p, user_state=%p, state=%p, size=%d, "
 251                         "constant=%d, index=%d, state_offset=%d, "
 252                         "delta_offset=%d }",
 253                         this,
 254                         user_state_,
 255                         state_,
 256                         size_,
 257                         is_constant_,
 258                         index_,
 259                         state_offset_,
 260                         delta_offset_);
 261   }
 262
 263   void EnableResidualBlockDependencies() {
 264     CHECK(residual_blocks_.get() == NULL)
 265         << "Ceres bug: There is already a residual block collection "
 266         << "for parameter block: " << ToString();
 267     residual_blocks_.reset(new ResidualBlockSet);
 268   }
 269
 270   void AddResidualBlock(ResidualBlock* residual_block) {
 271     CHECK(residual_blocks_.get() != NULL)
 272         << "Ceres bug: The residual block collection is null for parameter "
 273         << "block: " << ToString();
 274     residual_blocks_->insert(residual_block);
 275   }
 276
 277   void RemoveResidualBlock(ResidualBlock* residual_block) {
 278     CHECK(residual_blocks_.get() != NULL)
 279         << "Ceres bug: The residual block collection is null for parameter "
 280         << "block: " << ToString();
 281     CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end())
 282         << "Ceres bug: Missing residual for parameter block: " << ToString();
 283     residual_blocks_->erase(residual_block);
 284   }
 285
 286   // This is only intended for iterating; perhaps this should only expose
 287   // .begin() and .end().
 288   ResidualBlockSet* mutable_residual_blocks() {
 289     return residual_blocks_.get();
 290   }
 291
 292   double LowerBoundForParameter(int index) const {
 293     if (lower_bounds_.get() == NULL) {
 294       return -std::numeric_limits<double>::max();
 295     } else {
 296       return lower_bounds_[index];
 297     }
 298   }
 299
 300   double UpperBoundForParameter(int index) const {
 301     if (upper_bounds_.get() == NULL) {
 302       return std::numeric_limits<double>::max();
 303     } else {
 304       return upper_bounds_[index];
 305     }
 306   }
 307
 308  private:
 309   void Init(double* user_state,
 310             int size,
 311             int index,
 312             LocalParameterization* local_parameterization) {
 313     user_state_ = user_state;
 314     size_ = size;
 315     index_ = index;
 316     is_constant_ = false;
 317     state_ = user_state_;
 318
 319     local_parameterization_ = NULL;
 320     if (local_parameterization != NULL) {
 321       SetParameterization(local_parameterization);
 322     }
 323
 324     state_offset_ = -1;
 325     delta_offset_ = -1;
 326   }
 327
 328   bool UpdateLocalParameterizationJacobian() {
 329     if (local_parameterization_ == NULL) {
 330       return true;
 331     }
 332
 333     // Update the local to global Jacobian. In some cases this is
 334     // wasted effort; if this is a bottleneck, we will find a solution
 335     // at that time.
 336
 337     const int jacobian_size = Size() * LocalSize();
 338     InvalidateArray(jacobian_size,
 339                     local_parameterization_jacobian_.get());
 340     if (!local_parameterization_->ComputeJacobian(
 341             state_,
 342             local_parameterization_jacobian_.get())) {
 343       LOG(WARNING) << "Local parameterization Jacobian computation failed"
 344           "for x: " << ConstVectorRef(state_, Size()).transpose();
 345       return false;
 346     }
 347
 348     if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) {
 349       LOG(WARNING) << "Local parameterization Jacobian computation returned"
 350                    << "an invalid matrix for x: "
 351                    << ConstVectorRef(state_, Size()).transpose()
 352                    << "\n Jacobian matrix : "
 353                    << ConstMatrixRef(local_parameterization_jacobian_.get(),
 354                                      Size(),
 355                                      LocalSize());
 356       return false;
 357     }
 358     return true;
 359   }
 360
 361   double* user_state_;
 362   int size_;
 363   bool is_constant_;
 364   LocalParameterization* local_parameterization_;
 365
 366   // The "state" of the parameter. These fields are only needed while the
 367   // solver is running. While at first glance using mutable is a bad idea, this
 368   // ends up simplifying the internals of Ceres enough to justify the potential
 369   // pitfalls of using "mutable."
 370   mutable const double* state_;
 371   mutable scoped_array<double> local_parameterization_jacobian_;
 372
 373   // The index of the parameter. This is used by various other parts of Ceres to
 374   // permit switching from a ParameterBlock* to an index in another array.
 375   int32 index_;
 376
 377   // The offset of this parameter block inside a larger state vector.
 378   int32 state_offset_;
 379
 380   // The offset of this parameter block inside a larger delta vector.
 381   int32 delta_offset_;
 382
 383   // If non-null, contains the residual blocks this parameter block is in.
 384   scoped_ptr<ResidualBlockSet> residual_blocks_;
 385
 386   // Upper and lower bounds for the parameter block.  SetUpperBound
 387   // and SetLowerBound lazily initialize the upper_bounds_ and
 388   // lower_bounds_ arrays. If they are never called, then memory for
 389   // these arrays is never allocated. Thus for problems where there
 390   // are no bounds, or only one sided bounds we do not pay the cost of
 391   // allocating memory for the inactive bounds constraints.
 392   //
 393   // Upon initialization these arrays are initialized to
 394   // std::numeric_limits<double>::max() and
 395   // -std::numeric_limits<double>::max() respectively which correspond
 396   // to the parameter block being unconstrained.
 397   scoped_array<double> upper_bounds_;
 398   scoped_array<double> lower_bounds_;
 399
 400   // Necessary so ProblemImpl can clean up the parameterizations.
 401   friend class ProblemImpl;
 402 };
 403
 404 }  // namespace internal
 405 }  // namespace ceres
 406
 407 #endif  // CERES_INTERNAL_PARAMETER_BLOCK_H_