From 1209857a3320c5e489e02edd2030923e0916dbbf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Jun 2021 20:08:31 -0400 Subject: [PATCH 1/2] fix bug in LinearContainerFactor and warnings about Point3 --- gtsam/nonlinear/LinearContainerFactor.cpp | 3 ++- gtsam/nonlinear/tests/testLinearContainerFactor.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index d715eb5c7..9ee0681d2 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 2afa2b5fc..73bbdf55a 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); From f1bed302e4bed852d373e5930b674a62b11a1956 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 7 Jun 2021 10:23:15 -0400 Subject: [PATCH 2/2] 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); } /* ************************************************************************* */