Speed up Problem construction and destruction. Change-Id: I3147b0b60eedf40f8453d5a39ff04a572c445a2f
diff --git a/include/ceres/problem.h b/include/ceres/problem.h index 201cc7f..a8645c6 100644 --- a/include/ceres/problem.h +++ b/include/ceres/problem.h
@@ -122,7 +122,8 @@ Options() : cost_function_ownership(TAKE_OWNERSHIP), loss_function_ownership(TAKE_OWNERSHIP), - local_parameterization_ownership(TAKE_OWNERSHIP) {} + local_parameterization_ownership(TAKE_OWNERSHIP), + disable_all_safety_checks(false) {} // These flags control whether the Problem object owns the cost // functions, loss functions, and parameterizations passed into @@ -134,6 +135,18 @@ Ownership cost_function_ownership; Ownership loss_function_ownership; Ownership local_parameterization_ownership; + + // By default, Ceres performs a variety of safety checks when + // constructing the problem. There is a small but measurable + // performance penalty to these checks ~5%. If you are sure of + // your problem construction, and 5% of the problem construction + // time is truly an overhead you want to avoid, then you can set + // disable_all_safety_checks to true. + // + // WARNING: + // Do not set this to true, unless you are absolutely sure of what + // you are doing. + bool disable_all_safety_checks; }; // The default constructor is equivalent to the
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc index f061e33..e9d23ec 100644 --- a/internal/ceres/problem_impl.cc +++ b/internal/ceres/problem_impl.cc
@@ -73,51 +73,54 @@ << "size " << new_block_size << "."; } -static ParameterBlock* InternalAddParameterBlock( - double* values, - int size, - ParameterMap* parameter_map, - vector<ParameterBlock*>* parameter_blocks) { - CHECK(values) << "Null pointer passed to AddParameterBlock for a parameter " - << "with size " << size; +ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, + int size) { + CHECK(values != NULL) << "Null pointer passed to AddParameterBlock " + << "for a parameter with size " << size; // Ignore the request if there is a block for the given pointer already. - ParameterMap::iterator it = parameter_map->find(values); - if (it != parameter_map->end()) { - int existing_size = it->second->Size(); - CHECK(size == existing_size) - << "Tried adding a parameter block with the same double pointer, " - << values << ", twice, but with different block sizes. Original " - << "size was " << existing_size << " but new size is " - << size; + ParameterMap::iterator it = parameter_block_map_.find(values); + if (it != parameter_block_map_.end()) { + if (!options_.disable_all_safety_checks) { + int existing_size = it->second->Size(); + CHECK(size == existing_size) + << "Tried adding a parameter block with the same double pointer, " + << values << ", twice, but with different block sizes. Original " + << "size was " << existing_size << " but new size is " + << size; + } return it->second; } - // Before adding the parameter block, also check that it doesn't alias any - // other parameter blocks. - if (!parameter_map->empty()) { - ParameterMap::iterator lb = parameter_map->lower_bound(values); - // If lb is not the first block, check the previous block for aliasing. - if (lb != parameter_map->begin()) { - ParameterMap::iterator previous = lb; - --previous; - CheckForNoAliasing(previous->first, - previous->second->Size(), - values, - size); - } + if (!options_.disable_all_safety_checks) { + // Before adding the parameter block, also check that it doesn't alias any + // other parameter blocks. + if (!parameter_block_map_.empty()) { + ParameterMap::iterator lb = parameter_block_map_.lower_bound(values); - // If lb is not off the end, check lb for aliasing. - if (lb != parameter_map->end()) { - CheckForNoAliasing(lb->first, - lb->second->Size(), - values, - size); + // If lb is not the first block, check the previous block for aliasing. + if (lb != parameter_block_map_.begin()) { + ParameterMap::iterator previous = lb; + --previous; + CheckForNoAliasing(previous->first, + previous->second->Size(), + values, + size); + } + + // If lb is not off the end, check lb for aliasing. + if (lb != parameter_block_map_.end()) { + CheckForNoAliasing(lb->first, + lb->second->Size(), + values, + size); + } } } + ParameterBlock* new_parameter_block = new ParameterBlock(values, size); - (*parameter_map)[values] = new_parameter_block; - parameter_blocks->push_back(new_parameter_block); + parameter_block_map_[values] = new_parameter_block; + program_->parameter_blocks_.push_back(new_parameter_block); return new_parameter_block; } @@ -128,8 +131,14 @@ ProblemImpl::~ProblemImpl() { // Collect the unique cost/loss functions and delete the residuals. - set<CostFunction*> cost_functions; - set<LossFunction*> loss_functions; + const int num_residual_blocks = program_->residual_blocks_.size(); + + vector<CostFunction*> cost_functions; + cost_functions.reserve(num_residual_blocks); + + vector<LossFunction*> loss_functions; + loss_functions.reserve(num_residual_blocks); + for (int i = 0; i < program_->residual_blocks_.size(); ++i) { ResidualBlock* residual_block = program_->residual_blocks_[i]; @@ -137,11 +146,11 @@ // pointers as const pointers but we have ownership of them and // have the right to destroy them when the destructor is called. if (options_.cost_function_ownership == TAKE_OWNERSHIP) { - cost_functions.insert( + cost_functions.push_back( const_cast<CostFunction*>(residual_block->cost_function())); } if (options_.loss_function_ownership == TAKE_OWNERSHIP) { - loss_functions.insert( + loss_functions.push_back( const_cast<LossFunction*>(residual_block->loss_function())); } @@ -149,24 +158,25 @@ } // Collect the unique parameterizations and delete the parameters. - set<LocalParameterization*> local_parameterizations; + vector<LocalParameterization*> local_parameterizations; for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { ParameterBlock* parameter_block = program_->parameter_blocks_[i]; if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { - local_parameterizations.insert(parameter_block->local_parameterization_); + local_parameterizations.push_back( + parameter_block->local_parameterization_); } delete parameter_block; } // Delete the owned cost/loss functions and parameterizations. - STLDeleteContainerPointers(local_parameterizations.begin(), - local_parameterizations.end()); - STLDeleteContainerPointers(cost_functions.begin(), - cost_functions.end()); - STLDeleteContainerPointers(loss_functions.begin(), - loss_functions.end()); + STLDeleteUniqueContainerPointers(local_parameterizations.begin(), + local_parameterizations.end()); + STLDeleteUniqueContainerPointers(cost_functions.begin(), + cost_functions.end()); + STLDeleteUniqueContainerPointers(loss_functions.begin(), + loss_functions.end()); } const ResidualBlock* ProblemImpl::AddResidualBlock( @@ -180,25 +190,28 @@ // Check the sizes match. const vector<int16>& parameter_block_sizes = cost_function->parameter_block_sizes(); - CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) - << "Number of blocks input is different than the number of blocks " - << "that the cost function expects."; - // Check for duplicate parameter blocks. - vector<double*> sorted_parameter_blocks(parameter_blocks); - sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); - vector<double*>::const_iterator duplicate_items = - unique(sorted_parameter_blocks.begin(), - sorted_parameter_blocks.end()); - if (duplicate_items != sorted_parameter_blocks.end()) { - string blocks; - for (int i = 0; i < parameter_blocks.size(); ++i) { - blocks += internal::StringPrintf(" %p ", parameter_blocks[i]); + if (!options_.disable_all_safety_checks) { + CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) + << "Number of blocks input is different than the number of blocks " + << "that the cost function expects."; + + // Check for duplicate parameter blocks. + vector<double*> sorted_parameter_blocks(parameter_blocks); + sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); + vector<double*>::const_iterator duplicate_items = + unique(sorted_parameter_blocks.begin(), + sorted_parameter_blocks.end()); + if (duplicate_items != sorted_parameter_blocks.end()) { + string blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + blocks += internal::StringPrintf(" %p ", parameter_blocks[i]); + } + + LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " + << "are not allowed. Parameter block pointers: [" + << blocks << "]"; } - - LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " - << "are not allowed. Parameter block pointers: [" - << blocks << "]"; } // Add parameter blocks and convert the double*'s to parameter blocks. @@ -206,20 +219,20 @@ for (int i = 0; i < parameter_blocks.size(); ++i) { parameter_block_ptrs[i] = InternalAddParameterBlock(parameter_blocks[i], - parameter_block_sizes[i], - ¶meter_block_map_, - &program_->parameter_blocks_); + parameter_block_sizes[i]); } - // Check that the block sizes match the block sizes expected by the - // cost_function. - for (int i = 0; i < parameter_block_ptrs.size(); ++i) { - CHECK_EQ(cost_function->parameter_block_sizes()[i], - parameter_block_ptrs[i]->Size()) - << "The cost function expects parameter block " << i - << " of size " << cost_function->parameter_block_sizes()[i] - << " but was given a block of size " - << parameter_block_ptrs[i]->Size(); + if (!options_.disable_all_safety_checks) { + // Check that the block sizes match the block sizes expected by the + // cost_function. + for (int i = 0; i < parameter_block_ptrs.size(); ++i) { + CHECK_EQ(cost_function->parameter_block_sizes()[i], + parameter_block_ptrs[i]->Size()) + << "The cost function expects parameter block " << i + << " of size " << cost_function->parameter_block_sizes()[i] + << " but was given a block of size " + << parameter_block_ptrs[i]->Size(); + } } ResidualBlock* new_residual_block = @@ -372,10 +385,7 @@ } void ProblemImpl::AddParameterBlock(double* values, int size) { - InternalAddParameterBlock(values, - size, - ¶meter_block_map_, - &program_->parameter_blocks_); + InternalAddParameterBlock(values, size); } void ProblemImpl::AddParameterBlock( @@ -383,10 +393,7 @@ int size, LocalParameterization* local_parameterization) { ParameterBlock* parameter_block = - InternalAddParameterBlock(values, - size, - ¶meter_block_map_, - &program_->parameter_blocks_); + InternalAddParameterBlock(values, size); if (local_parameterization != NULL) { parameter_block->SetParameterization(local_parameterization); }
diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h index 2f20d0d..82a1956 100644 --- a/internal/ceres/problem_impl.h +++ b/internal/ceres/problem_impl.h
@@ -133,6 +133,8 @@ const ParameterMap& parameter_map() const { return parameter_block_map_; } private: + ParameterBlock* InternalAddParameterBlock(double* values, int size); + const Problem::Options options_; // The mapping from user pointers to parameter blocks.
diff --git a/internal/ceres/stl_util.h b/internal/ceres/stl_util.h index a1a19e8..5e92eff 100644 --- a/internal/ceres/stl_util.h +++ b/internal/ceres/stl_util.h
@@ -53,6 +53,18 @@ } } +template <class ForwardIterator> +void STLDeleteUniqueContainerPointers(ForwardIterator begin, + ForwardIterator end) { + sort(begin, end); + ForwardIterator new_end = unique(begin, end); + while (begin != new_end) { + ForwardIterator temp = begin; + ++begin; + delete *temp; + } +} + // STLDeleteElements() deletes all the elements in an STL container and clears // the container. This function is suitable for use with a vector, set, // hash_set, or any other STL container which defines sensible begin(), end(),