From 4e9d9acf10ce665cbc5c598f25af8c1a3f6c01a5 Mon Sep 17 00:00:00 2001
From: "k.koide" <k.koide@aist.go.jp>
Date: Tue, 22 Oct 2024 14:23:21 +0900
Subject: [PATCH] fix memory leak

---
 .../incremental_fixed_lag_smoother_with_fallback.hpp        | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp
index b560324f..79dcb2e7 100644
--- a/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp
+++ b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp
@@ -26,13 +26,13 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo
   template <class VALUE>
   VALUE calculateEstimate(Key key) const {
     try {
-      const auto& value = smoother->calculateEstimate(key);
+      const VALUE value = smoother->calculateEstimate<VALUE>(key);
       auto found = values.find(key);
       if (found != values.end()) {
-        found->value = value;
+        values.insert_or_assign(key, value);
       }
 
-      return value.cast<VALUE>();
+      return value;
     } catch (std::exception& e) {
       std::cerr << "warning: an exception was caught in fixed-lag smoother update!!" << std::endl;
       std::cerr << "       : " << e.what() << std::endl;