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 (at) 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 46 namespace ceres { 47 namespace internal { 48 49 class ProblemImpl; 50 class 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. 61 class 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
