From f1bed302e4bed852d373e5930b674a62b11a1956 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 7 Jun 2021 10:23:15 -0400 Subject: [PATCH] added test for this issue --- .../tests/testLinearContainerFactor.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 73bbdf55a..a5015546f 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -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); } /* ************************************************************************* */