Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
approximate_intersection::LookupGrid< PointT > Class Template Reference

LookupGrid class implements a fast occupancy grid creation and intersection data structure. The user provides 2d min/max bounds on the grid as well as cell side length (cells are always square). The user can then add points into the grid. Cells which contain points are marked as occupied. Once the grid is populated, intersections can be checked against. If the queried point lands in an occupied cell the intersection is reported as true. More...

#include <lookup_grid.hpp>

Collaboration diagram for approximate_intersection::LookupGrid< PointT >:
Collaboration graph

Public Member Functions

 LookupGrid ()
 Default constructor Note: This constructor is provided for convenience but users should normally use the constructor which takes a Config object. More...
 
 LookupGrid (Config config)
 Constructor. More...
 
Config get_config ()
 Return the current config. More...
 
bool intersects (PointT point) const
 Checks if a point lies within an occupied cell of the grid. More...
 
void insert (PointT point)
 Adds a point to the grid. The point must lie within the grid bounds. The cell which the point lies in is marked as occupied. More...
 

Protected Types

using Hasher = autoware::common::geometry::spatial_hash::Config2d
 
using HashIndex = autoware::common::geometry::spatial_hash::Index
 

Protected Attributes

Config config_
 Configuration. More...
 
Hasher hasher_
 Spatial hasher which defines the grid. More...
 
std::unordered_set< HashIndexoccupied_cells_
 The set of occupied cells identified by their hash index. More...
 

Static Protected Attributes

static constexpr size_t PLACE_HOLDER_CAPACITY = 1
 Static placeholder value for the Autoware.Auto Hasher in use which requires this value but does not utilize it. More...
 

Detailed Description

template<class PointT>
class approximate_intersection::LookupGrid< PointT >

LookupGrid class implements a fast occupancy grid creation and intersection data structure. The user provides 2d min/max bounds on the grid as well as cell side length (cells are always square). The user can then add points into the grid. Cells which contain points are marked as occupied. Once the grid is populated, intersections can be checked against. If the queried point lands in an occupied cell the intersection is reported as true.

Template Parameters
PointTThe type of 2d point which the grid will be built from. Must have publicly accessible .x and .y members.

Definition at line 37 of file lookup_grid.hpp.

Member Typedef Documentation

◆ Hasher

template<class PointT >
using approximate_intersection::LookupGrid< PointT >::Hasher = autoware::common::geometry::spatial_hash::Config2d
protected

Definition at line 40 of file lookup_grid.hpp.

◆ HashIndex

template<class PointT >
using approximate_intersection::LookupGrid< PointT >::HashIndex = autoware::common::geometry::spatial_hash::Index
protected

Definition at line 41 of file lookup_grid.hpp.

Constructor & Destructor Documentation

◆ LookupGrid() [1/2]

template<class PointT >
approximate_intersection::LookupGrid< PointT >::LookupGrid ( )
inline

Default constructor Note: This constructor is provided for convenience but users should normally use the constructor which takes a Config object.

Definition at line 63 of file lookup_grid.hpp.

63:LookupGrid(Config()) {};
LookupGrid()
Default constructor Note: This constructor is provided for convenience but users should normally use ...
Definition: lookup_grid.hpp:63

◆ LookupGrid() [2/2]

template<class PointT >
approximate_intersection::LookupGrid< PointT >::LookupGrid ( Config  config)
inline

Constructor.

Parameters
configThe configuration for the grid.

Definition at line 70 of file lookup_grid.hpp.

70 :
71 config_(config),
72 hasher_(config.min_x, config.max_x, config.min_y, config.max_y, config.cell_side_length, PLACE_HOLDER_CAPACITY)
73 {
74 double dx = config.max_x - config.min_x;
75 double dy = config.max_y - config.min_y;
76
77 double rough_cell_side_count = 0;
78
79 if (dx > dy) {
80 rough_cell_side_count = dx / config.cell_side_length;
81 } else {
82 rough_cell_side_count = dy / config.cell_side_length;
83 }
84
85 occupied_cells_.reserve(rough_cell_side_count * rough_cell_side_count);
86 }
static constexpr size_t PLACE_HOLDER_CAPACITY
Static placeholder value for the Autoware.Auto Hasher in use which requires this value but does not u...
Definition: lookup_grid.hpp:54
std::unordered_set< HashIndex > occupied_cells_
The set of occupied cells identified by their hash index.
Definition: lookup_grid.hpp:51
Hasher hasher_
Spatial hasher which defines the grid.
Definition: lookup_grid.hpp:48

References approximate_intersection::Config::cell_side_length, approximate_intersection::Config::max_x, approximate_intersection::Config::max_y, approximate_intersection::Config::min_x, approximate_intersection::Config::min_y, and approximate_intersection::LookupGrid< PointT >::occupied_cells_.

Member Function Documentation

◆ get_config()

template<class PointT >
Config approximate_intersection::LookupGrid< PointT >::get_config ( )
inline

Return the current config.

Returns
the config in use by this object

Definition at line 93 of file lookup_grid.hpp.

93 {
94 return config_;
95 }

References approximate_intersection::LookupGrid< PointT >::config_.

◆ insert()

template<class PointT >
void approximate_intersection::LookupGrid< PointT >::insert ( PointT  point)
inline

Adds a point to the grid. The point must lie within the grid bounds. The cell which the point lies in is marked as occupied.

Parameters
pointThe point to add to the grid

Definition at line 121 of file lookup_grid.hpp.

121 {
122 HashIndex cell = hasher_.bin(point.x, point.y, 0);
123 occupied_cells_.insert(cell);
124 }
autoware::common::geometry::spatial_hash::Index HashIndex
Definition: lookup_grid.hpp:41

References approximate_intersection::LookupGrid< PointT >::hasher_, approximate_intersection::LookupGrid< PointT >::occupied_cells_, and process_traj_logs::point.

◆ intersects()

template<class PointT >
bool approximate_intersection::LookupGrid< PointT >::intersects ( PointT  point) const
inline

Checks if a point lies within an occupied cell of the grid.

Parameters
pointThe point to check for intersection
Returns
True if the point lies within an occupied cell, false otherwise

Definition at line 104 of file lookup_grid.hpp.

104 {
105
106 HashIndex cell = hasher_.bin(point.x, point.y, 0);
107
108 if (occupied_cells_.find(cell) != occupied_cells_.end()) {
109 return true;
110 }
111
112 return false;
113 }

References approximate_intersection::LookupGrid< PointT >::hasher_, approximate_intersection::LookupGrid< PointT >::occupied_cells_, and process_traj_logs::point.

Member Data Documentation

◆ config_

template<class PointT >
Config approximate_intersection::LookupGrid< PointT >::config_
protected

Configuration.

Definition at line 45 of file lookup_grid.hpp.

Referenced by approximate_intersection::LookupGrid< PointT >::get_config().

◆ hasher_

template<class PointT >
Hasher approximate_intersection::LookupGrid< PointT >::hasher_
protected

Spatial hasher which defines the grid.

Definition at line 48 of file lookup_grid.hpp.

Referenced by approximate_intersection::LookupGrid< PointT >::insert(), and approximate_intersection::LookupGrid< PointT >::intersects().

◆ occupied_cells_

template<class PointT >
std::unordered_set<HashIndex> approximate_intersection::LookupGrid< PointT >::occupied_cells_
protected

◆ PLACE_HOLDER_CAPACITY

template<class PointT >
constexpr size_t approximate_intersection::LookupGrid< PointT >::PLACE_HOLDER_CAPACITY = 1
staticconstexprprotected

Static placeholder value for the Autoware.Auto Hasher in use which requires this value but does not utilize it.

Definition at line 54 of file lookup_grid.hpp.


The documentation for this class was generated from the following file: