internal/ceres/program.cc

   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 #include "ceres/program.h"
  32
  33 #include <map>
  34 #include <vector>
  35 #include "ceres/array_utils.h"
  36 #include "ceres/casts.h"
  37 #include "ceres/compressed_row_sparse_matrix.h"
  38 #include "ceres/cost_function.h"
  39 #include "ceres/evaluator.h"
  40 #include "ceres/internal/port.h"
  41 #include "ceres/local_parameterization.h"
  42 #include "ceres/loss_function.h"
  43 #include "ceres/map_util.h"
  44 #include "ceres/parameter_block.h"
  45 #include "ceres/problem.h"
  46 #include "ceres/residual_block.h"
  47 #include "ceres/stl_util.h"
  48 #include "ceres/triplet_sparse_matrix.h"
  49
  50 namespace ceres {
  51 namespace internal {
  52
  53 using std::max;
  54 using std::set;
  55 using std::string;
  56 using std::vector;
  57
  58 Program::Program() {}
  59
  60 Program::Program(const Program& program)
  61     : parameter_blocks_(program.parameter_blocks_),
  62       residual_blocks_(program.residual_blocks_) {
  63 }
  64
  65 const vector<ParameterBlock*>& Program::parameter_blocks() const {
  66   return parameter_blocks_;
  67 }
  68
  69 const vector<ResidualBlock*>& Program::residual_blocks() const {
  70   return residual_blocks_;
  71 }
  72
  73 vector<ParameterBlock*>* Program::mutable_parameter_blocks() {
  74   return &parameter_blocks_;
  75 }
  76
  77 vector<ResidualBlock*>* Program::mutable_residual_blocks() {
  78   return &residual_blocks_;
  79 }
  80
  81 bool Program::StateVectorToParameterBlocks(const double *state) {
  82   for (int i = 0; i < parameter_blocks_.size(); ++i) {
  83     if (!parameter_blocks_[i]->IsConstant() &&
  84         !parameter_blocks_[i]->SetState(state)) {
  85       return false;
  86     }
  87     state += parameter_blocks_[i]->Size();
  88   }
  89   return true;
  90 }
  91
  92 void Program::ParameterBlocksToStateVector(double *state) const {
  93   for (int i = 0; i < parameter_blocks_.size(); ++i) {
  94     parameter_blocks_[i]->GetState(state);
  95     state += parameter_blocks_[i]->Size();
  96   }
  97 }
  98
  99 void Program::CopyParameterBlockStateToUserState() {
 100   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 101     parameter_blocks_[i]->GetState(parameter_blocks_[i]->mutable_user_state());
 102   }
 103 }
 104
 105 bool Program::SetParameterBlockStatePtrsToUserStatePtrs() {
 106   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 107     if (!parameter_blocks_[i]->IsConstant() &&
 108         !parameter_blocks_[i]->SetState(parameter_blocks_[i]->user_state())) {
 109       return false;
 110     }
 111   }
 112   return true;
 113 }
 114
 115 bool Program::Plus(const double* state,
 116                    const double* delta,
 117                    double* state_plus_delta) const {
 118   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 119     if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) {
 120       return false;
 121     }
 122     state += parameter_blocks_[i]->Size();
 123     delta += parameter_blocks_[i]->LocalSize();
 124     state_plus_delta += parameter_blocks_[i]->Size();
 125   }
 126   return true;
 127 }
 128
 129 void Program::SetParameterOffsetsAndIndex() {
 130   // Set positions for all parameters appearing as arguments to residuals to one
 131   // past the end of the parameter block array.
 132   for (int i = 0; i < residual_blocks_.size(); ++i) {
 133     ResidualBlock* residual_block = residual_blocks_[i];
 134     for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
 135       residual_block->parameter_blocks()[j]->set_index(-1);
 136     }
 137   }
 138   // For parameters that appear in the program, set their position and offset.
 139   int state_offset = 0;
 140   int delta_offset = 0;
 141   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 142     parameter_blocks_[i]->set_index(i);
 143     parameter_blocks_[i]->set_state_offset(state_offset);
 144     parameter_blocks_[i]->set_delta_offset(delta_offset);
 145     state_offset += parameter_blocks_[i]->Size();
 146     delta_offset += parameter_blocks_[i]->LocalSize();
 147   }
 148 }
 149
 150 bool Program::IsValid() const {
 151   for (int i = 0; i < residual_blocks_.size(); ++i) {
 152     const ResidualBlock* residual_block = residual_blocks_[i];
 153     if (residual_block->index() != i) {
 154       LOG(WARNING) << "Residual block: " << i
 155                    << " has incorrect index: " << residual_block->index();
 156       return false;
 157     }
 158   }
 159
 160   int state_offset = 0;
 161   int delta_offset = 0;
 162   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 163     const ParameterBlock* parameter_block = parameter_blocks_[i];
 164     if (parameter_block->index() != i ||
 165         parameter_block->state_offset() != state_offset ||
 166         parameter_block->delta_offset() != delta_offset) {
 167       LOG(WARNING) << "Parameter block: " << i
 168                    << "has incorrect indexing information: "
 169                    << parameter_block->ToString();
 170       return false;
 171     }
 172
 173     state_offset += parameter_blocks_[i]->Size();
 174     delta_offset += parameter_blocks_[i]->LocalSize();
 175   }
 176
 177   return true;
 178 }
 179
 180 bool Program::ParameterBlocksAreFinite(string* message) const {
 181   CHECK_NOTNULL(message);
 182   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 183     const ParameterBlock* parameter_block = parameter_blocks_[i];
 184     const double* array = parameter_block->user_state();
 185     const int size = parameter_block->Size();
 186     const int invalid_index = FindInvalidValue(size, array);
 187     if (invalid_index != size) {
 188       *message = StringPrintf(
 189           "ParameterBlock: %p with size %d has at least one invalid value.\n"
 190           "First invalid value is at index: %d.\n"
 191           "Parameter block values: ",
 192           array, size, invalid_index);
 193       AppendArrayToString(size, array, message);
 194       return false;
 195     }
 196   }
 197   return true;
 198 }
 199
 200 bool Program::IsBoundsConstrained() const {
 201   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 202     const ParameterBlock* parameter_block = parameter_blocks_[i];
 203     if (parameter_block->IsConstant()) {
 204       continue;
 205     }
 206     const int size = parameter_block->Size();
 207     for (int j = 0; j < size; ++j) {
 208       const double lower_bound = parameter_block->LowerBoundForParameter(j);
 209       const double upper_bound = parameter_block->UpperBoundForParameter(j);
 210       if (lower_bound > -std::numeric_limits<double>::max() ||
 211           upper_bound < std::numeric_limits<double>::max()) {
 212         return true;
 213       }
 214     }
 215   }
 216   return false;
 217 }
 218
 219 bool Program::IsFeasible(string* message) const {
 220   CHECK_NOTNULL(message);
 221   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 222     const ParameterBlock* parameter_block = parameter_blocks_[i];
 223     const double* parameters = parameter_block->user_state();
 224     const int size = parameter_block->Size();
 225     if (parameter_block->IsConstant()) {
 226       // Constant parameter blocks must start in the feasible region
 227       // to ultimately produce a feasible solution, since Ceres cannot
 228       // change them.
 229       for (int j = 0; j < size; ++j) {
 230         const double lower_bound = parameter_block->LowerBoundForParameter(j);
 231         const double upper_bound = parameter_block->UpperBoundForParameter(j);
 232         if (parameters[j] < lower_bound || parameters[j] > upper_bound) {
 233           *message = StringPrintf(
 234               "ParameterBlock: %p with size %d has at least one infeasible "
 235               "value."
 236               "\nFirst infeasible value is at index: %d."
 237               "\nLower bound: %e, value: %e, upper bound: %e"
 238               "\nParameter block values: ",
 239               parameters, size, j, lower_bound, parameters[j], upper_bound);
 240           AppendArrayToString(size, parameters, message);
 241           return false;
 242         }
 243       }
 244     } else {
 245       // Variable parameter blocks must have non-empty feasible
 246       // regions, otherwise there is no way to produce a feasible
 247       // solution.
 248       for (int j = 0; j < size; ++j) {
 249         const double lower_bound = parameter_block->LowerBoundForParameter(j);
 250         const double upper_bound = parameter_block->UpperBoundForParameter(j);
 251         if (lower_bound >= upper_bound) {
 252           *message = StringPrintf(
 253               "ParameterBlock: %p with size %d has at least one infeasible "
 254               "bound."
 255               "\nFirst infeasible bound is at index: %d."
 256               "\nLower bound: %e, upper bound: %e"
 257               "\nParameter block values: ",
 258               parameters, size, j, lower_bound, upper_bound);
 259           AppendArrayToString(size, parameters, message);
 260           return false;
 261         }
 262       }
 263     }
 264   }
 265
 266   return true;
 267 }
 268
 269 Program* Program::CreateReducedProgram(
 270     vector<double*>* removed_parameter_blocks,
 271     double* fixed_cost,
 272     string* error) const {
 273   CHECK_NOTNULL(removed_parameter_blocks);
 274   CHECK_NOTNULL(fixed_cost);
 275   CHECK_NOTNULL(error);
 276
 277   scoped_ptr<Program> reduced_program(new Program(*this));
 278   if (!reduced_program->RemoveFixedBlocks(removed_parameter_blocks,
 279                                           fixed_cost,
 280                                           error)) {
 281     return NULL;
 282   }
 283
 284   reduced_program->SetParameterOffsetsAndIndex();
 285   return reduced_program.release();
 286 }
 287
 288 bool Program::RemoveFixedBlocks(vector<double*>* removed_parameter_blocks,
 289                                 double* fixed_cost,
 290                                 string* error) {
 291   CHECK_NOTNULL(removed_parameter_blocks);
 292   CHECK_NOTNULL(fixed_cost);
 293   CHECK_NOTNULL(error);
 294
 295   scoped_array<double> residual_block_evaluate_scratch;
 296   residual_block_evaluate_scratch.reset(
 297       new double[MaxScratchDoublesNeededForEvaluate()]);
 298   *fixed_cost = 0.0;
 299
 300   // Mark all the parameters as unused. Abuse the index member of the
 301   // parameter blocks for the marking.
 302   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 303     parameter_blocks_[i]->set_index(-1);
 304   }
 305
 306   // Filter out residual that have all-constant parameters, and mark
 307   // all the parameter blocks that appear in residuals.
 308   int num_active_residual_blocks = 0;
 309   for (int i = 0; i < residual_blocks_.size(); ++i) {
 310     ResidualBlock* residual_block = residual_blocks_[i];
 311     int num_parameter_blocks = residual_block->NumParameterBlocks();
 312
 313     // Determine if the residual block is fixed, and also mark varying
 314     // parameters that appear in the residual block.
 315     bool all_constant = true;
 316     for (int k = 0; k < num_parameter_blocks; k++) {
 317       ParameterBlock* parameter_block = residual_block->parameter_blocks()[k];
 318       if (!parameter_block->IsConstant()) {
 319         all_constant = false;
 320         parameter_block->set_index(1);
 321       }
 322     }
 323
 324     if (!all_constant) {
 325       residual_blocks_[num_active_residual_blocks++] = residual_block;
 326       continue;
 327     }
 328
 329     // The residual is constant and will be removed, so its cost is
 330     // added to the variable fixed_cost.
 331     double cost = 0.0;
 332     if (!residual_block->Evaluate(true,
 333                                   &cost,
 334                                   NULL,
 335                                   NULL,
 336                                   residual_block_evaluate_scratch.get())) {
 337       *error = StringPrintf("Evaluation of the residual %d failed during "
 338                             "removal of fixed residual blocks.", i);
 339       return false;
 340     }
 341     *fixed_cost += cost;
 342   }
 343   residual_blocks_.resize(num_active_residual_blocks);
 344
 345   // Filter out unused or fixed parameter blocks.
 346   int num_active_parameter_blocks = 0;
 347   removed_parameter_blocks->clear();
 348   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 349     ParameterBlock* parameter_block = parameter_blocks_[i];
 350     if (parameter_block->index() == -1) {
 351       removed_parameter_blocks->push_back(
 352           parameter_block->mutable_user_state());
 353     } else {
 354       parameter_blocks_[num_active_parameter_blocks++] = parameter_block;
 355     }
 356   }
 357   parameter_blocks_.resize(num_active_parameter_blocks);
 358
 359   if (!(((NumResidualBlocks() == 0) &&
 360          (NumParameterBlocks() == 0)) ||
 361         ((NumResidualBlocks() != 0) &&
 362          (NumParameterBlocks() != 0)))) {
 363     *error =  "Congratulations, you found a bug in Ceres. Please report it.";
 364     return false;
 365   }
 366
 367   return true;
 368 }
 369
 370 bool Program::IsParameterBlockSetIndependent(
 371     const set<double*>& independent_set) const {
 372   // Loop over each residual block and ensure that no two parameter
 373   // blocks in the same residual block are part of
 374   // parameter_block_ptrs as that would violate the assumption that it
 375   // is an independent set in the Hessian matrix.
 376   for (vector<ResidualBlock*>::const_iterator it = residual_blocks_.begin();
 377        it != residual_blocks_.end();
 378        ++it) {
 379     ParameterBlock* const* parameter_blocks = (*it)->parameter_blocks();
 380     const int num_parameter_blocks = (*it)->NumParameterBlocks();
 381     int count = 0;
 382     for (int i = 0; i < num_parameter_blocks; ++i) {
 383       count += independent_set.count(
 384           parameter_blocks[i]->mutable_user_state());
 385     }
 386     if (count > 1) {
 387       return false;
 388     }
 389   }
 390   return true;
 391 }
 392
 393 TripletSparseMatrix* Program::CreateJacobianBlockSparsityTranspose() const {
 394   // Matrix to store the block sparsity structure of the Jacobian.
 395   TripletSparseMatrix* tsm =
 396       new TripletSparseMatrix(NumParameterBlocks(),
 397                               NumResidualBlocks(),
 398                               10 * NumResidualBlocks());
 399   int num_nonzeros = 0;
 400   int* rows = tsm->mutable_rows();
 401   int* cols = tsm->mutable_cols();
 402   double* values = tsm->mutable_values();
 403
 404   for (int c = 0; c < residual_blocks_.size(); ++c) {
 405     const ResidualBlock* residual_block = residual_blocks_[c];
 406     const int num_parameter_blocks = residual_block->NumParameterBlocks();
 407     ParameterBlock* const* parameter_blocks =
 408         residual_block->parameter_blocks();
 409
 410     for (int j = 0; j < num_parameter_blocks; ++j) {
 411       if (parameter_blocks[j]->IsConstant()) {
 412         continue;
 413       }
 414
 415       // Re-size the matrix if needed.
 416       if (num_nonzeros >= tsm->max_num_nonzeros()) {
 417         tsm->set_num_nonzeros(num_nonzeros);
 418         tsm->Reserve(2 * num_nonzeros);
 419         rows = tsm->mutable_rows();
 420         cols = tsm->mutable_cols();
 421         values = tsm->mutable_values();
 422       }
 423
 424       const int r = parameter_blocks[j]->index();
 425       rows[num_nonzeros] = r;
 426       cols[num_nonzeros] = c;
 427       values[num_nonzeros] = 1.0;
 428       ++num_nonzeros;
 429     }
 430   }
 431
 432   tsm->set_num_nonzeros(num_nonzeros);
 433   return tsm;
 434 }
 435
 436 int Program::NumResidualBlocks() const {
 437   return residual_blocks_.size();
 438 }
 439
 440 int Program::NumParameterBlocks() const {
 441   return parameter_blocks_.size();
 442 }
 443
 444 int Program::NumResiduals() const {
 445   int num_residuals = 0;
 446   for (int i = 0; i < residual_blocks_.size(); ++i) {
 447     num_residuals += residual_blocks_[i]->NumResiduals();
 448   }
 449   return num_residuals;
 450 }
 451
 452 int Program::NumParameters() const {
 453   int num_parameters = 0;
 454   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 455     num_parameters += parameter_blocks_[i]->Size();
 456   }
 457   return num_parameters;
 458 }
 459
 460 int Program::NumEffectiveParameters() const {
 461   int num_parameters = 0;
 462   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 463     num_parameters += parameter_blocks_[i]->LocalSize();
 464   }
 465   return num_parameters;
 466 }
 467
 468 int Program::MaxScratchDoublesNeededForEvaluate() const {
 469   // Compute the scratch space needed for evaluate.
 470   int max_scratch_bytes_for_evaluate = 0;
 471   for (int i = 0; i < residual_blocks_.size(); ++i) {
 472     max_scratch_bytes_for_evaluate =
 473         max(max_scratch_bytes_for_evaluate,
 474             residual_blocks_[i]->NumScratchDoublesForEvaluate());
 475   }
 476   return max_scratch_bytes_for_evaluate;
 477 }
 478
 479 int Program::MaxDerivativesPerResidualBlock() const {
 480   int max_derivatives = 0;
 481   for (int i = 0; i < residual_blocks_.size(); ++i) {
 482     int derivatives = 0;
 483     ResidualBlock* residual_block = residual_blocks_[i];
 484     int num_parameters = residual_block->NumParameterBlocks();
 485     for (int j = 0; j < num_parameters; ++j) {
 486       derivatives += residual_block->NumResiduals() *
 487                      residual_block->parameter_blocks()[j]->LocalSize();
 488     }
 489     max_derivatives = max(max_derivatives, derivatives);
 490   }
 491   return max_derivatives;
 492 }
 493
 494 int Program::MaxParametersPerResidualBlock() const {
 495   int max_parameters = 0;
 496   for (int i = 0; i < residual_blocks_.size(); ++i) {
 497     max_parameters = max(max_parameters,
 498                          residual_blocks_[i]->NumParameterBlocks());
 499   }
 500   return max_parameters;
 501 }
 502
 503 int Program::MaxResidualsPerResidualBlock() const {
 504   int max_residuals = 0;
 505   for (int i = 0; i < residual_blocks_.size(); ++i) {
 506     max_residuals = max(max_residuals, residual_blocks_[i]->NumResiduals());
 507   }
 508   return max_residuals;
 509 }
 510
 511 string Program::ToString() const {
 512   string ret = "Program dump\n";
 513   ret += StringPrintf("Number of parameter blocks: %d\n", NumParameterBlocks());
 514   ret += StringPrintf("Number of parameters: %d\n", NumParameters());
 515   ret += "Parameters:\n";
 516   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 517     ret += StringPrintf("%d: %s\n",
 518                         i, parameter_blocks_[i]->ToString().c_str());
 519   }
 520   return ret;
 521 }
 522
 523 }  // namespace internal
 524 }  // namespace ceres