diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
index e4deef4d..239d5c42 100644
--- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
+++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
@@ -48,6 +48,7 @@
 // STL
 #include <math.h>
 #include <unordered_map>
+#include <unordered_set>
 #include <ctime>
 #include <iostream>
 #include <utility>
@@ -131,7 +132,8 @@ class SpatioTemporalVoxelGrid
   // Core making and clearing functions
   void Mark(const std::vector<observation::MeasurementReading>& marking_observations);
   void operator()(const observation::MeasurementReading& obs) const;
-  void ClearFrustums(const std::vector<observation::MeasurementReading>& clearing_observations);
+  void ClearFrustums(const std::vector<observation::MeasurementReading>& clearing_observations, \
+                     std::unordered_set<occupany_cell>& updated_cells);
 
   // Get the pointcloud of the underlying occupancy grid
   void GetOccupancyPointCloud(sensor_msgs::PointCloud2::Ptr& pc2);
@@ -158,7 +160,8 @@ class SpatioTemporalVoxelGrid
   double GetTemporalClearingDuration(const double& time_delta);
   double GetFrustumAcceleration(const double& time_delta, \
                                 const double& acceleration_factor);
-  void TemporalClearAndGenerateCostmap(std::vector<frustum_model>& frustums);
+  void TemporalClearAndGenerateCostmap(std::vector<frustum_model>& frustums, \
+                                       std::unordered_set<occupany_cell>& updated_cells);
 
   // Populate the costmap ROS api and pointcloud with a marked point
   void PopulateCostmapAndPointcloud(const openvdb::Coord& pt);
diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
index 77ae8bba..bed1e1f5 100644
--- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
+++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
@@ -114,7 +114,8 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer
   void ObservationsResetAfterReading() const;
 
   // Functions to interact with maps
-  void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y);
+  void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y, \
+                        std::unordered_set<volume_grid::occupany_cell>& updated_cells);
   bool updateFootprint(double robot_x, double robot_y, double robot_yaw, \
                        double* min_x, double* min_y, double* max_x, double* max_y);
   void ResetGrid(void);
diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp
index 4a67d2c0..89342419 100644
--- a/src/spatio_temporal_voxel_grid.cpp
+++ b/src/spatio_temporal_voxel_grid.cpp
@@ -98,8 +98,9 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void)
 }
 
 /*****************************************************************************/
-void SpatioTemporalVoxelGrid::ClearFrustums(const \
-               std::vector<observation::MeasurementReading>& clearing_readings)
+void SpatioTemporalVoxelGrid::ClearFrustums( \
+      const std::vector<observation::MeasurementReading>& clearing_readings, \
+      std::unordered_set<occupany_cell>& updated_cells)
 /*****************************************************************************/
 {
   boost::unique_lock<boost::mutex> lock(_grid_lock);
@@ -117,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \
 
   if(clearing_readings.size() == 0)
   {
-    TemporalClearAndGenerateCostmap(obs_frustums);
+    TemporalClearAndGenerateCostmap(obs_frustums, updated_cells);
     return;
   }
 
@@ -156,13 +157,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \
     frustum->TransformModel();
     obs_frustums.emplace_back(frustum, it->_decay_acceleration);
   }
-  TemporalClearAndGenerateCostmap(obs_frustums);
+  TemporalClearAndGenerateCostmap(obs_frustums, updated_cells);
   return;
 }
 
 /*****************************************************************************/
 void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
-                                          std::vector<frustum_model>& frustums)
+                              std::vector<frustum_model>& frustums,           \
+                              std::unordered_set<occupany_cell>& updated_cells)
 /*****************************************************************************/
 {
   // sample time once for all clearing readings
@@ -173,6 +175,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
   for (cit_grid; cit_grid.test(); ++cit_grid)
   {
     const openvdb::Coord pt_index(cit_grid.getCoord());
+    const openvdb::Vec3d pose_world = _grid->indexToWorld(pt_index);
 
     std::vector<frustum_model>::iterator frustum_it = frustums.begin();
     bool frustum_cycle = false;
@@ -188,7 +191,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
         continue;
       }
 
-      if ( frustum_it->frustum->IsInside(this->IndexToWorld(pt_index)) )
+      if ( frustum_it->frustum->IsInside(pose_world) )
       {
         frustum_cycle = true;
 
@@ -204,6 +207,10 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
           {
             std::cout << "Failed to clear point." << std::endl;
           }
+          else
+          {
+            updated_cells.insert(occupany_cell(pose_world[0], pose_world[1]));
+          }
         }
         else
         {
@@ -228,11 +235,16 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
         {
           std::cout << "Failed to clear point." << std::endl;
         }
+        else
+        {
+          updated_cells.insert(occupany_cell(pose_world[0], pose_world[1]));
+        }
         continue;
       }
     }
     // if here, we can add to costmap and PC2
     PopulateCostmapAndPointcloud(pt_index);
+    updated_cells.insert(occupany_cell(pose_world[0], pose_world[1]));
   }
 }
 
diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp
index 72d36ca7..172b2a36 100644
--- a/src/spatio_temporal_voxel_layer.cpp
+++ b/src/spatio_temporal_voxel_layer.cpp
@@ -662,24 +662,28 @@ void SpatioTemporalVoxelLayer::updateCosts( \
 }
 
 /*****************************************************************************/
-void SpatioTemporalVoxelLayer::UpdateROSCostmap(double* min_x, double* min_y, \
-                                                double* max_x, double* max_y)
+void SpatioTemporalVoxelLayer::UpdateROSCostmap( \
+                        double* min_x, double* min_y, \
+                        double* max_x, double* max_y, \
+                        std::unordered_set<volume_grid::occupany_cell>& updated_cells)
 /*****************************************************************************/
 {
   // grabs map of occupied cells from grid and adds to costmap_
   Costmap2D::resetMaps();
 
-  std::unordered_map<volume_grid::occupany_cell, uint>::iterator it;
-  for (it = _voxel_grid->GetFlattenedCostmap()->begin();
-       it != _voxel_grid->GetFlattenedCostmap()->end(); ++it)
+  auto cost_map = _voxel_grid->GetFlattenedCostmap();
+
+  for (const auto& cell: updated_cells)
   {
+    auto it = cost_map->find(cell);
     uint map_x, map_y;
-    if ( it->second >= _mark_threshold && \
+    if ( it != cost_map->end() && \
+         it->second >= _mark_threshold && \
          worldToMap(it->first.x, it->first.y, map_x, map_y))
     {
       costmap_[getIndex(map_x, map_y)] = costmap_2d::LETHAL_OBSTACLE;
-      touch(it->first.x, it->first.y, min_x, min_y, max_x, max_y);
     }
+    touch(cell.x, cell.y, min_x, min_y, max_x, max_y);
   }
 }
 
@@ -716,10 +720,13 @@ void SpatioTemporalVoxelLayer::updateBounds( \
   ObservationsResetAfterReading();
   current_ = current;
 
+  // cells that were updated
+  std::unordered_set<volume_grid::occupany_cell> updated_cells;
+
   // navigation mode: clear observations, mapping mode: save maps and publish
   if (!_mapping_mode)
   {
-    _voxel_grid->ClearFrustums(clearing_observations);
+    _voxel_grid->ClearFrustums(clearing_observations, updated_cells);
   }
   else if (ros::Time::now() - _last_map_save_time > _map_save_duration)
   {
@@ -740,7 +747,7 @@ void SpatioTemporalVoxelLayer::updateBounds( \
   _voxel_grid->Mark(marking_observations);
 
   // update the ROS Layered Costmap
-  UpdateROSCostmap(min_x, min_y, max_x, max_y);
+  UpdateROSCostmap(min_x, min_y, max_x, max_y, updated_cells);
 
   // publish point cloud in navigation mode
   if (_publish_voxels && !_mapping_mode)