fixed point cloud density when marking

SteveMacenski · Apr 15, 2021 · 2d22cfb · 2d22cfb
1 parent cd9c081
commit 2d22cfb
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 30 deletions.
diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
@@ -164,8 +164,8 @@ class SpatioTemporalVoxelGrid
                                        std::unordered_set<occupany_cell>& cleared_cells);
 
   // Populate the costmap ROS api and pointcloud with a marked point
-  void PopulateCostmapAndPointcloud(const openvdb::Coord& pt);
-  void PopulateCostmapAndPointcloud(const openvdb::Vec3d& pose_world);
+  void PopulateCostmapAndPointcloud(const openvdb::Coord& pt, \
+                                    const bool& pub_every_voxel);
 
   // Utilities for tranformation
   openvdb::Vec3d WorldToIndex(const openvdb::Vec3d& coord) const;

diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp
@@ -237,19 +237,20 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(                \
       }
     }
     // if here, we can add to costmap and PC2
-    PopulateCostmapAndPointcloud(pt_index);
+    PopulateCostmapAndPointcloud(pt_index, true);
   }
 }
 
 /*****************************************************************************/
-void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
-                                                            openvdb::Coord& pt)
+void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(                   \
+                                                   const openvdb::Coord& pt,  \
+                                                   const bool& pub_every_voxel)
 /*****************************************************************************/
 {
   // add pt to the pointcloud and costmap
   openvdb::Vec3d pose_world = this->IndexToWorld(pt);
 
-  if (_pub_voxels)
+  if (_pub_voxels && pub_every_voxel)
   {
     geometry_msgs::Point32 point;
     point.x = pose_world[0];
@@ -258,24 +259,6 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
     _grid_points->push_back(point);
   }
 
-  std::unordered_map<occupany_cell, uint>::iterator cell;
-  cell = _cost_map->find(occupany_cell(pose_world[0], pose_world[1]));
-  if (cell != _cost_map->end())
-  {
-    cell->second += 1;
-  }
-  else
-  {
-    _cost_map->insert(std::make_pair( \
-                              occupany_cell(pose_world[0], pose_world[1]), 1));
-  }
-}
-
-/*****************************************************************************/
-void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
-                                                    openvdb::Vec3d& pose_world)
-/*****************************************************************************/
-{
   std::unordered_map<occupany_cell, uint>::iterator cell;
   cell = _cost_map->find(occupany_cell(pose_world[0], pose_world[1]));
   if (cell != _cost_map->end())
@@ -287,7 +270,7 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
     _cost_map->insert(std::make_pair( \
                               occupany_cell(pose_world[0], pose_world[1]), 1));
 
-    if (_pub_voxels)
+    if (_pub_voxels && !pub_every_voxel)
     {
       geometry_msgs::Point32 point;
       point.x = pose_world[0];
@@ -343,12 +326,13 @@ void SpatioTemporalVoxelGrid::operator()(const \
       {
         continue;
       }
-      openvdb::Vec3d pose_world(*iter_x, *iter_y, *iter_z);      
-      PopulateCostmapAndPointcloud(pose_world);
-      openvdb::Vec3d mark_grid(this->WorldToIndex(pose_world));
+      openvdb::Vec3d mark_grid(this->WorldToIndex( \
+                                 openvdb::Vec3d(*iter_x, *iter_y, *iter_z)));
 
-      if(!this->MarkGridPoint(openvdb::Coord(mark_grid[0], mark_grid[1], \
-                                             mark_grid[2]), cur_time))
+      openvdb::Coord pt_index(mark_grid[0], mark_grid[1], mark_grid[2]);
+      PopulateCostmapAndPointcloud(pt_index, false);
+
+      if(!this->MarkGridPoint(pt_index, cur_time))
       {
         std::cout << "Failed to mark point." << std::endl;
       }