From 1209857a3320c5e489e02edd2030923e0916dbbf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Jun 2021 20:08:31 -0400 Subject: [PATCH] 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);