diff options
Diffstat (limited to 'extern/ceres/internal/ceres/problem_impl.cc')
| -rw-r--r-- | extern/ceres/internal/ceres/problem_impl.cc | 185 |
1 files changed, 116 insertions, 69 deletions
diff --git a/extern/ceres/internal/ceres/problem_impl.cc b/extern/ceres/internal/ceres/problem_impl.cc index 3155bc3569e..01a22c128be 100644 --- a/extern/ceres/internal/ceres/problem_impl.cc +++ b/extern/ceres/internal/ceres/problem_impl.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -49,9 +49,11 @@ #include "ceres/crs_matrix.h" #include "ceres/evaluation_callback.h" #include "ceres/evaluator.h" +#include "ceres/internal/export.h" #include "ceres/internal/fixed_array.h" -#include "ceres/internal/port.h" #include "ceres/loss_function.h" +#include "ceres/manifold.h" +#include "ceres/manifold_adapter.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/program.h" @@ -64,11 +66,6 @@ namespace ceres { namespace internal { - -using std::map; -using std::string; -using std::vector; - namespace { // Returns true if two regions of memory, a and b, with sizes size_a and size_b // respectively, overlap. @@ -130,7 +127,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, << "for a parameter with size " << size; // Ignore the request if there is a block for the given pointer already. - ParameterMap::iterator it = parameter_block_map_.find(values); + auto it = parameter_block_map_.find(values); if (it != parameter_block_map_.end()) { if (!options_.disable_all_safety_checks) { int existing_size = it->second->Size(); @@ -146,11 +143,11 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, // 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); + auto lb = parameter_block_map_.lower_bound(values); // If lb is not the first block, check the previous block for aliasing. if (lb != parameter_block_map_.begin()) { - ParameterMap::iterator previous = lb; + auto previous = lb; --previous; CheckForNoAliasing( previous->first, previous->second->Size(), values, size); @@ -165,7 +162,7 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, // Pass the index of the new parameter block as well to keep the index in // sync with the position of the parameter in the program's parameter vector. - ParameterBlock* new_parameter_block = + auto* new_parameter_block = new ParameterBlock(values, size, program_->parameter_blocks_.size()); // For dynamic problems, add the list of dependent residual blocks, which is @@ -192,7 +189,7 @@ void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { residual_block); } - ResidualBlockSet::iterator it = residual_block_set_.find(residual_block); + auto it = residual_block_set_.find(residual_block); residual_block_set_.erase(it); } DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); @@ -207,13 +204,13 @@ void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { // The const casts here are legit, since ResidualBlock holds these // pointers as const pointers but we have ownership of them and // have the right to destroy them when the destructor is called. - CostFunction* cost_function = + auto* cost_function = const_cast<CostFunction*>(residual_block->cost_function()); if (options_.cost_function_ownership == TAKE_OWNERSHIP) { DecrementValueOrDeleteKey(cost_function, &cost_function_ref_count_); } - LossFunction* loss_function = + auto* loss_function = const_cast<LossFunction*>(residual_block->loss_function()); if (options_.loss_function_ownership == TAKE_OWNERSHIP && loss_function != nullptr) { @@ -225,15 +222,7 @@ void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { // Deletes the parameter block in question, assuming there are no other // references to it inside the problem (e.g. by any residual blocks). -// Referenced parameterizations are tucked away for future deletion, since it -// is not possible to know whether other parts of the problem depend on them -// without doing a full scan. void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { - if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && - parameter_block->local_parameterization() != nullptr) { - local_parameterizations_to_delete_.push_back( - parameter_block->mutable_local_parameterization()); - } parameter_block_map_.erase(parameter_block->mutable_user_state()); delete parameter_block; } @@ -264,14 +253,18 @@ ProblemImpl::~ProblemImpl() { } // Collect the unique parameterizations and delete the parameters. - for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { - DeleteBlock(program_->parameter_blocks_[i]); + for (auto* parameter_block : program_->parameter_blocks_) { + DeleteBlock(parameter_block); } // Delete the owned parameterizations. STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), local_parameterizations_to_delete_.end()); + // Delete the owned manifolds. + STLDeleteUniqueContainerPointers(manifolds_to_delete_.begin(), + manifolds_to_delete_.end()); + if (context_impl_owned_) { delete context_impl_; } @@ -286,7 +279,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( CHECK_EQ(num_parameter_blocks, cost_function->parameter_block_sizes().size()); // Check the sizes match. - const vector<int32_t>& parameter_block_sizes = + const std::vector<int32_t>& parameter_block_sizes = cost_function->parameter_block_sizes(); if (!options_.disable_all_safety_checks) { @@ -295,7 +288,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( << "that the cost function expects."; // Check for duplicate parameter blocks. - vector<double*> sorted_parameter_blocks( + std::vector<double*> sorted_parameter_blocks( parameter_blocks, parameter_blocks + num_parameter_blocks); sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); const bool has_duplicate_items = @@ -303,7 +296,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( sorted_parameter_blocks.end()) != sorted_parameter_blocks.end()); if (has_duplicate_items) { - string blocks; + std::string blocks; for (int i = 0; i < num_parameter_blocks; ++i) { blocks += StringPrintf(" %p ", parameter_blocks[i]); } @@ -315,7 +308,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( } // Add parameter blocks and convert the double*'s to parameter blocks. - vector<ParameterBlock*> parameter_block_ptrs(num_parameter_blocks); + std::vector<ParameterBlock*> parameter_block_ptrs(num_parameter_blocks); for (int i = 0; i < num_parameter_blocks; ++i) { parameter_block_ptrs[i] = InternalAddParameterBlock( parameter_blocks[i], parameter_block_sizes[i]); @@ -334,7 +327,7 @@ ResidualBlockId ProblemImpl::AddResidualBlock( } } - ResidualBlock* new_residual_block = + auto* new_residual_block = new ResidualBlock(cost_function, loss_function, parameter_block_ptrs, @@ -372,12 +365,50 @@ void ProblemImpl::AddParameterBlock(double* values, int size) { InternalAddParameterBlock(values, size); } +void ProblemImpl::InternalSetParameterization( + double* values, + ParameterBlock* parameter_block, + LocalParameterization* local_parameterization) { + parameter_block_to_local_param_[values] = local_parameterization; + Manifold* manifold = nullptr; + if (local_parameterization != nullptr) { + if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { + local_parameterizations_to_delete_.push_back(local_parameterization); + } + + manifold = new ManifoldAdapter(local_parameterization); + // Add the manifold to manifolds_to_delete_ unconditionally since + // we own it and it will need to be deleted. + manifolds_to_delete_.push_back(manifold); + } + + parameter_block->SetManifold(manifold); +} + +void ProblemImpl::InternalSetManifold(double* values, + ParameterBlock* parameter_block, + Manifold* manifold) { + // Reset any association between this parameter block and a local + // parameterization. This only needs done while we are in the transition from + // LocalParameterization to Manifold. + parameter_block_to_local_param_[values] = nullptr; + if (manifold != nullptr && options_.manifold_ownership == TAKE_OWNERSHIP) { + manifolds_to_delete_.push_back(manifold); + } + parameter_block->SetManifold(manifold); +} + void ProblemImpl::AddParameterBlock( double* values, int size, LocalParameterization* local_parameterization) { ParameterBlock* parameter_block = InternalAddParameterBlock(values, size); - if (local_parameterization != nullptr) { - parameter_block->SetParameterization(local_parameterization); - } + InternalSetParameterization(values, parameter_block, local_parameterization); +} + +void ProblemImpl::AddParameterBlock(double* values, + int size, + Manifold* manifold) { + ParameterBlock* parameter_block = InternalAddParameterBlock(values, size); + InternalSetManifold(values, parameter_block, manifold); } // Delete a block from a vector of blocks, maintaining the indexing invariant. @@ -385,7 +416,7 @@ void ProblemImpl::AddParameterBlock( // vector over the element to remove, then popping the last element. It // destroys the ordering in the interest of speed. template <typename Block> -void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, +void ProblemImpl::DeleteBlockInVector(std::vector<Block*>* mutable_blocks, Block* block_to_remove) { CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) << "You found a Ceres bug! \n" @@ -411,7 +442,7 @@ void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { CHECK(residual_block != nullptr); // Verify that residual_block identifies a residual in the current problem. - const string residual_not_found_message = StringPrintf( + const std::string residual_not_found_message = StringPrintf( "Residual block to remove: %p not found. This usually means " "one of three things have happened:\n" " 1) residual_block is uninitialised and points to a random " @@ -449,11 +480,11 @@ void ProblemImpl::RemoveParameterBlock(const double* values) { if (options_.enable_fast_removal) { // Copy the dependent residuals from the parameter block because the set of // dependents will change after each call to RemoveResidualBlock(). - vector<ResidualBlock*> residual_blocks_to_remove( + std::vector<ResidualBlock*> residual_blocks_to_remove( parameter_block->mutable_residual_blocks()->begin(), parameter_block->mutable_residual_blocks()->end()); - for (int i = 0; i < residual_blocks_to_remove.size(); ++i) { - InternalRemoveResidualBlock(residual_blocks_to_remove[i]); + for (auto* residual_block : residual_blocks_to_remove) { + InternalRemoveResidualBlock(residual_block); } } else { // Scan all the residual blocks to remove ones that depend on the parameter @@ -518,20 +549,31 @@ void ProblemImpl::SetParameterization( << "you can set its local parameterization."; } - // If the parameter block already has a local parameterization and - // we are to take ownership of local parameterizations, then add it - // to local_parameterizations_to_delete_ for eventual deletion. - if (parameter_block->local_parameterization_ && - options_.local_parameterization_ownership == TAKE_OWNERSHIP) { - local_parameterizations_to_delete_.push_back( - parameter_block->local_parameterization_); + InternalSetParameterization(values, parameter_block, local_parameterization); +} + +void ProblemImpl::SetManifold(double* values, Manifold* manifold) { + ParameterBlock* parameter_block = + FindWithDefault(parameter_block_map_, values, nullptr); + if (parameter_block == nullptr) { + LOG(FATAL) << "Parameter block not found: " << values + << ". You must add the parameter block to the problem before " + << "you can set its manifold."; } - parameter_block->SetParameterization(local_parameterization); + InternalSetManifold(values, parameter_block, manifold); } const LocalParameterization* ProblemImpl::GetParameterization( const double* values) const { + return FindWithDefault(parameter_block_to_local_param_, values, nullptr); +} + +bool ProblemImpl::HasParameterization(const double* values) const { + return GetParameterization(values) != nullptr; +} + +const Manifold* ProblemImpl::GetManifold(const double* values) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { @@ -540,7 +582,11 @@ const LocalParameterization* ProblemImpl::GetParameterization( << "you can get its local parameterization."; } - return parameter_block->local_parameterization(); + return parameter_block->manifold(); +} + +bool ProblemImpl::HasManifold(const double* values) const { + return GetManifold(values) != nullptr; } void ProblemImpl::SetParameterLowerBound(double* values, @@ -596,8 +642,8 @@ double ProblemImpl::GetParameterUpperBound(const double* values, bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, double* cost, - vector<double>* residuals, - vector<double>* gradient, + std::vector<double>* residuals, + std::vector<double>* gradient, CRSMatrix* jacobian) { if (cost == nullptr && residuals == nullptr && gradient == nullptr && jacobian == nullptr) { @@ -612,11 +658,11 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, ? evaluate_options.residual_blocks : program_->residual_blocks()); - const vector<double*>& parameter_block_ptrs = + const std::vector<double*>& parameter_block_ptrs = evaluate_options.parameter_blocks; - vector<ParameterBlock*> variable_parameter_blocks; - vector<ParameterBlock*>& parameter_blocks = + std::vector<ParameterBlock*> variable_parameter_blocks; + std::vector<ParameterBlock*>& parameter_blocks = *program.mutable_parameter_blocks(); if (parameter_block_ptrs.size() == 0) { @@ -649,11 +695,12 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, // columns of the jacobian, we need to make sure that they are // constant during evaluation and then make them variable again // after we are done. - vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks()); - vector<ParameterBlock*> included_parameter_blocks( + std::vector<ParameterBlock*> all_parameter_blocks( + program_->parameter_blocks()); + std::vector<ParameterBlock*> included_parameter_blocks( program.parameter_blocks()); - vector<ParameterBlock*> excluded_parameter_blocks; + std::vector<ParameterBlock*> excluded_parameter_blocks; sort(all_parameter_blocks.begin(), all_parameter_blocks.end()); sort(included_parameter_blocks.begin(), included_parameter_blocks.end()); set_difference(all_parameter_blocks.begin(), @@ -663,8 +710,7 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, back_inserter(excluded_parameter_blocks)); variable_parameter_blocks.reserve(excluded_parameter_blocks.size()); - for (int i = 0; i < excluded_parameter_blocks.size(); ++i) { - ParameterBlock* parameter_block = excluded_parameter_blocks[i]; + for (auto* parameter_block : excluded_parameter_blocks) { if (!parameter_block->IsConstant()) { variable_parameter_blocks.push_back(parameter_block); parameter_block->SetConstant(); @@ -716,8 +762,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, std::unique_ptr<CompressedRowSparseMatrix> tmp_jacobian; if (jacobian != nullptr) { - tmp_jacobian.reset( - down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); + tmp_jacobian.reset(down_cast<CompressedRowSparseMatrix*>( + evaluator->CreateJacobian().release())); } // Point the state pointers to the user state pointers. This is @@ -749,8 +795,8 @@ bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, // Make the parameter blocks that were temporarily marked constant, // variable again. - for (int i = 0; i < variable_parameter_blocks.size(); ++i) { - variable_parameter_blocks[i]->SetVarying(); + for (auto* parameter_block : variable_parameter_blocks) { + parameter_block->SetVarying(); } if (status) { @@ -829,24 +875,25 @@ int ProblemImpl::ParameterBlockSize(const double* values) const { return parameter_block->Size(); } -int ProblemImpl::ParameterBlockLocalSize(const double* values) const { +int ProblemImpl::ParameterBlockTangentSize(const double* values) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " - << "you can get its local size."; + << "you can get its tangent size."; } - return parameter_block->LocalSize(); + return parameter_block->TangentSize(); } -bool ProblemImpl::HasParameterBlock(const double* parameter_block) const { - return (parameter_block_map_.find(const_cast<double*>(parameter_block)) != +bool ProblemImpl::HasParameterBlock(const double* values) const { + return (parameter_block_map_.find(const_cast<double*>(values)) != parameter_block_map_.end()); } -void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { +void ProblemImpl::GetParameterBlocks( + std::vector<double*>* parameter_blocks) const { CHECK(parameter_blocks != nullptr); parameter_blocks->resize(0); parameter_blocks->reserve(parameter_block_map_.size()); @@ -856,14 +903,14 @@ void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { } void ProblemImpl::GetResidualBlocks( - vector<ResidualBlockId>* residual_blocks) const { + std::vector<ResidualBlockId>* residual_blocks) const { CHECK(residual_blocks != nullptr); *residual_blocks = program().residual_blocks(); } void ProblemImpl::GetParameterBlocksForResidualBlock( const ResidualBlockId residual_block, - vector<double*>* parameter_blocks) const { + std::vector<double*>* parameter_blocks) const { int num_parameter_blocks = residual_block->NumParameterBlocks(); CHECK(parameter_blocks != nullptr); parameter_blocks->resize(num_parameter_blocks); @@ -884,7 +931,7 @@ const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock( } void ProblemImpl::GetResidualBlocksForParameterBlock( - const double* values, vector<ResidualBlockId>* residual_blocks) const { + const double* values, std::vector<ResidualBlockId>* residual_blocks) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { |