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);