diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index d715eb5c78..9ee0681d2a 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -175,9 +175,10 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( Values newLinearizationPoint; for (size_t i = 0; i < factor_->size(); ++i) { auto mapping = rekey_mapping.find(factor_->keys()[i]); - if (mapping != rekey_mapping.end()) + if (mapping != rekey_mapping.end()) { new_factor->factor_->keys()[i] = mapping->second; newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first)); + } } new_factor->linearizationPoint_ = newLinearizationPoint; diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 2afa2b5fc9..a5015546f5 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -326,13 +326,13 @@ TEST(TestLinearContainerFactor, Rekey) { // Make an example factor auto nonlinear_factor = boost::make_shared>( - gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(), + gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0), gtsam::noiseModel::Isotropic::Sigma(3, 1)); // Linearize and create an LCF gtsam::Values linearization_pt; - linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3()); - linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3()); + linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0)); + linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0)); LinearContainerFactor lcf_factor( nonlinear_factor->linearize(linearization_pt), linearization_pt); @@ -363,6 +363,34 @@ TEST(TestLinearContainerFactor, Rekey) { CHECK(assert_equal(linearization_pt_rekeyed.keys()[0], lcf_factor_rekey_ptr->keys()[1])); } +/* ************************************************************************* */ +TEST(TestLinearContainerFactor, Rekey2) { + // Make an example factor + auto nonlinear_factor = + boost::make_shared>( + gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0), + gtsam::noiseModel::Isotropic::Sigma(3, 1)); + + // Linearize and create an LCF + gtsam::Values linearization_pt; + linearization_pt.insert(gtsam::Symbol('x', 0), gtsam::Point3(0, 0, 0)); + linearization_pt.insert(gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0)); + + LinearContainerFactor lcf_factor( + nonlinear_factor->linearize(linearization_pt), linearization_pt); + + // Define a key mapping with only a single key remapped. + // This should throw an exception if there is a bug. + std::map key_map; + key_map[gtsam::Symbol('x', 0)] = gtsam::Symbol('x', 4); + + // Cast back to LCF ptr + LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr = + boost::static_pointer_cast( + lcf_factor.rekey(key_map)); + CHECK(lcf_factor_rekey_ptr); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */