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