diff --git a/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.hpp b/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.hpp
index af2f66cff36f281d75dd3564fdb0ddb361038c91..6cf61f8dd552d45eedb20beed9da7b19209383d5 100644
--- a/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.hpp
+++ b/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.hpp
@@ -343,12 +343,14 @@ class AdaptiveMultiVertexFinder {
   /// of reverts to the old state of the vertex fitter before the bad
   /// vertex was added to the fit (if not refitAfterBadVertex).
   ///
+  /// @param vtx The last added vertex which will be removed
   /// @param allVertices Vector containing the unique_ptr to vertices
   /// @param allVerticesPtr Vector containing the actual addresses
   /// @param fitterState The current vertex fitter state
   /// @param oldFitterState The old vertex fitter state
   /// @param vFitterOptions The vertex fitter options
   Result<void> deleteLastVertex(
+      Vertex<InputTrack_t>& vtx,
       std::vector<std::unique_ptr<Vertex<InputTrack_t>>>& allVertices,
       std::vector<Vertex<InputTrack_t>*>& allVerticesPtr,
       FitterState_t& fitterState, FitterState_t& oldFitterState,
diff --git a/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.ipp b/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.ipp
index b89c6509b9217e8cdfaae7aab4979a9e6c589f7e..94d44719c516ba8950390421d2aa013c8a8c0f68 100644
--- a/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.ipp
+++ b/Core/include/Acts/Vertexing/AdaptiveMultiVertexFinder.ipp
@@ -80,7 +80,7 @@ auto Acts::AdaptiveMultiVertexFinder<vfitter_t, sfinder_t>::find(
       break;
     }
     // Update fitter state with all vertices
-    fitterState.updateTrkToVerticesMultiMap(allVerticesPtr);
+    fitterState.addVertexToMultiMap(vtxCandidate);
 
     // Perform the fit
     auto fitResult = m_cfg.vertexFitter.addVtxToFit(
@@ -117,8 +117,8 @@ auto Acts::AdaptiveMultiVertexFinder<vfitter_t, sfinder_t>::find(
     // Delete vertex from allVertices list again if it's not kept
     if (not keepVertex) {
       auto deleteVertexResult =
-          deleteLastVertex(allVertices, allVerticesPtr, fitterState,
-                           oldFitterState, vFitterOptions);
+          deleteLastVertex(vtxCandidate, allVertices, allVerticesPtr,
+                           fitterState, oldFitterState, vFitterOptions);
       if (not deleteVertexResult.ok()) {
         return deleteVertexResult.error();
       }
@@ -514,6 +514,7 @@ auto Acts::AdaptiveMultiVertexFinder<vfitter_t, sfinder_t>::isMergedVertex(
 
 template <typename vfitter_t, typename sfinder_t>
 auto Acts::AdaptiveMultiVertexFinder<vfitter_t, sfinder_t>::deleteLastVertex(
+    Vertex<InputTrack_t>& vtx,
     std::vector<std::unique_ptr<Vertex<InputTrack_t>>>& allVertices,
     std::vector<Vertex<InputTrack_t>*>& allVerticesPtr,
     FitterState_t& fitterState, FitterState_t& oldFitterState,
@@ -535,7 +536,7 @@ auto Acts::AdaptiveMultiVertexFinder<vfitter_t, sfinder_t>::deleteLastVertex(
 
   } else {
     // Update fitter state with removed vertex candidate
-    fitterState.updateTrkToVerticesMultiMap(allVerticesPtr);
+    fitterState.removeVertexFromMultiMap(vtx);
 
     // TODO: clean tracksAtVerticesMap maybe here? i.e. remove all entries
     // with old vertex?
diff --git a/Core/include/Acts/Vertexing/AdaptiveMultiVertexFitter.hpp b/Core/include/Acts/Vertexing/AdaptiveMultiVertexFitter.hpp
index c96566e97590ead185a1a7d91a657639a2cb23e9..dc7c03bccba4209930436cffd2a9a20bf57f544a 100644
--- a/Core/include/Acts/Vertexing/AdaptiveMultiVertexFitter.hpp
+++ b/Core/include/Acts/Vertexing/AdaptiveMultiVertexFitter.hpp
@@ -71,24 +71,21 @@ class AdaptiveMultiVertexFitter {
     /// @brief Default State constructor
     State() = default;
 
-    void updateTrkToVerticesMultiMap(
-        std::vector<Vertex<input_track_t>>& vtxList) {
-      trackToVerticesMultiMap.clear();
-      for (auto& vtx : vtxList) {
-        // Add vertex link for each track
-        for (auto& trk : vtxInfoMap[&vtx].trackLinks) {
-          trackToVerticesMultiMap.emplace(trk, &vtx);
-        }
+    // Adds a vertex to trackToVerticesMultiMap
+    void addVertexToMultiMap(Vertex<InputTrack_t>& vtx) {
+      for (auto trk : vtxInfoMap[&vtx].trackLinks) {
+        trackToVerticesMultiMap.emplace(trk, &vtx);
       }
     }
 
-    void updateTrkToVerticesMultiMap(
-        std::vector<Vertex<input_track_t>*>& vtxList) {
-      trackToVerticesMultiMap.clear();
-      for (auto vtx : vtxList) {
-        // Add vertex link for each track
-        for (auto trk : vtxInfoMap[vtx].trackLinks) {
-          trackToVerticesMultiMap.emplace(trk, vtx);
+    // Removes a vertex from trackToVerticesMultiMap
+    void removeVertexFromMultiMap(Vertex<InputTrack_t>& vtx) {
+      for (auto iter = trackToVerticesMultiMap.begin();
+           iter != trackToVerticesMultiMap.end();) {
+        if (iter->second == &vtx) {
+          iter = trackToVerticesMultiMap.erase(iter);
+        } else {
+          ++iter;
         }
       }
     }
@@ -169,9 +166,9 @@ class AdaptiveMultiVertexFitter {
   ///
   /// @return Result<void> object
   Result<void> fit(
-      State& state, const std::vector<Vertex<input_track_t>*>& verticesToFit,
+      State& state, const std::vector<Vertex<InputTrack_t>*>& verticesToFit,
       const Linearizer_t& linearizer,
-      const VertexFitterOptions<input_track_t>& vFitterOptions) const;
+      const VertexFitterOptions<InputTrack_t>& vFitterOptions) const;
 
   /// @brief Adds new vertex to an existing multi-vertex fit
   /// and fits everything together (by invoking the fit_impl method):
diff --git a/Tests/UnitTests/Core/Vertexing/AdaptiveMultiVertexFitterTests.cpp b/Tests/UnitTests/Core/Vertexing/AdaptiveMultiVertexFitterTests.cpp
index 71bbdb19c5112aef7ea53eeecf94d813d1f18925..dffb8fc63ec1528bc6fab95d2480bb0814009cf7 100644
--- a/Tests/UnitTests/Core/Vertexing/AdaptiveMultiVertexFitterTests.cpp
+++ b/Tests/UnitTests/Core/Vertexing/AdaptiveMultiVertexFitterTests.cpp
@@ -201,20 +201,16 @@ BOOST_AUTO_TEST_CASE(adaptive_multi_vertex_fitter_test) {
     }
   }
 
-  if (debugMode) {
-    std::cout << "Tracks linked to each vertex: " << std::endl;
-    int c = 0;
-    for (auto& vtx : vtxPtrList) {
-      c++;
-      std::cout << c << ". vertex, with ptr: " << vtx << std::endl;
+  for (auto& vtx : vtxPtrList) {
+    state.addVertexToMultiMap(*vtx);
+    if (debugMode) {
+      std::cout << "Vertex, with ptr: " << vtx << std::endl;
       for (auto& trk : state.vtxInfoMap[vtx].trackLinks) {
         std::cout << "\t track ptr: " << trk << std::endl;
       }
     }
   }
 
-  state.updateTrkToVerticesMultiMap(vtxPtrList);
-
   if (debugMode) {
     std::cout << "Checking all vertices linked to a single track: "
               << std::endl;
@@ -483,7 +479,8 @@ BOOST_AUTO_TEST_CASE(adaptive_multi_vertex_fitter_test_athena) {
   state.vtxInfoMap[&vtx1] = std::move(vtxInfo1);
   state.vtxInfoMap[&vtx2] = std::move(vtxInfo2);
 
-  state.updateTrkToVerticesMultiMap(vtxList);
+  state.addVertexToMultiMap(vtx1);
+  state.addVertexToMultiMap(vtx2);
 
   // Fit vertices
   fitter.fit(state, vtxList, linearizer, fitterOptions);