1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 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: sameeragarwal@google.com (Sameer Agarwal)
30//         mierle@gmail.com (Keir Mierle)
31
32#include "ceres/problem_impl.h"
33
34#include <algorithm>
35#include <cstddef>
36#include <iterator>
37#include <set>
38#include <string>
39#include <utility>
40#include <vector>
41#include "ceres/casts.h"
42#include "ceres/compressed_row_sparse_matrix.h"
43#include "ceres/cost_function.h"
44#include "ceres/crs_matrix.h"
45#include "ceres/evaluator.h"
46#include "ceres/loss_function.h"
47#include "ceres/map_util.h"
48#include "ceres/parameter_block.h"
49#include "ceres/program.h"
50#include "ceres/residual_block.h"
51#include "ceres/stl_util.h"
52#include "ceres/stringprintf.h"
53#include "glog/logging.h"
54
55namespace ceres {
56namespace internal {
57
58typedef map<double*, internal::ParameterBlock*> ParameterMap;
59
60namespace {
61internal::ParameterBlock* FindParameterBlockOrDie(
62    const ParameterMap& parameter_map,
63    double* parameter_block) {
64  ParameterMap::const_iterator it = parameter_map.find(parameter_block);
65  CHECK(it != parameter_map.end())
66      << "Parameter block not found: " << parameter_block;
67  return it->second;
68}
69
70// Returns true if two regions of memory, a and b, with sizes size_a and size_b
71// respectively, overlap.
72bool RegionsAlias(const double* a, int size_a,
73                  const double* b, int size_b) {
74  return (a < b) ? b < (a + size_a)
75                 : a < (b + size_b);
76}
77
78void CheckForNoAliasing(double* existing_block,
79                        int existing_block_size,
80                        double* new_block,
81                        int new_block_size) {
82  CHECK(!RegionsAlias(existing_block, existing_block_size,
83                      new_block, new_block_size))
84      << "Aliasing detected between existing parameter block at memory "
85      << "location " << existing_block
86      << " and has size " << existing_block_size << " with new parameter "
87      << "block that has memory address " << new_block << " and would have "
88      << "size " << new_block_size << ".";
89}
90
91}  // namespace
92
93ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,
94                                                       int size) {
95  CHECK(values != NULL) << "Null pointer passed to AddParameterBlock "
96                        << "for a parameter with size " << size;
97
98  // Ignore the request if there is a block for the given pointer already.
99  ParameterMap::iterator it = parameter_block_map_.find(values);
100  if (it != parameter_block_map_.end()) {
101    if (!options_.disable_all_safety_checks) {
102      int existing_size = it->second->Size();
103      CHECK(size == existing_size)
104          << "Tried adding a parameter block with the same double pointer, "
105          << values << ", twice, but with different block sizes. Original "
106          << "size was " << existing_size << " but new size is "
107          << size;
108    }
109    return it->second;
110  }
111
112  if (!options_.disable_all_safety_checks) {
113    // Before adding the parameter block, also check that it doesn't alias any
114    // other parameter blocks.
115    if (!parameter_block_map_.empty()) {
116      ParameterMap::iterator lb = parameter_block_map_.lower_bound(values);
117
118      // If lb is not the first block, check the previous block for aliasing.
119      if (lb != parameter_block_map_.begin()) {
120        ParameterMap::iterator previous = lb;
121        --previous;
122        CheckForNoAliasing(previous->first,
123                           previous->second->Size(),
124                           values,
125                           size);
126      }
127
128      // If lb is not off the end, check lb for aliasing.
129      if (lb != parameter_block_map_.end()) {
130        CheckForNoAliasing(lb->first,
131                           lb->second->Size(),
132                           values,
133                           size);
134      }
135    }
136  }
137
138  // Pass the index of the new parameter block as well to keep the index in
139  // sync with the position of the parameter in the program's parameter vector.
140  ParameterBlock* new_parameter_block =
141      new ParameterBlock(values, size, program_->parameter_blocks_.size());
142
143  // For dynamic problems, add the list of dependent residual blocks, which is
144  // empty to start.
145  if (options_.enable_fast_removal) {
146    new_parameter_block->EnableResidualBlockDependencies();
147  }
148  parameter_block_map_[values] = new_parameter_block;
149  program_->parameter_blocks_.push_back(new_parameter_block);
150  return new_parameter_block;
151}
152
153void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) {
154  CHECK_NOTNULL(residual_block);
155  // Perform no check on the validity of residual_block, that is handled in
156  // the public method: RemoveResidualBlock().
157
158  // If needed, remove the parameter dependencies on this residual block.
159  if (options_.enable_fast_removal) {
160    const int num_parameter_blocks_for_residual =
161        residual_block->NumParameterBlocks();
162    for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {
163      residual_block->parameter_blocks()[i]
164          ->RemoveResidualBlock(residual_block);
165    }
166
167    ResidualBlockSet::iterator it = residual_block_set_.find(residual_block);
168    residual_block_set_.erase(it);
169  }
170  DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);
171}
172
173// Deletes the residual block in question, assuming there are no other
174// references to it inside the problem (e.g. by another parameter). Referenced
175// cost and loss functions are tucked away for future deletion, since it is not
176// possible to know whether other parts of the problem depend on them without
177// doing a full scan.
178void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) {
179  // The const casts here are legit, since ResidualBlock holds these
180  // pointers as const pointers but we have ownership of them and
181  // have the right to destroy them when the destructor is called.
182  if (options_.cost_function_ownership == TAKE_OWNERSHIP &&
183      residual_block->cost_function() != NULL) {
184    cost_functions_to_delete_.push_back(
185        const_cast<CostFunction*>(residual_block->cost_function()));
186  }
187  if (options_.loss_function_ownership == TAKE_OWNERSHIP &&
188      residual_block->loss_function() != NULL) {
189    loss_functions_to_delete_.push_back(
190        const_cast<LossFunction*>(residual_block->loss_function()));
191  }
192  delete residual_block;
193}
194
195// Deletes the parameter block in question, assuming there are no other
196// references to it inside the problem (e.g. by any residual blocks).
197// Referenced parameterizations are tucked away for future deletion, since it
198// is not possible to know whether other parts of the problem depend on them
199// without doing a full scan.
200void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) {
201  if (options_.local_parameterization_ownership == TAKE_OWNERSHIP &&
202      parameter_block->local_parameterization() != NULL) {
203    local_parameterizations_to_delete_.push_back(
204        parameter_block->mutable_local_parameterization());
205  }
206  parameter_block_map_.erase(parameter_block->mutable_user_state());
207  delete parameter_block;
208}
209
210ProblemImpl::ProblemImpl() : program_(new internal::Program) {}
211ProblemImpl::ProblemImpl(const Problem::Options& options)
212    : options_(options),
213      program_(new internal::Program) {}
214
215ProblemImpl::~ProblemImpl() {
216  // Collect the unique cost/loss functions and delete the residuals.
217  const int num_residual_blocks = program_->residual_blocks_.size();
218  cost_functions_to_delete_.reserve(num_residual_blocks);
219  loss_functions_to_delete_.reserve(num_residual_blocks);
220  for (int i = 0; i < program_->residual_blocks_.size(); ++i) {
221    DeleteBlock(program_->residual_blocks_[i]);
222  }
223
224  // Collect the unique parameterizations and delete the parameters.
225  for (int i = 0; i < program_->parameter_blocks_.size(); ++i) {
226    DeleteBlock(program_->parameter_blocks_[i]);
227  }
228
229  // Delete the owned cost/loss functions and parameterizations.
230  STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(),
231                                   local_parameterizations_to_delete_.end());
232  STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(),
233                                   cost_functions_to_delete_.end());
234  STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(),
235                                   loss_functions_to_delete_.end());
236}
237
238ResidualBlock* ProblemImpl::AddResidualBlock(
239    CostFunction* cost_function,
240    LossFunction* loss_function,
241    const vector<double*>& parameter_blocks) {
242  CHECK_NOTNULL(cost_function);
243  CHECK_EQ(parameter_blocks.size(),
244           cost_function->parameter_block_sizes().size());
245
246  // Check the sizes match.
247  const vector<int32>& parameter_block_sizes =
248      cost_function->parameter_block_sizes();
249
250  if (!options_.disable_all_safety_checks) {
251    CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size())
252        << "Number of blocks input is different than the number of blocks "
253        << "that the cost function expects.";
254
255    // Check for duplicate parameter blocks.
256    vector<double*> sorted_parameter_blocks(parameter_blocks);
257    sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end());
258    vector<double*>::const_iterator duplicate_items =
259        unique(sorted_parameter_blocks.begin(),
260               sorted_parameter_blocks.end());
261    if (duplicate_items != sorted_parameter_blocks.end()) {
262      string blocks;
263      for (int i = 0; i < parameter_blocks.size(); ++i) {
264        blocks += StringPrintf(" %p ", parameter_blocks[i]);
265      }
266
267      LOG(FATAL) << "Duplicate parameter blocks in a residual parameter "
268                 << "are not allowed. Parameter block pointers: ["
269                 << blocks << "]";
270    }
271  }
272
273  // Add parameter blocks and convert the double*'s to parameter blocks.
274  vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size());
275  for (int i = 0; i < parameter_blocks.size(); ++i) {
276    parameter_block_ptrs[i] =
277        InternalAddParameterBlock(parameter_blocks[i],
278                                  parameter_block_sizes[i]);
279  }
280
281  if (!options_.disable_all_safety_checks) {
282    // Check that the block sizes match the block sizes expected by the
283    // cost_function.
284    for (int i = 0; i < parameter_block_ptrs.size(); ++i) {
285      CHECK_EQ(cost_function->parameter_block_sizes()[i],
286               parameter_block_ptrs[i]->Size())
287          << "The cost function expects parameter block " << i
288          << " of size " << cost_function->parameter_block_sizes()[i]
289          << " but was given a block of size "
290          << parameter_block_ptrs[i]->Size();
291    }
292  }
293
294  ResidualBlock* new_residual_block =
295      new ResidualBlock(cost_function,
296                        loss_function,
297                        parameter_block_ptrs,
298                        program_->residual_blocks_.size());
299
300  // Add dependencies on the residual to the parameter blocks.
301  if (options_.enable_fast_removal) {
302    for (int i = 0; i < parameter_blocks.size(); ++i) {
303      parameter_block_ptrs[i]->AddResidualBlock(new_residual_block);
304    }
305  }
306
307  program_->residual_blocks_.push_back(new_residual_block);
308
309  if (options_.enable_fast_removal) {
310    residual_block_set_.insert(new_residual_block);
311  }
312
313  return new_residual_block;
314}
315
316// Unfortunately, macros don't help much to reduce this code, and var args don't
317// work because of the ambiguous case that there is no loss function.
318ResidualBlock* ProblemImpl::AddResidualBlock(
319    CostFunction* cost_function,
320    LossFunction* loss_function,
321    double* x0) {
322  vector<double*> residual_parameters;
323  residual_parameters.push_back(x0);
324  return AddResidualBlock(cost_function, loss_function, residual_parameters);
325}
326
327ResidualBlock* ProblemImpl::AddResidualBlock(
328    CostFunction* cost_function,
329    LossFunction* loss_function,
330    double* x0, double* x1) {
331  vector<double*> residual_parameters;
332  residual_parameters.push_back(x0);
333  residual_parameters.push_back(x1);
334  return AddResidualBlock(cost_function, loss_function, residual_parameters);
335}
336
337ResidualBlock* ProblemImpl::AddResidualBlock(
338    CostFunction* cost_function,
339    LossFunction* loss_function,
340    double* x0, double* x1, double* x2) {
341  vector<double*> residual_parameters;
342  residual_parameters.push_back(x0);
343  residual_parameters.push_back(x1);
344  residual_parameters.push_back(x2);
345  return AddResidualBlock(cost_function, loss_function, residual_parameters);
346}
347
348ResidualBlock* ProblemImpl::AddResidualBlock(
349    CostFunction* cost_function,
350    LossFunction* loss_function,
351    double* x0, double* x1, double* x2, double* x3) {
352  vector<double*> residual_parameters;
353  residual_parameters.push_back(x0);
354  residual_parameters.push_back(x1);
355  residual_parameters.push_back(x2);
356  residual_parameters.push_back(x3);
357  return AddResidualBlock(cost_function, loss_function, residual_parameters);
358}
359
360ResidualBlock* ProblemImpl::AddResidualBlock(
361    CostFunction* cost_function,
362    LossFunction* loss_function,
363    double* x0, double* x1, double* x2, double* x3, double* x4) {
364  vector<double*> residual_parameters;
365  residual_parameters.push_back(x0);
366  residual_parameters.push_back(x1);
367  residual_parameters.push_back(x2);
368  residual_parameters.push_back(x3);
369  residual_parameters.push_back(x4);
370  return AddResidualBlock(cost_function, loss_function, residual_parameters);
371}
372
373ResidualBlock* ProblemImpl::AddResidualBlock(
374    CostFunction* cost_function,
375    LossFunction* loss_function,
376    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) {
377  vector<double*> residual_parameters;
378  residual_parameters.push_back(x0);
379  residual_parameters.push_back(x1);
380  residual_parameters.push_back(x2);
381  residual_parameters.push_back(x3);
382  residual_parameters.push_back(x4);
383  residual_parameters.push_back(x5);
384  return AddResidualBlock(cost_function, loss_function, residual_parameters);
385}
386
387ResidualBlock* ProblemImpl::AddResidualBlock(
388    CostFunction* cost_function,
389    LossFunction* loss_function,
390    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
391    double* x6) {
392  vector<double*> residual_parameters;
393  residual_parameters.push_back(x0);
394  residual_parameters.push_back(x1);
395  residual_parameters.push_back(x2);
396  residual_parameters.push_back(x3);
397  residual_parameters.push_back(x4);
398  residual_parameters.push_back(x5);
399  residual_parameters.push_back(x6);
400  return AddResidualBlock(cost_function, loss_function, residual_parameters);
401}
402
403ResidualBlock* ProblemImpl::AddResidualBlock(
404    CostFunction* cost_function,
405    LossFunction* loss_function,
406    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
407    double* x6, double* x7) {
408  vector<double*> residual_parameters;
409  residual_parameters.push_back(x0);
410  residual_parameters.push_back(x1);
411  residual_parameters.push_back(x2);
412  residual_parameters.push_back(x3);
413  residual_parameters.push_back(x4);
414  residual_parameters.push_back(x5);
415  residual_parameters.push_back(x6);
416  residual_parameters.push_back(x7);
417  return AddResidualBlock(cost_function, loss_function, residual_parameters);
418}
419
420ResidualBlock* ProblemImpl::AddResidualBlock(
421    CostFunction* cost_function,
422    LossFunction* loss_function,
423    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
424    double* x6, double* x7, double* x8) {
425  vector<double*> residual_parameters;
426  residual_parameters.push_back(x0);
427  residual_parameters.push_back(x1);
428  residual_parameters.push_back(x2);
429  residual_parameters.push_back(x3);
430  residual_parameters.push_back(x4);
431  residual_parameters.push_back(x5);
432  residual_parameters.push_back(x6);
433  residual_parameters.push_back(x7);
434  residual_parameters.push_back(x8);
435  return AddResidualBlock(cost_function, loss_function, residual_parameters);
436}
437
438ResidualBlock* ProblemImpl::AddResidualBlock(
439    CostFunction* cost_function,
440    LossFunction* loss_function,
441    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,
442    double* x6, double* x7, double* x8, double* x9) {
443  vector<double*> residual_parameters;
444  residual_parameters.push_back(x0);
445  residual_parameters.push_back(x1);
446  residual_parameters.push_back(x2);
447  residual_parameters.push_back(x3);
448  residual_parameters.push_back(x4);
449  residual_parameters.push_back(x5);
450  residual_parameters.push_back(x6);
451  residual_parameters.push_back(x7);
452  residual_parameters.push_back(x8);
453  residual_parameters.push_back(x9);
454  return AddResidualBlock(cost_function, loss_function, residual_parameters);
455}
456
457void ProblemImpl::AddParameterBlock(double* values, int size) {
458  InternalAddParameterBlock(values, size);
459}
460
461void ProblemImpl::AddParameterBlock(
462    double* values,
463    int size,
464    LocalParameterization* local_parameterization) {
465  ParameterBlock* parameter_block =
466      InternalAddParameterBlock(values, size);
467  if (local_parameterization != NULL) {
468    parameter_block->SetParameterization(local_parameterization);
469  }
470}
471
472// Delete a block from a vector of blocks, maintaining the indexing invariant.
473// This is done in constant time by moving an element from the end of the
474// vector over the element to remove, then popping the last element. It
475// destroys the ordering in the interest of speed.
476template<typename Block>
477void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,
478                                      Block* block_to_remove) {
479  CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove)
480      << "You found a Ceres bug! \n"
481      << "Block requested: "
482      << block_to_remove->ToString() << "\n"
483      << "Block present: "
484      << (*mutable_blocks)[block_to_remove->index()]->ToString();
485
486  // Prepare the to-be-moved block for the new, lower-in-index position by
487  // setting the index to the blocks final location.
488  Block* tmp = mutable_blocks->back();
489  tmp->set_index(block_to_remove->index());
490
491  // Overwrite the to-be-deleted residual block with the one at the end.
492  (*mutable_blocks)[block_to_remove->index()] = tmp;
493
494  DeleteBlock(block_to_remove);
495
496  // The block is gone so shrink the vector of blocks accordingly.
497  mutable_blocks->pop_back();
498}
499
500void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) {
501  CHECK_NOTNULL(residual_block);
502
503  // Verify that residual_block identifies a residual in the current problem.
504  const string residual_not_found_message =
505      StringPrintf("Residual block to remove: %p not found. This usually means "
506                   "one of three things have happened:\n"
507                   " 1) residual_block is uninitialised and points to a random "
508                   "area in memory.\n"
509                   " 2) residual_block represented a residual that was added to"
510                   " the problem, but referred to a parameter block which has "
511                   "since been removed, which removes all residuals which "
512                   "depend on that parameter block, and was thus removed.\n"
513                   " 3) residual_block referred to a residual that has already "
514                   "been removed from the problem (by the user).",
515                   residual_block);
516  if (options_.enable_fast_removal) {
517    CHECK(residual_block_set_.find(residual_block) !=
518          residual_block_set_.end())
519        << residual_not_found_message;
520  } else {
521    // Perform a full search over all current residuals.
522    CHECK(std::find(program_->residual_blocks().begin(),
523                    program_->residual_blocks().end(),
524                    residual_block) != program_->residual_blocks().end())
525        << residual_not_found_message;
526  }
527
528  InternalRemoveResidualBlock(residual_block);
529}
530
531void ProblemImpl::RemoveParameterBlock(double* values) {
532  ParameterBlock* parameter_block =
533      FindParameterBlockOrDie(parameter_block_map_, values);
534
535  if (options_.enable_fast_removal) {
536    // Copy the dependent residuals from the parameter block because the set of
537    // dependents will change after each call to RemoveResidualBlock().
538    vector<ResidualBlock*> residual_blocks_to_remove(
539        parameter_block->mutable_residual_blocks()->begin(),
540        parameter_block->mutable_residual_blocks()->end());
541    for (int i = 0; i < residual_blocks_to_remove.size(); ++i) {
542      InternalRemoveResidualBlock(residual_blocks_to_remove[i]);
543    }
544  } else {
545    // Scan all the residual blocks to remove ones that depend on the parameter
546    // block. Do the scan backwards since the vector changes while iterating.
547    const int num_residual_blocks = NumResidualBlocks();
548    for (int i = num_residual_blocks - 1; i >= 0; --i) {
549      ResidualBlock* residual_block =
550          (*(program_->mutable_residual_blocks()))[i];
551      const int num_parameter_blocks = residual_block->NumParameterBlocks();
552      for (int j = 0; j < num_parameter_blocks; ++j) {
553        if (residual_block->parameter_blocks()[j] == parameter_block) {
554          InternalRemoveResidualBlock(residual_block);
555          // The parameter blocks are guaranteed unique.
556          break;
557        }
558      }
559    }
560  }
561  DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block);
562}
563
564void ProblemImpl::SetParameterBlockConstant(double* values) {
565  FindParameterBlockOrDie(parameter_block_map_, values)->SetConstant();
566}
567
568void ProblemImpl::SetParameterBlockVariable(double* values) {
569  FindParameterBlockOrDie(parameter_block_map_, values)->SetVarying();
570}
571
572void ProblemImpl::SetParameterization(
573    double* values,
574    LocalParameterization* local_parameterization) {
575  FindParameterBlockOrDie(parameter_block_map_, values)
576      ->SetParameterization(local_parameterization);
577}
578
579const LocalParameterization* ProblemImpl::GetParameterization(
580    double* values) const {
581  return FindParameterBlockOrDie(parameter_block_map_, values)
582      ->local_parameterization();
583}
584
585void ProblemImpl::SetParameterLowerBound(double* values,
586                                         int index,
587                                         double lower_bound) {
588  FindParameterBlockOrDie(parameter_block_map_, values)
589      ->SetLowerBound(index, lower_bound);
590}
591
592void ProblemImpl::SetParameterUpperBound(double* values,
593                                         int index,
594                                         double upper_bound) {
595  FindParameterBlockOrDie(parameter_block_map_, values)
596      ->SetUpperBound(index, upper_bound);
597}
598
599bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,
600                           double* cost,
601                           vector<double>* residuals,
602                           vector<double>* gradient,
603                           CRSMatrix* jacobian) {
604  if (cost == NULL &&
605      residuals == NULL &&
606      gradient == NULL &&
607      jacobian == NULL) {
608    LOG(INFO) << "Nothing to do.";
609    return true;
610  }
611
612  // If the user supplied residual blocks, then use them, otherwise
613  // take the residual blocks from the underlying program.
614  Program program;
615  *program.mutable_residual_blocks() =
616      ((evaluate_options.residual_blocks.size() > 0)
617       ? evaluate_options.residual_blocks : program_->residual_blocks());
618
619  const vector<double*>& parameter_block_ptrs =
620      evaluate_options.parameter_blocks;
621
622  vector<ParameterBlock*> variable_parameter_blocks;
623  vector<ParameterBlock*>& parameter_blocks =
624      *program.mutable_parameter_blocks();
625
626  if (parameter_block_ptrs.size() == 0) {
627    // The user did not provide any parameter blocks, so default to
628    // using all the parameter blocks in the order that they are in
629    // the underlying program object.
630    parameter_blocks = program_->parameter_blocks();
631  } else {
632    // The user supplied a vector of parameter blocks. Using this list
633    // requires a number of steps.
634
635    // 1. Convert double* into ParameterBlock*
636    parameter_blocks.resize(parameter_block_ptrs.size());
637    for (int i = 0; i < parameter_block_ptrs.size(); ++i) {
638      parameter_blocks[i] =
639          FindParameterBlockOrDie(parameter_block_map_,
640                                  parameter_block_ptrs[i]);
641    }
642
643    // 2. The user may have only supplied a subset of parameter
644    // blocks, so identify the ones that are not supplied by the user
645    // and are NOT constant. These parameter blocks are stored in
646    // variable_parameter_blocks.
647    //
648    // To ensure that the parameter blocks are not included in the
649    // columns of the jacobian, we need to make sure that they are
650    // constant during evaluation and then make them variable again
651    // after we are done.
652    vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks());
653    vector<ParameterBlock*> included_parameter_blocks(
654        program.parameter_blocks());
655
656    vector<ParameterBlock*> excluded_parameter_blocks;
657    sort(all_parameter_blocks.begin(), all_parameter_blocks.end());
658    sort(included_parameter_blocks.begin(), included_parameter_blocks.end());
659    set_difference(all_parameter_blocks.begin(),
660                   all_parameter_blocks.end(),
661                   included_parameter_blocks.begin(),
662                   included_parameter_blocks.end(),
663                   back_inserter(excluded_parameter_blocks));
664
665    variable_parameter_blocks.reserve(excluded_parameter_blocks.size());
666    for (int i = 0; i < excluded_parameter_blocks.size(); ++i) {
667      ParameterBlock* parameter_block = excluded_parameter_blocks[i];
668      if (!parameter_block->IsConstant()) {
669        variable_parameter_blocks.push_back(parameter_block);
670        parameter_block->SetConstant();
671      }
672    }
673  }
674
675  // Setup the Parameter indices and offsets before an evaluator can
676  // be constructed and used.
677  program.SetParameterOffsetsAndIndex();
678
679  Evaluator::Options evaluator_options;
680
681  // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or
682  // CXSparse, here it just being used for telling the evaluator to
683  // use a SparseRowCompressedMatrix for the jacobian. This is because
684  // the Evaluator decides the storage for the Jacobian based on the
685  // type of linear solver being used.
686  evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
687  evaluator_options.num_threads = evaluate_options.num_threads;
688
689  string error;
690  scoped_ptr<Evaluator> evaluator(
691      Evaluator::Create(evaluator_options, &program, &error));
692  if (evaluator.get() == NULL) {
693    LOG(ERROR) << "Unable to create an Evaluator object. "
694               << "Error: " << error
695               << "This is a Ceres bug; please contact the developers!";
696
697    // Make the parameter blocks that were temporarily marked
698    // constant, variable again.
699    for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
700      variable_parameter_blocks[i]->SetVarying();
701    }
702
703    program_->SetParameterBlockStatePtrsToUserStatePtrs();
704    program_->SetParameterOffsetsAndIndex();
705    return false;
706  }
707
708  if (residuals !=NULL) {
709    residuals->resize(evaluator->NumResiduals());
710  }
711
712  if (gradient != NULL) {
713    gradient->resize(evaluator->NumEffectiveParameters());
714  }
715
716  scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian;
717  if (jacobian != NULL) {
718    tmp_jacobian.reset(
719        down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian()));
720  }
721
722  // Point the state pointers to the user state pointers. This is
723  // needed so that we can extract a parameter vector which is then
724  // passed to Evaluator::Evaluate.
725  program.SetParameterBlockStatePtrsToUserStatePtrs();
726
727  // Copy the value of the parameter blocks into a vector, since the
728  // Evaluate::Evaluate method needs its input as such. The previous
729  // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that
730  // these values are the ones corresponding to the actual state of
731  // the parameter blocks, rather than the temporary state pointer
732  // used for evaluation.
733  Vector parameters(program.NumParameters());
734  program.ParameterBlocksToStateVector(parameters.data());
735
736  double tmp_cost = 0;
737
738  Evaluator::EvaluateOptions evaluator_evaluate_options;
739  evaluator_evaluate_options.apply_loss_function =
740      evaluate_options.apply_loss_function;
741  bool status = evaluator->Evaluate(evaluator_evaluate_options,
742                                    parameters.data(),
743                                    &tmp_cost,
744                                    residuals != NULL ? &(*residuals)[0] : NULL,
745                                    gradient != NULL ? &(*gradient)[0] : NULL,
746                                    tmp_jacobian.get());
747
748  // Make the parameter blocks that were temporarily marked constant,
749  // variable again.
750  for (int i = 0; i < variable_parameter_blocks.size(); ++i) {
751    variable_parameter_blocks[i]->SetVarying();
752  }
753
754  if (status) {
755    if (cost != NULL) {
756      *cost = tmp_cost;
757    }
758    if (jacobian != NULL) {
759      tmp_jacobian->ToCRSMatrix(jacobian);
760    }
761  }
762
763  program_->SetParameterBlockStatePtrsToUserStatePtrs();
764  program_->SetParameterOffsetsAndIndex();
765  return status;
766}
767
768int ProblemImpl::NumParameterBlocks() const {
769  return program_->NumParameterBlocks();
770}
771
772int ProblemImpl::NumParameters() const {
773  return program_->NumParameters();
774}
775
776int ProblemImpl::NumResidualBlocks() const {
777  return program_->NumResidualBlocks();
778}
779
780int ProblemImpl::NumResiduals() const {
781  return program_->NumResiduals();
782}
783
784int ProblemImpl::ParameterBlockSize(const double* parameter_block) const {
785  return FindParameterBlockOrDie(parameter_block_map_,
786                                 const_cast<double*>(parameter_block))->Size();
787};
788
789int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const {
790  return FindParameterBlockOrDie(
791      parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize();
792};
793
794bool ProblemImpl::HasParameterBlock(const double* parameter_block) const {
795  return (parameter_block_map_.find(const_cast<double*>(parameter_block)) !=
796          parameter_block_map_.end());
797}
798
799void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {
800  CHECK_NOTNULL(parameter_blocks);
801  parameter_blocks->resize(0);
802  for (ParameterMap::const_iterator it = parameter_block_map_.begin();
803       it != parameter_block_map_.end();
804       ++it) {
805    parameter_blocks->push_back(it->first);
806  }
807}
808
809void ProblemImpl::GetResidualBlocks(
810    vector<ResidualBlockId>* residual_blocks) const {
811  CHECK_NOTNULL(residual_blocks);
812  *residual_blocks = program().residual_blocks();
813}
814
815void ProblemImpl::GetParameterBlocksForResidualBlock(
816    const ResidualBlockId residual_block,
817    vector<double*>* parameter_blocks) const {
818  int num_parameter_blocks = residual_block->NumParameterBlocks();
819  CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks);
820  for (int i = 0; i < num_parameter_blocks; ++i) {
821    (*parameter_blocks)[i] =
822        residual_block->parameter_blocks()[i]->mutable_user_state();
823  }
824}
825
826void ProblemImpl::GetResidualBlocksForParameterBlock(
827    const double* values,
828    vector<ResidualBlockId>* residual_blocks) const {
829  ParameterBlock* parameter_block =
830      FindParameterBlockOrDie(parameter_block_map_,
831                              const_cast<double*>(values));
832
833  if (options_.enable_fast_removal) {
834    // In this case the residual blocks that depend on the parameter block are
835    // stored in the parameter block already, so just copy them out.
836    CHECK_NOTNULL(residual_blocks)->resize(
837        parameter_block->mutable_residual_blocks()->size());
838    std::copy(parameter_block->mutable_residual_blocks()->begin(),
839              parameter_block->mutable_residual_blocks()->end(),
840              residual_blocks->begin());
841    return;
842  }
843
844  // Find residual blocks that depend on the parameter block.
845  CHECK_NOTNULL(residual_blocks)->clear();
846  const int num_residual_blocks = NumResidualBlocks();
847  for (int i = 0; i < num_residual_blocks; ++i) {
848    ResidualBlock* residual_block =
849        (*(program_->mutable_residual_blocks()))[i];
850    const int num_parameter_blocks = residual_block->NumParameterBlocks();
851    for (int j = 0; j < num_parameter_blocks; ++j) {
852      if (residual_block->parameter_blocks()[j] == parameter_block) {
853        residual_blocks->push_back(residual_block);
854        // The parameter blocks are guaranteed unique.
855        break;
856      }
857    }
858  }
859}
860
861}  // namespace internal
862}  // namespace ceres
863