Class EsdfIntegrator¶
 Defined in File esdf_integrator.h
Class Documentation¶

class
EsdfIntegrator
¶ Builds an ESDF layer out of a given TSDF layer.
For a description of this algorithm, please see: https://arxiv.org/abs/1611.03631
Public Functions

void
addNewRobotPosition
(const Point &position)¶ Used for planning  allocates sphere around as observed but occupied, and clears space in a smaller sphere around current position.
Points added this way are marked as “hallucinated,” and can subsequently be cleared based on this.

void
updateFromTsdfLayerBatch
()¶ Update from a TSDF layer in batch, clearing the current ESDF layer in the process.

void
updateFromTsdfLayer
(bool clear_updated_flag)¶ Incrementally update from the TSDF layer, optionally clearing the updated flag of all changed TSDF voxels.

void
updateFromTsdfBlocks
(const BlockIndexList &tsdf_blocks, bool incremental = false)¶ Shortcut for pushing neighbors (i.e., incremental update) by default.
Not necessary in batch.

void
processRaiseSet
()¶ For incremental updates, the raise set contains all fixed voxels whose distances have INCREASED since last iteration.
This means that all voxels that have these voxels as parents need to be invalidated and assigned new values. The raise set is always empty in batch operations.

void
processOpenSet
()¶ The core of ESDF updates: processes a queue of voxels to get the minimum possible distance value in each voxel, by checking the values of its neighbors and distance to neighbors.
The update is done once the open set is empty.

bool
updateVoxelFromNeighbors
(const GlobalIndex &global_index)¶ For new voxels, etc.
update its value from its neighbors. Sort of the inverse of what the open set does (pushes the value of voxels TO its neighbors). Used mostly for adding new voxels in.

bool
isFixed
(FloatingPoint dist_m) const¶

void
clear
()¶ Clears the state of the integrator, in case robot pose clearance is used.

float
getEsdfMaxDistance
() const¶ Update some specific settings.

void
setEsdfMaxDistance
(float max_distance)¶

bool
getFullEuclidean
() const¶

void
setFullEuclidean
(bool full_euclidean)¶
Protected Attributes

BucketQueue<GlobalIndex>
open_
¶ Open Queue for incremental updates.
Contains global voxel indices for the ESDF layer.

AlignedQueue<GlobalIndex>
raise_
¶ Raise set for updates; these are values that used to be in the fixed frontier and now have a higher value, or their children which need to have their values invalidated.

size_t
voxels_per_side_
¶

FloatingPoint
voxel_size_
¶

struct
Config
¶ Public Members

bool
full_euclidean_distance
= false¶ Whether to use full euclidean distance (true) or quasieuclidean (false).
Full euclidean is slightly more accurate (up to 8% in the worst case) but slower.

FloatingPoint
max_distance_m
= 2.0¶ Maximum distance to calculate the actual distance to.
Any values above this will be set to default_distance_m.

FloatingPoint
min_distance_m
= 0.2¶ Should mirror (or be smaller than) truncation distance in tsdf integrator.

FloatingPoint
default_distance_m
= 2.0¶ Default distance set for unknown values and values > max_distance_m.

FloatingPoint
min_diff_m
= 0.001¶ For cheaper but less accurate map updates: the minimum difference in a voxel distance, before the change is propagated.

float
min_weight
= 1e6¶ Minimum weight to consider a TSDF value seen at.

int
num_buckets
= 20¶ Number of buckets for the bucketed priority queue.

bool
multi_queue
= false¶ Whether to push stuff to the open queue multiple times, with updated distances.

bool
add_occupied_crust
= false¶ Whether to add an outside layer of occupied voxels.
Basically just sets all unknown voxels in the allocated blocks to occupied. Try to only use this for batch processing, otherwise look into addNewRobotPosition below, which uses clear spheres.

FloatingPoint
clear_sphere_radius
= 1.5¶ For marking unknown space around a robot as free or occupied, these are the radiuses used around each robot position.

FloatingPoint
occupied_sphere_radius
= 5.0¶

bool

void