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 #include "ceres/program.h" 32 33 #include <map> 34 #include <vector> 35 #include "ceres/casts.h" 36 #include "ceres/compressed_row_sparse_matrix.h" 37 #include "ceres/cost_function.h" 38 #include "ceres/evaluator.h" 39 #include "ceres/internal/port.h" 40 #include "ceres/local_parameterization.h" 41 #include "ceres/loss_function.h" 42 #include "ceres/map_util.h" 43 #include "ceres/parameter_block.h" 44 #include "ceres/problem.h" 45 #include "ceres/residual_block.h" 46 #include "ceres/stl_util.h" 47 48 namespace ceres { 49 namespace internal { 50 51 Program::Program() {} 52 53 Program::Program(const Program& program) 54 : parameter_blocks_(program.parameter_blocks_), 55 residual_blocks_(program.residual_blocks_) { 56 } 57 58 const vector<ParameterBlock*>& Program::parameter_blocks() const { 59 return parameter_blocks_; 60 } 61 62 const vector<ResidualBlock*>& Program::residual_blocks() const { 63 return residual_blocks_; 64 } 65 66 vector<ParameterBlock*>* Program::mutable_parameter_blocks() { 67 return ¶meter_blocks_; 68 } 69 70 vector<ResidualBlock*>* Program::mutable_residual_blocks() { 71 return &residual_blocks_; 72 } 73 74 bool Program::StateVectorToParameterBlocks(const double *state) { 75 for (int i = 0; i < parameter_blocks_.size(); ++i) { 76 if (!parameter_blocks_[i]->IsConstant() && 77 !parameter_blocks_[i]->SetState(state)) { 78 return false; 79 } 80 state += parameter_blocks_[i]->Size(); 81 } 82 return true; 83 } 84 85 void Program::ParameterBlocksToStateVector(double *state) const { 86 for (int i = 0; i < parameter_blocks_.size(); ++i) { 87 parameter_blocks_[i]->GetState(state); 88 state += parameter_blocks_[i]->Size(); 89 } 90 } 91 92 void Program::CopyParameterBlockStateToUserState() { 93 for (int i = 0; i < parameter_blocks_.size(); ++i) { 94 parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state()); 95 } 96 } 97 98 bool Program::SetParameterBlockStatePtrsToUserStatePtrs() { 99 for (int i = 0; i < parameter_blocks_.size(); ++i) { 100 if (!parameter_blocks_[i]->IsConstant() && 101 !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) { 102 return false; 103 } 104 } 105 return true; 106 } 107 108 bool Program::Plus(const double* state, 109 const double* delta, 110 double* state_plus_delta) const { 111 for (int i = 0; i < parameter_blocks_.size(); ++i) { 112 if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { 113 return false; 114 } 115 state += parameter_blocks_[i]->Size(); 116 delta += parameter_blocks_[i]->LocalSize(); 117 state_plus_delta += parameter_blocks_[i]->Size(); 118 } 119 return true; 120 } 121 122 void Program::SetParameterOffsetsAndIndex() { 123 // Set positions for all parameters appearing as arguments to residuals to one 124 // past the end of the parameter block array. 125 for (int i = 0; i < residual_blocks_.size(); ++i) { 126 ResidualBlock* residual_block = residual_blocks_[i]; 127 for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { 128 residual_block->parameter_blocks()[j]->set_index(-1); 129 } 130 } 131 // For parameters that appear in the program, set their position and offset. 132 int state_offset = 0; 133 int delta_offset = 0; 134 for (int i = 0; i < parameter_blocks_.size(); ++i) { 135 parameter_blocks_[i]->set_index(i); 136 parameter_blocks_[i]->set_state_offset(state_offset); 137 parameter_blocks_[i]->set_delta_offset(delta_offset); 138 state_offset += parameter_blocks_[i]->Size(); 139 delta_offset += parameter_blocks_[i]->LocalSize(); 140 } 141 } 142 143 int Program::NumResidualBlocks() const { 144 return residual_blocks_.size(); 145 } 146 147 int Program::NumParameterBlocks() const { 148 return parameter_blocks_.size(); 149 } 150 151 int Program::NumResiduals() const { 152 int num_residuals = 0; 153 for (int i = 0; i < residual_blocks_.size(); ++i) { 154 num_residuals += residual_blocks_[i]->NumResiduals(); 155 } 156 return num_residuals; 157 } 158 159 int Program::NumParameters() const { 160 int num_parameters = 0; 161 for (int i = 0; i < parameter_blocks_.size(); ++i) { 162 num_parameters += parameter_blocks_[i]->Size(); 163 } 164 return num_parameters; 165 } 166 167 int Program::NumEffectiveParameters() const { 168 int num_parameters = 0; 169 for (int i = 0; i < parameter_blocks_.size(); ++i) { 170 num_parameters += parameter_blocks_[i]->LocalSize(); 171 } 172 return num_parameters; 173 } 174 175 int Program::MaxScratchDoublesNeededForEvaluate() const { 176 // Compute the scratch space needed for evaluate. 177 int max_scratch_bytes_for_evaluate = 0; 178 for (int i = 0; i < residual_blocks_.size(); ++i) { 179 max_scratch_bytes_for_evaluate = 180 max(max_scratch_bytes_for_evaluate, 181 residual_blocks_[i]->NumScratchDoublesForEvaluate()); 182 } 183 return max_scratch_bytes_for_evaluate; 184 } 185 186 int Program::MaxDerivativesPerResidualBlock() const { 187 int max_derivatives = 0; 188 for (int i = 0; i < residual_blocks_.size(); ++i) { 189 int derivatives = 0; 190 ResidualBlock* residual_block = residual_blocks_[i]; 191 int num_parameters = residual_block->NumParameterBlocks(); 192 for (int j = 0; j < num_parameters; ++j) { 193 derivatives += residual_block->NumResiduals() * 194 residual_block->parameter_blocks()[j]->LocalSize(); 195 } 196 max_derivatives = max(max_derivatives, derivatives); 197 } 198 return max_derivatives; 199 } 200 201 int Program::MaxParametersPerResidualBlock() const { 202 int max_parameters = 0; 203 for (int i = 0; i < residual_blocks_.size(); ++i) { 204 max_parameters = max(max_parameters, 205 residual_blocks_[i]->NumParameterBlocks()); 206 } 207 return max_parameters; 208 } 209 210 int Program::MaxResidualsPerResidualBlock() const { 211 int max_residuals = 0; 212 for (int i = 0; i < residual_blocks_.size(); ++i) { 213 max_residuals = max(max_residuals, 214 residual_blocks_[i]->NumResiduals()); 215 } 216 return max_residuals; 217 } 218 219 string Program::ToString() const { 220 string ret = "Program dump\n"; 221 ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks()); 222 ret += StringPrintf("Number of parameters: %d\n", NumParameters()); 223 ret += "Parameters:\n"; 224 for (int i = 0; i < parameter_blocks_.size(); ++i) { 225 ret += StringPrintf("%d: %s\n", 226 i, parameter_blocks_[i]->ToString().c_str()); 227 } 228 return ret; 229 } 230 231 } // namespace internal 232 } // namespace ceres 233
