Class GridCellLoopDetector

Class Documentation

class GridCellLoopDetector

Simple spatial-hash based loop detector for poses.

Uses a 3D grid (cell size = cell_size_m) to find candidate revisits.

Detection algorithm (for each incoming pose): 1) Convert pose to a grid cell. 2) Search the current cell and its 26 neighbors. 3) Keep candidates within a Euclidean radius of one cell size. 4) Keep only candidates that are sufficiently separated along the trajectory (path length difference >= min_distance_m). 5) Sort candidates by spatial distance and return best-first indices.

The detector also enforces a global gate: at least min_distance_m path travel must occur between two successive loop-triggering insertions.

Public Types

using CellDescriptor = algorithm::impl::SpatialHashCell3D

Spatial hash cell descriptor type.

using CellDescriptorHash = algorithm::impl::SpatialHashCell3DHash

Hash functor for CellDescriptor.

Public Functions

inline GridCellLoopDetector(double min_distance_m, double cell_size_m = 1.0)
Parameters:

  • min_distance_m[in] Minimum travelled distance required between successive loop-closure additions (meters). This same value is also used as the minimum along-trajectory separation between the current pose and a candidate pose.

  • cell_size_m[in] Spatial hash grid cell size (meters).

inline std::vector<size_t> add_pose(const Eigen::Vector3d &pose, double dist_travelled)

Add a pose and return loop candidates found.

Parameters:

  • pose[in] 3D position (x,y,z) in meters.

  • dist_travelled[in] Cumulative path length in meters.

Returns:

Indices of previously added poses that satisfy all loop criteria, sorted from nearest to farthest in Euclidean distance.

Public Static Functions

static inline CellDescriptor compute_cell(const Eigen::Vector3d &pos, double cell_size)

Compute the grid cell for a given position.

Parameters:

  • pos[in] 3D position.

  • cell_size[in] Grid cell size in meters.

Returns:

The CellDescriptor for the cell containing pos.