Skip to content

Adding min_age_outside_frustum parameter (solving the noisy edge problem) #183

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

Open
wants to merge 5 commits into
base: noetic-devel
Choose a base branch
from
Open

Adding min_age_outside_frustum parameter (solving the noisy edge problem) #183

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
0a32f24
add outside duration threshold
Sep 27, 2020
5574686
change name
Sep 27, 2020
a5033cf
increase maximum value
Sep 27, 2020
2692f15
french spelling
Sep 28, 2020
4b6a610
removing space and same get and dyn param
Sep 29, 2020
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
1 change: 1 addition & 0 deletions cfg/SpatioTemporalVoxelLayer.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ gen.add("mark_threshold", double_t, 0, "Over this threshold, the measurement wil
gen.add("update_footprint_enabled", bool_t, 0, "Cleans where the robot's footprint is", True)
gen.add("track_unknown_space", bool_t, 1, "If true, marks will be UNKNOWN (255) otherwise, FREE (0)", True)
gen.add("decay_model", int_t, 1, "Decay models", 0, edit_method=decay_model_enum)
gen.add("min_age_outside_frustum", double_t, 1, "the minimum age that a voxel should have to be allow to live outside frustrums", 0.0, 0.0, 100.0)
gen.add("voxel_decay", double_t, 1, "Seconds if linear, e^n if exponential", 10.0, -1, 200.0)
gen.add("mapping_mode", bool_t, 0, "Mapping mode", False)
gen.add("map_save_duration", double_t, 0, "f mapping, how often to save a map for safety", 60.0, 1.0, 2000.0)
Expand Down
4 changes: 2 additions & 2 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 @@ -124,7 +124,7 @@ class SpatioTemporalVoxelGrid
typedef openvdb::math::Ray<openvdb::Real>::Vec3T Vec3Type;

SpatioTemporalVoxelGrid(const float& voxel_size, const double& background_value,
const int& decay_model, const double& voxel_decay,
const int& decay_model, const double& min_age_outside_frustum, const double& voxel_decay,
const bool& pub_voxels);
~SpatioTemporalVoxelGrid(void);

Expand Down Expand Up @@ -169,7 +169,7 @@ class SpatioTemporalVoxelGrid

mutable openvdb::DoubleGrid::Ptr _grid;
int _decay_model;
double _background_value, _voxel_size, _voxel_decay;
double _background_value, _voxel_size, _min_age_outside_frustum, _voxel_decay;
bool _pub_voxels;
std::vector<geometry_msgs::Point32>* _grid_points;
std::unordered_map<occupany_cell, uint>* _cost_map;
Expand Down
2 changes: 1 addition & 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 @@ -161,7 +161,7 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer
ros::Duration _map_save_duration;
ros::Time _last_map_save_time;
std::string _global_frame;
double _voxel_size, _voxel_decay;
double _voxel_size, _min_age_outside_frustum, _voxel_decay;
int _combination_method, _mark_threshold;
volume_grid::GlobalDecayModel _decay_model;
bool _update_footprint_enabled, _enabled;
Expand Down
11 changes: 9 additions & 2 deletions src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,12 @@ namespace volume_grid
/*****************************************************************************/
SpatioTemporalVoxelGrid::SpatioTemporalVoxelGrid(const float& voxel_size, \
const double& background_value, const int& decay_model,\
const double& min_age_outside_frustum, \
const double& voxel_decay, const bool& pub_voxels) :
_background_value(background_value), \
_voxel_size(voxel_size), \
_decay_model(decay_model), \
_min_age_outside_frustum(min_age_outside_frustum), \
_voxel_decay(voxel_decay), \
_pub_voxels(pub_voxels), \
_grid_points(new std::vector<geometry_msgs::Point32>), \
Expand Down Expand Up @@ -218,10 +220,15 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
}
}

// if not inside any, check against nominal decay model
// if not inside any, check against nominal decay model, also clearing if
// too young.
// Depending on min_age_outside_frustum value and cycle frequency,
// a voxel too young means that it was marked without being in a frustrum
// (or, edgy case, the point that marked the voxel was in a frustrum
// but the center of that voxel was not), in that case : clearing
if(!frustum_cycle)
{
if (base_duration_to_decay < 0.)
if (base_duration_to_decay < 0. || time_since_marking < _min_age_outside_frustum)
Copy link
Owner

@SteveMacenski SteveMacenski Oct 8, 2020 •
edited
Loading

Choose a reason for hiding this comment

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

So this wouldn't only set the threshold for newly marked points, but also points outside of the frustum that have been accelerated before, since time_since_marking is set as the time it was marked, but is also updated by the frustum acceleration if it appeared in another iteration in the frustum.

That means that this is going to not only affect the clearing of recently marked points, but also longer-standing points that have had any level of frustum acceleration already applied. This is still overclearing.

I could make the argument that as long as the minimum age is very small, we can approximate those older points as being likely to be removed as well very soon in the next update cycle, but it does break that clear divide.

I really question if this is necessary. Going back to your problem statement: The original goal was to solve the noisy frustum edges problem . That could be much more easily resolved for this layer and all other perception tasks you have on your robots by setting up a trivial crop filter either in STVL to knock off the edges or - more correctly - add the crop filter to your filter chain attached to your depth sensor feeds. That way you just don't have those edge points even marking at all and since they're notoriously noisy and unreliable due to poor edge calibration, you really don't want to be using that information anywhere anyway. Why not just filter that out beforehand? That seems like the best overall solution to your problem both here and in any other processing pipeline you'd ever create using the realsense cameras.

{
// expired by temporal clearing
if(!this->ClearGridPoint(pt_index))
Expand Down
6 changes: 5 additions & 1 deletion src/spatio_temporal_voxel_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,8 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
layered_costmap_->isTrackingUnknown());
nh.param("decay_model", decay_model_int, 0);
_decay_model = static_cast<volume_grid::GlobalDecayModel>(decay_model_int);
// min_age_outside_frustum param
nh.param("min_age_outside_frustum", _min_age_outside_frustum, 0.0);
// decay param
nh.param("voxel_decay", _voxel_decay, -1.);
// whether to map or navigate
Expand Down Expand Up @@ -130,6 +132,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
_voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \
(double)default_value_, \
_decay_model, \
_min_age_outside_frustum, \
_voxel_decay, \
_publish_voxels);
matchSize();
Expand Down Expand Up @@ -620,14 +623,15 @@ void SpatioTemporalVoxelLayer::DynamicReconfigureCallback( \
costmap_2d::NO_INFORMATION : costmap_2d::FREE_SPACE;
default_value_ = default_value;
_voxel_size = config.voxel_size;
_min_age_outside_frustum = config.min_age_outside_frustum;
_voxel_decay = config.voxel_decay;
_decay_model = static_cast<volume_grid::GlobalDecayModel>(config.decay_model);
_publish_voxels = config.publish_voxel_map;

delete _voxel_grid;
_voxel_grid = new volume_grid::SpatioTemporalVoxelGrid(_voxel_size, \
static_cast<double>(default_value_), _decay_model, \
_voxel_decay, _publish_voxels);
_min_age_outside_frustum, _voxel_decay, _publish_voxels);
}
}

Expand Down