diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 160423a64c..56a2eac653 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -171,16 +171,24 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( // Update the keys to the properties as well // Downncast so we have access to members auto new_factor = std::static_pointer_cast(rekeyed_base_factor); - // Create a new Values to assign later - Values newLinearizationPoint; + // Rekey the underlying linear factor for (size_t i = 0; i < factor_->size(); ++i) { auto mapping = rekey_mapping.find(factor_->keys()[i]); if (mapping != rekey_mapping.end()) { new_factor->factor_->keys()[i] = mapping->second; - newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first)); } } - new_factor->linearizationPoint_ = newLinearizationPoint; + // Rekey the linearization point only if it exists + if (linearizationPoint_) { + Values newLinearizationPoint; + for (size_t i = 0; i < factor_->size(); ++i) { + auto mapping = rekey_mapping.find(factor_->keys()[i]); + if (mapping != rekey_mapping.end()) { + newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first)); + } + } + new_factor->linearizationPoint_ = newLinearizationPoint; + } // upcast back and return return std::static_pointer_cast(new_factor); @@ -194,13 +202,15 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( // Downncast so we have access to members auto new_factor = std::static_pointer_cast(rekeyed_base_factor); new_factor->factor_->keys() = new_factor->keys(); - // Create a new Values to assign later - Values newLinearizationPoint; - for(size_t i=0; ikeys()[i]; - newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key)); + // Rekey the linearization point only if it exists + if (linearizationPoint_) { + Values newLinearizationPoint; + for (size_t i = 0; i < new_keys.size(); ++i) { + Key cur_key = linearizationPoint_->keys()[i]; + newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key)); + } + new_factor->linearizationPoint_ = newLinearizationPoint; } - new_factor->linearizationPoint_ = newLinearizationPoint; // upcast back and return return std::static_pointer_cast(new_factor); diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index db1f6d0582..33df107b19 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -389,6 +389,32 @@ TEST(TestLinearContainerFactor, Rekey2) { CHECK(lcf_factor_rekey_ptr); } +/* ************************************************************************* */ +// Test that rekey works without a linearization point (issue #1904) +TEST(TestLinearContainerFactor, RekeyWithoutLinearizationPoint) { + double mu = 1e10; + size_t dim = 3; + Key key = 0; + + HessianFactor H(key, mu * Matrix::Identity(dim, dim), Vector::Zero(dim), + 0.0); + LinearContainerFactor factor(H); + + // Rekey with map + Key new_key = 1; + std::map rekey_mapping = {{key, new_key}}; + auto rekeyed = factor.rekey(rekey_mapping); + CHECK(rekeyed); + EXPECT(rekeyed->keys()[0] == new_key); + + // Rekey with vector + Key new_key2 = 2; + KeyVector rekey_vector = {new_key2}; + auto rekeyed2 = factor.rekey(rekey_vector); + CHECK(rekeyed2); + EXPECT(rekeyed2->keys()[0] == new_key2); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */