Fix LinearContainerFactor::rekey() segfault without linearization point

gtsam/nonlinear/LinearContainerFactor.cpp

@@ -171,16 +171,24 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
   // Update the keys to the properties as well
   // Downncast so we have access to members
   auto new_factor = std::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
-  // Create a new Values to assign later
-  Values newLinearizationPoint;
+  // Rekey the underlying linear factor
   for (size_t i = 0; i < factor_->size(); ++i) {
     auto mapping = rekey_mapping.find(factor_->keys()[i]);
     if (mapping != rekey_mapping.end()) {
       new_factor->factor_->keys()[i] = mapping->second;
-      newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
     }
   }
-  new_factor->linearizationPoint_ = newLinearizationPoint;
+  // Rekey the linearization point only if it exists
+  if (linearizationPoint_) {
+    Values newLinearizationPoint;
+    for (size_t i = 0; i < factor_->size(); ++i) {
+      auto mapping = rekey_mapping.find(factor_->keys()[i]);
+      if (mapping != rekey_mapping.end()) {
+        newLinearizationPoint.insert(mapping->second, linearizationPoint_->at(mapping->first));
+      }
+    }
+    new_factor->linearizationPoint_ = newLinearizationPoint;
+  }
 
   // upcast back and return
   return std::static_pointer_cast<NonlinearFactor>(new_factor);
@@ -194,13 +202,15 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
   // Downncast so we have access to members
   auto new_factor = std::static_pointer_cast<LinearContainerFactor>(rekeyed_base_factor);
   new_factor->factor_->keys() = new_factor->keys();
-  // Create a new Values to assign later
-  Values newLinearizationPoint;
-  for(size_t i=0; i<new_keys.size(); ++i) {
-    Key cur_key = linearizationPoint_->keys()[i];
-    newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key));
+  // Rekey the linearization point only if it exists
+  if (linearizationPoint_) {
+    Values newLinearizationPoint;
+    for (size_t i = 0; i < new_keys.size(); ++i) {
+      Key cur_key = linearizationPoint_->keys()[i];
+      newLinearizationPoint.insert(new_keys[i], linearizationPoint_->at(cur_key));
+    }
+    new_factor->linearizationPoint_ = newLinearizationPoint;
   }
-  new_factor->linearizationPoint_ = newLinearizationPoint;
 
   // upcast back and return
   return std::static_pointer_cast<NonlinearFactor>(new_factor);

gtsam/nonlinear/tests/testLinearContainerFactor.cpp

@@ -389,6 +389,32 @@ TEST(TestLinearContainerFactor, Rekey2) {
   CHECK(lcf_factor_rekey_ptr);
 }
 
+/* ************************************************************************* */
+// Test that rekey works without a linearization point (issue #1904)
+TEST(TestLinearContainerFactor, RekeyWithoutLinearizationPoint) {
+  double mu = 1e10;
+  size_t dim = 3;
+  Key key = 0;
+
+  HessianFactor H(key, mu * Matrix::Identity(dim, dim), Vector::Zero(dim),
+                   0.0);
+  LinearContainerFactor factor(H);
+
+  // Rekey with map
+  Key new_key = 1;
+  std::map<Key, Key> rekey_mapping = {{key, new_key}};
+  auto rekeyed = factor.rekey(rekey_mapping);
+  CHECK(rekeyed);
+  EXPECT(rekeyed->keys()[0] == new_key);
+
+  // Rekey with vector
+  Key new_key2 = 2;
+  KeyVector rekey_vector = {new_key2};
+  auto rekeyed2 = factor.rekey(rekey_vector);
+  CHECK(rekeyed2);
+  EXPECT(rekeyed2->keys()[0] == new_key2);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */