Class EsdfIntegrator

Nested Relationships

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

EsdfIntegrator(const Config &config, Layer<TsdfVoxel> *tsdf_layer, Layer<EsdfVoxel> *esdf_layer)
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)

Short-cut 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

Config config_
Layer<TsdfVoxel> *tsdf_layer_
Layer<EsdfVoxel> *esdf_layer_
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_
IndexSet updated_blocks_
struct Config

Public Members

bool full_euclidean_distance = false

Whether to use full euclidean distance (true) or quasi-euclidean (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 = 1e-6

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