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