Merge pull request #139 from MOLAorg/fix/monothonic-kf-ids

Fix: ensure monothonic KF ids

Author: jlblancoc

mola_metric_maps/include/mola_metric_maps/KeyframePointCloudMap.h

@@ -486,6 +486,12 @@ class KeyframePointCloudMap : public mrpt::maps::CMetricMap,
   /// KF ids evicted since the last call to drainEvictedKeyFrameIDs().
   std::vector<KeyFrameID> evicted_kf_ids_;
 
+  /// Monotonically increasing KF id allocator. Incremented on every
+  /// insertion, never decremented on eviction, so ids are never reused.
+  /// Reset to 0 on internal_clear(); restored to max-loaded-id+1 in
+  /// serializeFrom().
+  KeyFrameID next_free_kf_id_ = 0;
+
   /// for cached_ and _keyframes
   mutable mrpt::containers::NonCopiableData<std::recursive_mutex> state_mtx_;
 
@@ -546,15 +552,10 @@ class KeyframePointCloudMap : public mrpt::maps::CMetricMap,
     return {kf_idx, local_pt_idx};
   }
 
-  /// Return the next key-frame ID, which is the size of the map:
-  [[nodiscard]] KeyFrameID nextFreeKeyFrameID() const
-  {
-    if (keyframes_.empty())
-    {
-      return 0;  // First key-frame
-    }
-    return keyframes_.rbegin()->first + 1;  // Next ID
-  }
+  /// Returns the next monotonically-allocated key-frame ID. The id is NOT
+  /// consumed by this call; callers must use it as the key for the new KF
+  /// and then bump `next_free_kf_id_`.
+  [[nodiscard]] KeyFrameID nextFreeKeyFrameID() const { return next_free_kf_id_; }
 };
 
 }  // namespace mola

mola_metric_maps/src/KeyframePointCloudMap.cpp

@@ -195,6 +195,10 @@ void KeyframePointCloudMap::serializeFrom(mrpt::serialization::CArchive& in, uin
       MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
   };
 
+  // Restore the monotonic id counter so future insertions don't collide
+  // with already-loaded ids.
+  next_free_kf_id_ = keyframes_.empty() ? 0 : (keyframes_.rbegin()->first + 1);
+
   // cache reset:
   cached_.reset();
 }
@@ -545,7 +549,7 @@ void KeyframePointCloudMap::merge_with(
       continue;
     }
     auto [it, isNew] =
-        keyframes_.emplace(nextFreeKeyFrameID(), creationOptions.k_correspondences_for_cov);
+        keyframes_.emplace(next_free_kf_id_++, creationOptions.k_correspondences_for_cov);
     auto& new_kf = it->second;
 
     // copy
@@ -1165,6 +1169,7 @@ void KeyframePointCloudMap::internal_clear()
   keyframes_.clear();
   last_inserted_kf_id_.reset();
   evicted_kf_ids_.clear();
+  next_free_kf_id_ = 0;
   cached_.reset();
 }
 
@@ -1244,9 +1249,9 @@ bool KeyframePointCloudMap::internal_insertObservation(
   {
     ASSERT_(obsPC->pointcloud);
 
-    // Add KF:
+    // Add KF: allocate a fresh monotonic id (never reused, even after eviction).
     auto [it, isNew] =
-        keyframes_.emplace(nextFreeKeyFrameID(), creationOptions.k_correspondences_for_cov);
+        keyframes_.emplace(next_free_kf_id_++, creationOptions.k_correspondences_for_cov);
     auto& new_kf = it->second;
 
     new_kf.timestamp = obs.timestamp;