Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fixes #199 #200

Merged
merged 10 commits into from
May 26, 2021
Merged

fixes #199 #200

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
d2a7dc5
fixes #199
Apr 10, 2021
e482f7e
applied requested changes
Apr 14, 2021
019f1a7
renamed remaining updated_cells to cleared_cells
Apr 14, 2021
cd9c081
changed loop to use iterators
Apr 14, 2021
2d22cfb
fixed point cloud density when marking
Apr 15, 2021
651fcd7
small semantic change
Apr 16, 2021
883a3f0
cleared points are no longer added to the costmap
May 8, 2021
f443a2b
removed duplicate code
May 8, 2021
4088745
break early from frustum clearing. reverted fix to marking lag
May 11, 2021
5a07645
revert fix to marking lag
May 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 8 additions & 4 deletions include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
// STL
#include <math.h>
#include <unordered_map>
#include <unordered_set>
#include <ctime>
#include <iostream>
#include <utility>
Expand Down Expand Up @@ -130,8 +131,9 @@ 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 operator()(const observation::MeasurementReading& obs);
void ClearFrustums(const std::vector<observation::MeasurementReading>& clearing_observations, \
std::unordered_set<occupany_cell>& cleared_cells);

// Get the pointcloud of the underlying occupancy grid
void GetOccupancyPointCloud(sensor_msgs::PointCloud2::Ptr& pc2);
Expand All @@ -158,10 +160,12 @@ 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>& cleared_cells);

// Populate the costmap ROS api and pointcloud with a marked point
void PopulateCostmapAndPointcloud(const openvdb::Coord& pt);
void PopulateCostmapAndPointcloud(const openvdb::Coord& pt, \
const bool& pub_every_voxel);

// Utilities for tranformation
openvdb::Vec3d WorldToIndex(const openvdb::Vec3d& coord) const;
Expand Down
3 changes: 2 additions & 1 deletion include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>& cleared_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);
Expand Down
45 changes: 31 additions & 14 deletions src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>& cleared_cells)
/*****************************************************************************/
{
boost::unique_lock<boost::mutex> lock(_grid_lock);
Expand All @@ -117,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \

if(clearing_readings.size() == 0)
{
TemporalClearAndGenerateCostmap(obs_frustums);
TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
return;
}

Expand Down Expand Up @@ -156,13 +157,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \
frustum->TransformModel();
obs_frustums.emplace_back(frustum, it->_decay_acceleration);
}
TemporalClearAndGenerateCostmap(obs_frustums);
TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
return;
}

/*****************************************************************************/
void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
std::vector<frustum_model>& frustums)
std::vector<frustum_model>& frustums, \
std::unordered_set<occupany_cell>& cleared_cells)
/*****************************************************************************/
{
// sample time once for all clearing readings
Expand All @@ -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 = this->IndexToWorld(pt_index);

std::vector<frustum_model>::iterator frustum_it = frustums.begin();
bool frustum_cycle = false;
Expand All @@ -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;

Expand All @@ -200,6 +203,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
if (time_until_decay < 0.)
{
// expired by acceleration
cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1]));
if(!this->ClearGridPoint(pt_index))
{
std::cout << "Failed to clear point." << std::endl;
Expand All @@ -224,6 +228,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
if (base_duration_to_decay < 0.)
{
// expired by temporal clearing
cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1]));
if(!this->ClearGridPoint(pt_index))
{
std::cout << "Failed to clear point." << std::endl;
Expand All @@ -232,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( \
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
const openvdb::Coord& pt, \
const bool& pub_every_voxel)
/*****************************************************************************/
{
// add pt to the pointcloud and costmap
openvdb::Vec3d pose_world = _grid->indexToWorld(pt);
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];
Expand All @@ -263,6 +269,15 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
{
_cost_map->insert(std::make_pair( \
occupany_cell(pose_world[0], pose_world[1]), 1));

if (_pub_voxels && !pub_every_voxel)
{
geometry_msgs::Point32 point;
point.x = pose_world[0];
point.y = pose_world[1];
point.z = pose_world[2];
_grid_points->push_back(point);
}
}
}

Expand All @@ -287,7 +302,7 @@ void SpatioTemporalVoxelGrid::Mark(const \

/*****************************************************************************/
void SpatioTemporalVoxelGrid::operator()(const \
observation::MeasurementReading& obs) const
observation::MeasurementReading& obs)
/*****************************************************************************/
{
if (obs._marking)
Expand All @@ -314,8 +329,10 @@ void SpatioTemporalVoxelGrid::operator()(const \
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;
}
Expand Down
18 changes: 14 additions & 4 deletions src/spatio_temporal_voxel_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -662,8 +662,10 @@ 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>& cleared_cells)
/*****************************************************************************/
{
// grabs map of occupied cells from grid and adds to costmap_
Expand All @@ -681,6 +683,12 @@ void SpatioTemporalVoxelLayer::UpdateROSCostmap(double* min_x, double* min_y, \
touch(it->first.x, it->first.y, min_x, min_y, max_x, max_y);
}
}

std::unordered_set<volume_grid::occupany_cell>::iterator cell;
for (cell = cleared_cells.begin(); cell != cleared_cells.end(); ++cell)
{
touch(cell->x, cell->y, min_x, min_y, max_x, max_y);
}
}

/*****************************************************************************/
Expand Down Expand Up @@ -716,10 +724,12 @@ void SpatioTemporalVoxelLayer::updateBounds( \
ObservationsResetAfterReading();
current_ = current;

std::unordered_set<volume_grid::occupany_cell> cleared_cells;
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why an unordered_set? Why not a vector for instance? Was this experimentally shown to be faster?

Copy link
Contributor Author

@siferati siferati Apr 14, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I chose unordered_set to avoid duplicates. When clearing the world points, we don't care about the Z coordinate. If we clear world point (1, 2, 3) and (1, 2, 4), they're both gonna be in the same costmap cell, so there is no need to store and iterate through it twice.

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, that seems reasonable. I'd ask though if you'd test both and use which ever is practically faster. The STL Vector implementation is very cache friendly, so even areas where you wouldn't think it would be faster to use (because of duplications for instance), it ends up being faster due to continuous memory buffers (I ran into that alot while developing this: what you "think" should be faster based on theory and what is actually faster are not always intuitive). I don't know if it would be faster or not, so please test and let me know.

You can create an update timer pretty easily with chrono

StartTime = std::chrono::system_clock::now();
... do work
endTime = std::chrono::system_clock::now();
std::cout << "time took " std::chrono::duration_cast<std::chrono::milliseconds>(endTime - StartTime).count() << " ms."<< std::endl;

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You're right, vector is a lot faster than unordered_set when iterating through its contents, even though its size is a lot bigger due to duplicated elements. However, vector is only faster if we don't do anything while iterating. Since we need to touch every element, then unordered_set becomes the faster alternative since it has fewer elements - meaning less calls to touch.

I recorded two small clips where you can see the time it takes for both cases, when decay time is set to 0 (i.e. instant clearing):

Copy link
Owner

@SteveMacenski SteveMacenski Apr 15, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What was the looping testing? Just the loop without touch (so empty for?)

Keep in mind its not just access for iteration, its also about insertion time since vector insertion would probably be faster due to no hash table lookups for existing keys. You should try to measure the full update and take some metrics based on that since the insertions are pretty built into the clearing function. I agree its looking good for the unordered_set but just want to validate. I did alot of testing on datastructures to make STVL real-time so I want to ensure this doesn't have an undo impact on the overall performance.

So tl;dr

  • Measure this update cycle metric
  • Measure this with vector update cycle metric
  • Measure the baseline before-you-changed-anything metric

So we can answer the questions:

  • Is vector or unordered_set the right answer?
  • What impact does this have on the update time compared to what existed before?

Copy link
Contributor Author

@siferati siferati Apr 16, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I averaged 450 update cycles with the following scenario (decay time set to 0):

image

The start time was set right before defining the cleared_cells variable:

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

Line 727 in 651fcd7

std::unordered_set<volume_grid::occupany_cell> cleared_cells;

and end time at the very bottom of the function, right before return.

If I set the start time at the top of the function the execution times were very inconsistent, I'm guessing due to the calls to GetObservations methods. I don't think my changes should affect the performance before we clear anyway, so I set the start time after the observations are obtained, in order to get more consistent times.

Results (units in microseconds):

  • baseline
    • mean: 2983
    • std: 430
  • unordered_set
    • mean: 3761
    • std: 921
  • vector
    • mean: 3512
    • std: 836

So unordered_set and vector have basically the same performance, which was ~0.5ms worse than the baseline.


// navigation mode: clear observations, mapping mode: save maps and publish
if (!_mapping_mode)
{
_voxel_grid->ClearFrustums(clearing_observations);
_voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
}
else if (ros::Time::now() - _last_map_save_time > _map_save_duration)
{
Expand All @@ -740,7 +750,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, cleared_cells);

// publish point cloud in navigation mode
if (_publish_voxels && !_mapping_mode)
Expand Down