diff --git a/include/ceres/problem.h b/include/ceres/problem.h index a8645c64c..f0ed1b00a 100644 --- a/include/ceres/problem.h +++ b/include/ceres/problem.h @@ -59,10 +59,9 @@ class ParameterBlock; class ResidualBlock; } // namespace internal -// A ResidualBlockId is a handle clients can use to delete residual -// blocks after creating them. They are opaque for any purposes other -// than that. -typedef const internal::ResidualBlock* ResidualBlockId; +// A ResidualBlockId is an opaque handle clients can use to remove residual +// blocks from a Problem after adding them. +typedef internal::ResidualBlock* ResidualBlockId; // A class to represent non-linear least squares problems. Such // problems have a cost function that is a sum of error terms (known @@ -123,6 +122,7 @@ class Problem { : cost_function_ownership(TAKE_OWNERSHIP), loss_function_ownership(TAKE_OWNERSHIP), local_parameterization_ownership(TAKE_OWNERSHIP), + enable_fast_parameter_block_removal(false), disable_all_safety_checks(false) {} // These flags control whether the Problem object owns the cost @@ -136,15 +136,26 @@ class Problem { 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 + // If true, trades memory for a faster RemoveParameterBlock() operation. + // + // RemoveParameterBlock() takes time proportional to the size of the entire + // Problem. If you only remove parameter blocks from the Problem + // occassionaly, this may be acceptable. However, if you are modifying the + // Problem frequently, and have memory to spare, then flip this switch to + // make RemoveParameterBlock() take time proportional to the number of + // residual blocks that depend on it. The increase in memory usage is an + // additonal hash set per parameter block containing all the residuals that + // depend on the parameter block. + bool enable_fast_parameter_block_removal; + + // By default, Ceres performs a variety of safety checks when constructing + // the problem. There is a small but measurable performance penalty to + // these checks, typically around 5% of construction time. If you are sure + // your problem construction is correct, 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 + // WARNING: Do not set this to true, unless you are absolutely sure of what // you are doing. bool disable_all_safety_checks; }; @@ -257,6 +268,33 @@ class Problem { int size, LocalParameterization* local_parameterization); + // Remove a parameter block from the problem. The parameterization of the + // parameter block, if it exists, will persist until the deletion of the + // problem (similar to cost/loss functions in residual block removal). Any + // residual blocks that depend on the parameter are also removed, as + // described above in RemoveResidualBlock(). + // + // If Problem::Options::enable_fast_parameter_block_removal is true, then the + // removal is fast (almost constant time). Otherwise, removing a parameter + // block will incur a scan of the entire Problem object. + // + // WARNING: Removing a residual or parameter block will destroy the implicit + // ordering, rendering the jacobian or residuals returned from the solver + // uninterpretable. If you depend on the evaluated jacobian, do not use + // remove! This may change in a future release. + void RemoveParameterBlock(double* values); + + // Remove a residual block from the problem. Any parameters that the residual + // block depends on are not removed. The cost and loss functions for the + // residual block will not get deleted immediately; won't happen until the + // problem itself is deleted. + // + // WARNING: Removing a residual or parameter block will destroy the implicit + // ordering, rendering the jacobian or residuals returned from the solver + // uninterpretable. If you depend on the evaluated jacobian, do not use + // remove! This may change in a future release. + void RemoveResidualBlock(ResidualBlockId residual_block); + // Hold the indicated parameter block constant during optimization. void SetParameterBlockConstant(double* values); diff --git a/internal/ceres/parameter_block.h b/internal/ceres/parameter_block.h index f20805ca8..4fcafe021 100644 --- a/internal/ceres/parameter_block.h +++ b/internal/ceres/parameter_block.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// Copyright 2010, 2011, 2012, 2013 Google Inc. All rights reserved. // http://code.google.com/p/ceres-solver/ // // Redistribution and use in source and binary forms, with or without @@ -34,6 +34,7 @@ #include #include #include "ceres/array_utils.h" +#include "ceres/collections_port.h" #include "ceres/integral_types.h" #include "ceres/internal/eigen.h" #include "ceres/internal/port.h" @@ -46,6 +47,7 @@ namespace ceres { namespace internal { class ProblemImpl; +class ResidualBlock; // The parameter block encodes the location of the user's original value, and // also the "current state" of the parameter. The evaluator uses whatever is in @@ -58,13 +60,28 @@ class ProblemImpl; // responsible for the proper disposal of the local parameterization. class ParameterBlock { public: - ParameterBlock(double* user_state, int size) { - Init(user_state, size, NULL); + // TODO(keir): Decide what data structure is best here. Should this be a set? + // Probably not, because sets are memory inefficient. However, if it's a + // vector, you can get into pathological linear performance when removing a + // residual block from a problem where all the residual blocks depend on one + // parameter; for example, shared focal length in a bundle adjustment + // problem. It might be worth making a custom structure that is just an array + // when it is small, but transitions to a hash set when it has more elements. + // + // For now, use a hash set. + typedef HashSet ResidualBlockSet; + + // Create a parameter block with the user state, size, and index specified. + // The size is the size of the parameter block and the index is the position + // if the parameter block inside a Program (if any). + ParameterBlock(double* user_state, int size, int index) { + Init(user_state, size, index, NULL); } ParameterBlock(double* user_state, int size, + int index, LocalParameterization* local_parameterization) { - Init(user_state, size, local_parameterization); + Init(user_state, size, index, local_parameterization); } // The size of the parameter block. @@ -187,12 +204,43 @@ class ParameterBlock { delta_offset_); } + void EnableResidualBlockDependencies() { + CHECK(residual_blocks_ == NULL) + << "Ceres bug: There is already a residual block collection " + << "for parameter block: " << ToString(); + residual_blocks_ = new ResidualBlockSet; + } + + void AddResidualBlock(ResidualBlock* residual_block) { + CHECK(residual_blocks_ != NULL) + << "Ceres bug: The residual block collection is null for parameter " + << "block: " << ToString(); + residual_blocks_->insert(residual_block); + } + + void RemoveResidualBlock(ResidualBlock* residual_block) { + CHECK(residual_blocks_ != NULL) + << "Ceres bug: The residual block collection is null for parameter " + << "block: " << ToString(); + CHECK(residual_blocks_->find(residual_block) != residual_blocks_->end()) + << "Ceres bug: Missing residual for parameter block: " << ToString(); + residual_blocks_->erase(residual_block); + } + + // This is only intended for iterating; perhaps this should only expose + // .begin() and .end(). + ResidualBlockSet* mutable_residual_blocks() { + return residual_blocks_; + } + private: void Init(double* user_state, int size, + int index, LocalParameterization* local_parameterization) { user_state_ = user_state; size_ = size; + index_ = index; is_constant_ = false; state_ = user_state_; @@ -201,9 +249,10 @@ class ParameterBlock { SetParameterization(local_parameterization); } - index_ = -1; state_offset_ = -1; delta_offset_ = -1; + + residual_blocks_ = NULL; } bool UpdateLocalParameterizationJacobian() { @@ -261,6 +310,9 @@ class ParameterBlock { // The offset of this parameter block inside a larger delta vector. int32 delta_offset_; + // If non-null, contains the residual blocks this parameter block is in. + ResidualBlockSet* residual_blocks_; + // Necessary so ProblemImpl can clean up the parameterizations. friend class ProblemImpl; }; diff --git a/internal/ceres/parameter_block_test.cc b/internal/ceres/parameter_block_test.cc index 35998dc45..09156f866 100644 --- a/internal/ceres/parameter_block_test.cc +++ b/internal/ceres/parameter_block_test.cc @@ -38,7 +38,7 @@ namespace internal { TEST(ParameterBlock, SetLocalParameterization) { double x[3] = { 1.0, 2.0, 3.0 }; - ParameterBlock parameter_block(x, 3); + ParameterBlock parameter_block(x, 3, -1); // The indices to set constant within the parameter block (used later). vector indices; @@ -111,7 +111,7 @@ struct TestParameterization : public LocalParameterization { TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) { TestParameterization test_parameterization; double x[1] = { 1.0 }; - ParameterBlock parameter_block(x, 1, &test_parameterization); + ParameterBlock parameter_block(x, 1, -1, &test_parameterization); EXPECT_EQ(2.0, *parameter_block.LocalParameterizationJacobian()); @@ -122,7 +122,7 @@ TEST(ParameterBlock, SetStateUpdatesLocalParameterizationJacobian) { TEST(ParameterBlock, PlusWithNoLocalParameterization) { double x[2] = { 1.0, 2.0 }; - ParameterBlock parameter_block(x, 2); + ParameterBlock parameter_block(x, 2, -1); double delta[2] = { 0.2, 0.3 }; double x_plus_delta[2]; @@ -164,7 +164,7 @@ class BadLocalParameterization : public LocalParameterization { TEST(ParameterBlock, DetectBadLocalParameterization) { double x = 1; BadLocalParameterization bad_parameterization; - ParameterBlock parameter_block(&x, 1, &bad_parameterization); + ParameterBlock parameter_block(&x, 1, -1, &bad_parameterization); double y = 2; EXPECT_FALSE(parameter_block.SetState(&y)); } diff --git a/internal/ceres/problem.cc b/internal/ceres/problem.cc index 7ee5b5c15..c8f4a21eb 100644 --- a/internal/ceres/problem.cc +++ b/internal/ceres/problem.cc @@ -36,8 +36,6 @@ namespace ceres { -class ResidualBlock; - Problem::Problem() : problem_impl_(new internal::ProblemImpl) {} Problem::Problem(const Problem::Options& options) : problem_impl_(new internal::ProblemImpl(options)) {} @@ -156,6 +154,14 @@ void Problem::AddParameterBlock(double* values, problem_impl_->AddParameterBlock(values, size, local_parameterization); } +void Problem::RemoveResidualBlock(ResidualBlockId residual_block) { + problem_impl_->RemoveResidualBlock(residual_block); +} + +void Problem::RemoveParameterBlock(double* values) { + problem_impl_->RemoveParameterBlock(values); +} + void Problem::SetParameterBlockConstant(double* values) { problem_impl_->SetParameterBlockConstant(values); } diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc index e9d23ece0..6154ddf65 100644 --- a/internal/ceres/problem_impl.cc +++ b/internal/ceres/problem_impl.cc @@ -118,12 +118,58 @@ ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, } } - ParameterBlock* new_parameter_block = new ParameterBlock(values, size); + // 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 = + new ParameterBlock(values, size, program_->parameter_blocks_.size()); + + // For dynamic problems, add the list of dependent residual blocks, which is + // empty to start. + if (options_.enable_fast_parameter_block_removal) { + new_parameter_block->EnableResidualBlockDependencies(); + } parameter_block_map_[values] = new_parameter_block; program_->parameter_blocks_.push_back(new_parameter_block); return new_parameter_block; } +// Deletes the residual block in question, assuming there are no other +// references to it inside the problem (e.g. by another parameter). Referenced +// cost and loss functions 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(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. + if (options_.cost_function_ownership == TAKE_OWNERSHIP && + residual_block->cost_function() != NULL) { + cost_functions_to_delete_.push_back( + const_cast(residual_block->cost_function())); + } + if (options_.loss_function_ownership == TAKE_OWNERSHIP && + residual_block->loss_function() != NULL) { + loss_functions_to_delete_.push_back( + const_cast(residual_block->loss_function())); + } + delete 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() != NULL) { + local_parameterizations_to_delete_.push_back( + parameter_block->mutable_local_parameterization()); + } + parameter_block_map_.erase(parameter_block->mutable_user_state()); + delete parameter_block; +} + ProblemImpl::ProblemImpl() : program_(new internal::Program) {} ProblemImpl::ProblemImpl(const Problem::Options& options) : options_(options), @@ -132,54 +178,27 @@ ProblemImpl::ProblemImpl(const Problem::Options& options) ProblemImpl::~ProblemImpl() { // Collect the unique cost/loss functions and delete the residuals. const int num_residual_blocks = program_->residual_blocks_.size(); - - vector cost_functions; - cost_functions.reserve(num_residual_blocks); - - vector loss_functions; - loss_functions.reserve(num_residual_blocks); - + cost_functions_to_delete_.reserve(num_residual_blocks); + loss_functions_to_delete_.reserve(num_residual_blocks); for (int i = 0; i < program_->residual_blocks_.size(); ++i) { - ResidualBlock* residual_block = program_->residual_blocks_[i]; - - // 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. - if (options_.cost_function_ownership == TAKE_OWNERSHIP) { - cost_functions.push_back( - const_cast(residual_block->cost_function())); - } - if (options_.loss_function_ownership == TAKE_OWNERSHIP) { - loss_functions.push_back( - const_cast(residual_block->loss_function())); - } - - delete residual_block; + DeleteBlock(program_->residual_blocks_[i]); } // Collect the unique parameterizations and delete the parameters. - vector 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.push_back( - parameter_block->local_parameterization_); - } - - delete parameter_block; + DeleteBlock(program_->parameter_blocks_[i]); } // Delete the owned cost/loss functions and parameterizations. - STLDeleteUniqueContainerPointers(local_parameterizations.begin(), - local_parameterizations.end()); - STLDeleteUniqueContainerPointers(cost_functions.begin(), - cost_functions.end()); - STLDeleteUniqueContainerPointers(loss_functions.begin(), - loss_functions.end()); + STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), + local_parameterizations_to_delete_.end()); + STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(), + cost_functions_to_delete_.end()); + STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(), + loss_functions_to_delete_.end()); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, const vector& parameter_blocks) { @@ -238,14 +257,23 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( ResidualBlock* new_residual_block = new ResidualBlock(cost_function, loss_function, - parameter_block_ptrs); + parameter_block_ptrs, + program_->residual_blocks_.size()); + + // Add dependencies on the residual to the parameter blocks. + if (options_.enable_fast_parameter_block_removal) { + for (int i = 0; i < parameter_blocks.size(); ++i) { + parameter_block_ptrs[i]->AddResidualBlock(new_residual_block); + } + } + program_->residual_blocks_.push_back(new_residual_block); return new_residual_block; } // Unfortunately, macros don't help much to reduce this code, and var args don't // work because of the ambiguous case that there is no loss function. -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0) { @@ -254,7 +282,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1) { @@ -264,7 +292,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2) { @@ -275,7 +303,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3) { @@ -287,7 +315,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3, double* x4) { @@ -300,7 +328,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { @@ -314,7 +342,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, @@ -330,7 +358,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, @@ -347,7 +375,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, @@ -365,7 +393,7 @@ const ResidualBlock* ProblemImpl::AddResidualBlock( return AddResidualBlock(cost_function, loss_function, residual_parameters); } -const ResidualBlock* ProblemImpl::AddResidualBlock( +ResidualBlock* ProblemImpl::AddResidualBlock( CostFunction* cost_function, LossFunction* loss_function, double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, @@ -399,6 +427,77 @@ void ProblemImpl::AddParameterBlock( } } +// Delete a block from a vector of blocks, maintaining the indexing invariant. +// This is done in constant time by moving an element from the end of the +// vector over the element to remove, then popping the last element. It +// destroys the ordering in the interest of speed. +template +void ProblemImpl::DeleteBlockInVector(vector* mutable_blocks, + Block* block_to_remove) { + CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) + << "You found a Ceres bug! Block: " << block_to_remove->ToString(); + + // Prepare the to-be-moved block for the new, lower-in-index position by + // setting the index to the blocks final location. + Block* tmp = mutable_blocks->back(); + tmp->set_index(block_to_remove->index()); + + // Overwrite the to-be-deleted residual block with the one at the end. + (*mutable_blocks)[block_to_remove->index()] = tmp; + + DeleteBlock(block_to_remove); + + // The block is gone so shrink the vector of blocks accordingly. + mutable_blocks->pop_back(); +} + +void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { + CHECK_NOTNULL(residual_block); + + // If needed, remove the parameter dependencies on this residual block. + if (options_.enable_fast_parameter_block_removal) { + const int num_parameter_blocks_for_residual = + residual_block->NumParameterBlocks(); + for (int i = 0; i < num_parameter_blocks_for_residual; ++i) { + residual_block->parameter_blocks()[i] + ->RemoveResidualBlock(residual_block); + } + } + DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); +} + +void ProblemImpl::RemoveParameterBlock(double* values) { + ParameterBlock* parameter_block = FindOrDie(parameter_block_map_, values); + + if (options_.enable_fast_parameter_block_removal) { + // Copy the dependent residuals from the parameter block because the set of + // dependents will change after each call to RemoveResidualBlock(). + vector 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) { + RemoveResidualBlock(residual_blocks_to_remove[i]); + } + } else { + // Scan all the residual blocks to remove ones that depend on the parameter + // block. Do the scan backwards since the vector changes while iterating. + const int num_residual_blocks = NumResidualBlocks(); + for (int i = num_residual_blocks - 1; i >= 0; --i) { + ResidualBlock* residual_block = + (*(program_->mutable_residual_blocks()))[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int i = 0; i < num_parameter_blocks; ++i) { + if (residual_block->parameter_blocks()[i] == parameter_block) { + RemoveResidualBlock(residual_block); + // The parameter blocks are guaranteed unique. + break; + } + } + } + } + DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block); +} + void ProblemImpl::SetParameterBlockConstant(double* values) { FindOrDie(parameter_block_map_, values)->SetConstant(); } diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h index 82a19561c..536e73a12 100644 --- a/internal/ceres/problem_impl.h +++ b/internal/ceres/problem_impl.h @@ -118,6 +118,10 @@ class ProblemImpl { void AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization); + + void RemoveResidualBlock(ResidualBlock* residual_block); + void RemoveParameterBlock(double* values); + void SetParameterBlockConstant(double* values); void SetParameterBlockVariable(double* values); void SetParameterization(double* values, @@ -135,12 +139,33 @@ class ProblemImpl { private: ParameterBlock* InternalAddParameterBlock(double* values, int size); + // Delete the arguments in question. These differ from the Remove* functions + // in that they do not clean up references to the block to delete; they + // merely delete them. + template + void DeleteBlockInVector(vector* mutable_blocks, + Block* block_to_remove); + void DeleteBlock(ResidualBlock* residual_block); + void DeleteBlock(ParameterBlock* parameter_block); + const Problem::Options options_; // The mapping from user pointers to parameter blocks. map parameter_block_map_; + // The actual parameter and residual blocks. internal::scoped_ptr program_; + + // When removing residual and parameter blocks, cost/loss functions and + // parameterizations have ambiguous ownership. Instead of scanning the entire + // problem to see if the cost/loss/parameterization is shared with other + // residual or parameter blocks, buffer them until destruction. + // + // TODO(keir): See if it makes sense to use sets instead. + vector cost_functions_to_delete_; + vector loss_functions_to_delete_; + vector local_parameterizations_to_delete_; + CERES_DISALLOW_COPY_AND_ASSIGN(ProblemImpl); }; diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc index 4afe1b55a..55f355bde 100644 --- a/internal/ceres/problem_test.cc +++ b/internal/ceres/problem_test.cc @@ -30,10 +30,14 @@ // keir@google.com (Keir Mierle) #include "ceres/problem.h" +#include "ceres/problem_impl.h" #include "gtest/gtest.h" #include "ceres/cost_function.h" #include "ceres/local_parameterization.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" #include "ceres/sized_cost_function.h" #include "ceres/internal/scoped_ptr.h" @@ -293,11 +297,11 @@ TEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) { class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> { public: - explicit DestructorCountingCostFunction(int *counter) - : counter_(counter) {} + explicit DestructorCountingCostFunction(int *num_destructions) + : num_destructions_(num_destructions) {} virtual ~DestructorCountingCostFunction() { - *counter_ += 1; + *num_destructions_ += 1; } virtual bool Evaluate(double const* const* parameters, @@ -307,12 +311,12 @@ class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> { } private: - int* counter_; + int* num_destructions_; }; TEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) { double y[4], z[5]; - int counter = 0; + int num_destructions = 0; // Add a cost function multiple times and check to make sure that // the destructor on the cost function is only called once. @@ -321,15 +325,375 @@ TEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) { problem.AddParameterBlock(y, 4); problem.AddParameterBlock(z, 5); - CostFunction* cost = new DestructorCountingCostFunction(&counter); + CostFunction* cost = new DestructorCountingCostFunction(&num_destructions); problem.AddResidualBlock(cost, NULL, y, z); problem.AddResidualBlock(cost, NULL, y, z); problem.AddResidualBlock(cost, NULL, y, z); + EXPECT_EQ(3, problem.NumResidualBlocks()); } // Check that the destructor was called only once. - CHECK_EQ(counter, 1); + CHECK_EQ(num_destructions, 1); } +TEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) { + double y[4], z[5], w[4]; + int num_destructions = 0; + { + Problem problem; + problem.AddParameterBlock(y, 4); + problem.AddParameterBlock(z, 5); + + CostFunction* cost_yz = + new DestructorCountingCostFunction(&num_destructions); + CostFunction* cost_wz = + new DestructorCountingCostFunction(&num_destructions); + ResidualBlock* r_yz = problem.AddResidualBlock(cost_yz, NULL, y, z); + ResidualBlock* r_wz = problem.AddResidualBlock(cost_wz, NULL, w, z); + EXPECT_EQ(2, problem.NumResidualBlocks()); + + // In the current implementation, the destructor shouldn't get run yet. + problem.RemoveResidualBlock(r_yz); + CHECK_EQ(num_destructions, 0); + problem.RemoveResidualBlock(r_wz); + CHECK_EQ(num_destructions, 0); + + EXPECT_EQ(0, problem.NumResidualBlocks()); + } + CHECK_EQ(num_destructions, 2); +} + +// Make the dynamic problem tests (e.g. for removing residual blocks) +// parameterized on whether the low-latency mode is enabled or not. +// +// This tests against ProblemImpl instead of Problem in order to inspect the +// state of the resulting Program; this is difficult with only the thin Problem +// interface. +struct DynamicProblem : public ::testing::TestWithParam { + DynamicProblem() { + Problem::Options options; + options.enable_fast_parameter_block_removal = GetParam(); + problem.reset(new ProblemImpl(options)); + } + + ParameterBlock* GetParameterBlock(int block) { + return problem->program().parameter_blocks()[block]; + } + ResidualBlock* GetResidualBlock(int block) { + return problem->program().residual_blocks()[block]; + } + + bool HasResidualBlock(ResidualBlock* residual_block) { + return find(problem->program().residual_blocks().begin(), + problem->program().residual_blocks().end(), + residual_block) != problem->program().residual_blocks().end(); + } + + // The next block of functions until the end are only for testing the + // residual block removals. + void ExpectParameterBlockContainsResidualBlock( + double* values, + ResidualBlock* residual_block) { + ParameterBlock* parameter_block = + FindOrDie(problem->parameter_map(), values); + EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()), + residual_block)); + } + + void ExpectSize(double* values, int size) { + ParameterBlock* parameter_block = + FindOrDie(problem->parameter_map(), values); + EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size()); + } + + // Degenerate case. + void ExpectParameterBlockContains(double* values) { + ExpectSize(values, 0); + } + + void ExpectParameterBlockContains(double* values, + ResidualBlock* r1) { + ExpectSize(values, 1); + ExpectParameterBlockContainsResidualBlock(values, r1); + } + + void ExpectParameterBlockContains(double* values, + ResidualBlock* r1, + ResidualBlock* r2) { + ExpectSize(values, 2); + ExpectParameterBlockContainsResidualBlock(values, r1); + ExpectParameterBlockContainsResidualBlock(values, r2); + } + + void ExpectParameterBlockContains(double* values, + ResidualBlock* r1, + ResidualBlock* r2, + ResidualBlock* r3) { + ExpectSize(values, 3); + ExpectParameterBlockContainsResidualBlock(values, r1); + ExpectParameterBlockContainsResidualBlock(values, r2); + ExpectParameterBlockContainsResidualBlock(values, r3); + } + + void ExpectParameterBlockContains(double* values, + ResidualBlock* r1, + ResidualBlock* r2, + ResidualBlock* r3, + ResidualBlock* r4) { + ExpectSize(values, 4); + ExpectParameterBlockContainsResidualBlock(values, r1); + ExpectParameterBlockContainsResidualBlock(values, r2); + ExpectParameterBlockContainsResidualBlock(values, r3); + ExpectParameterBlockContainsResidualBlock(values, r4); + } + + scoped_ptr problem; + double y[4], z[5], w[3]; +}; + +TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { + problem->AddParameterBlock(y, 4); + problem->AddParameterBlock(z, 5); + problem->AddParameterBlock(w, 3); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(y, GetParameterBlock(0)->user_state()); + EXPECT_EQ(z, GetParameterBlock(1)->user_state()); + EXPECT_EQ(w, GetParameterBlock(2)->user_state()); + + // w is at the end, which might break the swapping logic so try adding and + // removing it. + problem->RemoveParameterBlock(w); + ASSERT_EQ(2, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(y, GetParameterBlock(0)->user_state()); + EXPECT_EQ(z, GetParameterBlock(1)->user_state()); + problem->AddParameterBlock(w, 3); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(y, GetParameterBlock(0)->user_state()); + EXPECT_EQ(z, GetParameterBlock(1)->user_state()); + EXPECT_EQ(w, GetParameterBlock(2)->user_state()); + + // Now remove z, which is in the middle, and add it back. + problem->RemoveParameterBlock(z); + ASSERT_EQ(2, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(y, GetParameterBlock(0)->user_state()); + EXPECT_EQ(w, GetParameterBlock(1)->user_state()); + problem->AddParameterBlock(z, 5); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(y, GetParameterBlock(0)->user_state()); + EXPECT_EQ(w, GetParameterBlock(1)->user_state()); + EXPECT_EQ(z, GetParameterBlock(2)->user_state()); + + // Now remove everything. + // y + problem->RemoveParameterBlock(y); + ASSERT_EQ(2, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(z, GetParameterBlock(0)->user_state()); + EXPECT_EQ(w, GetParameterBlock(1)->user_state()); + + // z + problem->RemoveParameterBlock(z); + ASSERT_EQ(1, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(w, GetParameterBlock(0)->user_state()); + + // w + problem->RemoveParameterBlock(w); + EXPECT_EQ(0, problem->NumParameterBlocks()); + EXPECT_EQ(0, problem->NumResidualBlocks()); +} + +TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { + problem->AddParameterBlock(y, 4); + problem->AddParameterBlock(z, 5); + problem->AddParameterBlock(w, 3); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + EXPECT_EQ(y, GetParameterBlock(0)->user_state()); + EXPECT_EQ(z, GetParameterBlock(1)->user_state()); + EXPECT_EQ(w, GetParameterBlock(2)->user_state()); + + // Add all combinations of cost functions. + CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); + CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); + CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); + CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); + CostFunction* cost_y = new UnaryCostFunction (1, 4); + CostFunction* cost_z = new UnaryCostFunction (1, 5); + CostFunction* cost_w = new UnaryCostFunction (1, 3); + + ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); + ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); + ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); + ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); + ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); + ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); + ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); + + EXPECT_EQ(3, problem->NumParameterBlocks()); + EXPECT_EQ(7, problem->NumResidualBlocks()); + + // Remove w, which should remove r_yzw, r_yw, r_zw, r_w. + problem->RemoveParameterBlock(w); + ASSERT_EQ(2, problem->NumParameterBlocks()); + ASSERT_EQ(3, problem->NumResidualBlocks()); + + ASSERT_FALSE(HasResidualBlock(r_yzw)); + ASSERT_TRUE (HasResidualBlock(r_yz )); + ASSERT_FALSE(HasResidualBlock(r_yw )); + ASSERT_FALSE(HasResidualBlock(r_zw )); + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_TRUE (HasResidualBlock(r_z )); + ASSERT_FALSE(HasResidualBlock(r_w )); + + // Remove z, which will remove almost everything else. + problem->RemoveParameterBlock(z); + ASSERT_EQ(1, problem->NumParameterBlocks()); + ASSERT_EQ(1, problem->NumResidualBlocks()); + + ASSERT_FALSE(HasResidualBlock(r_yzw)); + ASSERT_FALSE(HasResidualBlock(r_yz )); + ASSERT_FALSE(HasResidualBlock(r_yw )); + ASSERT_FALSE(HasResidualBlock(r_zw )); + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_FALSE(HasResidualBlock(r_z )); + ASSERT_FALSE(HasResidualBlock(r_w )); + + // Remove y; all gone. + problem->RemoveParameterBlock(y); + EXPECT_EQ(0, problem->NumParameterBlocks()); + EXPECT_EQ(0, problem->NumResidualBlocks()); +} + +TEST_P(DynamicProblem, RemoveResidualBlock) { + problem->AddParameterBlock(y, 4); + problem->AddParameterBlock(z, 5); + problem->AddParameterBlock(w, 3); + + // Add all combinations of cost functions. + CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); + CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); + CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); + CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); + CostFunction* cost_y = new UnaryCostFunction (1, 4); + CostFunction* cost_z = new UnaryCostFunction (1, 5); + CostFunction* cost_w = new UnaryCostFunction (1, 3); + + ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); + ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); + ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); + ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); + ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); + ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); + ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); + + if (GetParam()) { + // In this test parameterization, there should be back-pointers from the + // parameter blocks to the residual blocks. + ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y); + ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z); + ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w); + } else { + // Otherwise, nothing. + EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL); + EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL); + EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL); + } + EXPECT_EQ(3, problem->NumParameterBlocks()); + EXPECT_EQ(7, problem->NumResidualBlocks()); + + // Remove each residual and check the state after each removal. + + // Remove r_yzw. + problem->RemoveResidualBlock(r_yzw); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(6, problem->NumResidualBlocks()); + if (GetParam()) { + ExpectParameterBlockContains(y, r_yz, r_yw, r_y); + ExpectParameterBlockContains(z, r_yz, r_zw, r_z); + ExpectParameterBlockContains(w, r_yw, r_zw, r_w); + } + ASSERT_TRUE (HasResidualBlock(r_yz )); + ASSERT_TRUE (HasResidualBlock(r_yw )); + ASSERT_TRUE (HasResidualBlock(r_zw )); + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_TRUE (HasResidualBlock(r_z )); + ASSERT_TRUE (HasResidualBlock(r_w )); + + // Remove r_yw. + problem->RemoveResidualBlock(r_yw); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(5, problem->NumResidualBlocks()); + if (GetParam()) { + ExpectParameterBlockContains(y, r_yz, r_y); + ExpectParameterBlockContains(z, r_yz, r_zw, r_z); + ExpectParameterBlockContains(w, r_zw, r_w); + } + ASSERT_TRUE (HasResidualBlock(r_yz )); + ASSERT_TRUE (HasResidualBlock(r_zw )); + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_TRUE (HasResidualBlock(r_z )); + ASSERT_TRUE (HasResidualBlock(r_w )); + + // Remove r_zw. + problem->RemoveResidualBlock(r_zw); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(4, problem->NumResidualBlocks()); + if (GetParam()) { + ExpectParameterBlockContains(y, r_yz, r_y); + ExpectParameterBlockContains(z, r_yz, r_z); + ExpectParameterBlockContains(w, r_w); + } + ASSERT_TRUE (HasResidualBlock(r_yz )); + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_TRUE (HasResidualBlock(r_z )); + ASSERT_TRUE (HasResidualBlock(r_w )); + + // Remove r_w. + problem->RemoveResidualBlock(r_w); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(3, problem->NumResidualBlocks()); + if (GetParam()) { + ExpectParameterBlockContains(y, r_yz, r_y); + ExpectParameterBlockContains(z, r_yz, r_z); + ExpectParameterBlockContains(w); + } + ASSERT_TRUE (HasResidualBlock(r_yz )); + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_TRUE (HasResidualBlock(r_z )); + + // Remove r_yz. + problem->RemoveResidualBlock(r_yz); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(2, problem->NumResidualBlocks()); + if (GetParam()) { + ExpectParameterBlockContains(y, r_y); + ExpectParameterBlockContains(z, r_z); + ExpectParameterBlockContains(w); + } + ASSERT_TRUE (HasResidualBlock(r_y )); + ASSERT_TRUE (HasResidualBlock(r_z )); + + // Remove the last two. + problem->RemoveResidualBlock(r_z); + problem->RemoveResidualBlock(r_y); + ASSERT_EQ(3, problem->NumParameterBlocks()); + ASSERT_EQ(0, problem->NumResidualBlocks()); + if (GetParam()) { + ExpectParameterBlockContains(y); + ExpectParameterBlockContains(z); + ExpectParameterBlockContains(w); + } +} + +INSTANTIATE_TEST_CASE_P(OptionsInstantiation, + DynamicProblem, + ::testing::Values(true, false)); + } // namespace internal } // namespace ceres diff --git a/internal/ceres/residual_block.cc b/internal/ceres/residual_block.cc index bdb88b1dd..7f789605e 100644 --- a/internal/ceres/residual_block.cc +++ b/internal/ceres/residual_block.cc @@ -49,12 +49,14 @@ namespace internal { ResidualBlock::ResidualBlock(const CostFunction* cost_function, const LossFunction* loss_function, - const vector& parameter_blocks) + const vector& parameter_blocks, + int index) : cost_function_(cost_function), loss_function_(loss_function), parameter_blocks_( new ParameterBlock* [ - cost_function->parameter_block_sizes().size()]) { + cost_function->parameter_block_sizes().size()]), + index_(index) { std::copy(parameter_blocks.begin(), parameter_blocks.end(), parameter_blocks_.get()); diff --git a/internal/ceres/residual_block.h b/internal/ceres/residual_block.h index e0a06e789..3921d1d46 100644 --- a/internal/ceres/residual_block.h +++ b/internal/ceres/residual_block.h @@ -34,11 +34,13 @@ #ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ #define CERES_INTERNAL_RESIDUAL_BLOCK_H_ +#include #include #include "ceres/cost_function.h" #include "ceres/internal/port.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/stringprintf.h" #include "ceres/types.h" namespace ceres { @@ -64,9 +66,13 @@ class ParameterBlock; // loss functions, and parameter blocks. class ResidualBlock { public: + // Construct the residual block with the given cost/loss functions. Loss may + // be null. The index is the index of the residual block in the Program's + // residual_blocks array. ResidualBlock(const CostFunction* cost_function, const LossFunction* loss_function, - const vector& parameter_blocks); + const vector& parameter_blocks, + int index); // Evaluates the residual term, storing the scalar cost in *cost, the residual // components in *residuals, and the jacobians between the parameters and @@ -112,10 +118,23 @@ class ResidualBlock { // The minimum amount of scratch space needed to pass to Evaluate(). int NumScratchDoublesForEvaluate() const; + // This residual block's index in an array. + int index() const { return index_; } + void set_index(int index) { index_ = index; } + + string ToString() { + return StringPrintf("{residual block; index=%d}", index_); + } + private: const CostFunction* cost_function_; const LossFunction* loss_function_; scoped_array parameter_blocks_; + + // The index of the residual, typically in a Program. This is only to permit + // switching from a ResidualBlock* to an index in the Program's array, needed + // to do efficient removals. + int32 index_; }; } // namespace internal diff --git a/internal/ceres/residual_block_test.cc b/internal/ceres/residual_block_test.cc index 92b79f6bf..fddd44ea7 100644 --- a/internal/ceres/residual_block_test.cc +++ b/internal/ceres/residual_block_test.cc @@ -77,13 +77,13 @@ TEST(ResidualBlock, EvaluteWithNoLossFunctionOrLocalParameterizations) { // Prepare the parameter blocks. double values_x[2]; - ParameterBlock x(values_x, 2); + ParameterBlock x(values_x, 2, -1); double values_y[3]; - ParameterBlock y(values_y, 3); + ParameterBlock y(values_y, 3, -1); double values_z[4]; - ParameterBlock z(values_z, 4); + ParameterBlock z(values_z, 4, -1); vector parameters; parameters.push_back(&x); @@ -93,7 +93,7 @@ TEST(ResidualBlock, EvaluteWithNoLossFunctionOrLocalParameterizations) { TernaryCostFunction cost_function(3, 2, 3, 4); // Create the object under tests. - ResidualBlock residual_block(&cost_function, NULL, parameters); + ResidualBlock residual_block(&cost_function, NULL, parameters, -1); // Verify getters. EXPECT_EQ(&cost_function, residual_block.cost_function()); @@ -204,13 +204,13 @@ TEST(ResidualBlock, EvaluteWithLocalParameterizations) { // Prepare the parameter blocks. double values_x[2]; - ParameterBlock x(values_x, 2); + ParameterBlock x(values_x, 2, -1); double values_y[3]; - ParameterBlock y(values_y, 3); + ParameterBlock y(values_y, 3, -1); double values_z[4]; - ParameterBlock z(values_z, 4); + ParameterBlock z(values_z, 4, -1); vector parameters; parameters.push_back(&x); @@ -232,7 +232,7 @@ TEST(ResidualBlock, EvaluteWithLocalParameterizations) { LocallyParameterizedCostFunction cost_function; // Create the object under tests. - ResidualBlock residual_block(&cost_function, NULL, parameters); + ResidualBlock residual_block(&cost_function, NULL, parameters, -1); // Verify getters. EXPECT_EQ(&cost_function, residual_block.cost_function()); diff --git a/internal/ceres/residual_block_utils_test.cc b/internal/ceres/residual_block_utils_test.cc index db9ad6d06..24723b323 100644 --- a/internal/ceres/residual_block_utils_test.cc +++ b/internal/ceres/residual_block_utils_test.cc @@ -45,13 +45,14 @@ namespace internal { // with one residual succeeds with true or dies. void CheckEvaluation(const CostFunction& cost_function, bool is_good) { double x = 1.0; - ParameterBlock parameter_block(&x, 1); + ParameterBlock parameter_block(&x, 1, -1); vector parameter_blocks; parameter_blocks.push_back(¶meter_block); ResidualBlock residual_block(&cost_function, NULL, - parameter_blocks); + parameter_blocks, + -1); scoped_array scratch( new double[residual_block.NumScratchDoublesForEvaluate()]);