Autoware.Auto
autoware::localization::ndt::NDTMapBase< Derived, VoxelT > Class Template Reference

#include <ndt_map.hpp>

Inheritance diagram for autoware::localization::ndt::NDTMapBase< Derived, VoxelT >:
Collaboration diagram for autoware::localization::ndt::NDTMapBase< Derived, VoxelT >:

Public Types

using Grid = std::unordered_map< uint64_t, VoxelT >
 
using Point = Eigen::Vector3d
 
using Config = autoware::perception::filters::voxel_grid::Config
 
using TimePoint = std::chrono::system_clock::time_point
 
using VoxelViewVector = std::vector< VoxelView< VoxelT > >
 

Public Member Functions

 NDTMapBase (const Config &voxel_grid_config)
 
 NDTMapBase (const NDTMapBase &)=delete
 
NDTMapBaseoperator= (const NDTMapBase &)=delete
 
 NDTMapBase (NDTMapBase &&)=default
 
NDTMapBaseoperator= (NDTMapBase &&)=default
 
const VoxelViewVectorcell (float32_t x, float32_t y, float32_t z) const
 
const VoxelViewVectorcell (const Point &pt) const
 
void insert (const sensor_msgs::msg::PointCloud2 &msg)
 
uint64_t size () const noexcept
 
auto cell_size () const noexcept
 
Grid::const_iterator begin () const noexcept
 Returns an const iterator to the first element of the map. More...
 
Grid::iterator begin () noexcept
 Returns an iterator to the first element of the map. More...
 
Grid::const_iterator cbegin () const noexcept
 Returns a const iterator to the first element of the map. More...
 
Grid::const_iterator end () const noexcept
 Returns a const iterator to one past the last element of the map. More...
 
Grid::iterator end () noexcept
 Returns an iterator to one past the last element of the map. More...
 
Grid::const_iterator cend () const noexcept
 Returns a const iterator to one past the last element of the map. More...
 
void clear () noexcept
 Clear all voxels in the map. More...
 
TimePoint stamp () const noexcept
 
const std::string & frame_id () const noexcept
 

Protected Member Functions

auto index (const Point &pt) const
 
VoxelT & voxel (uint64_t idx)
 
auto emplace (uint64_t key, const VoxelT &&vx)
 
- Protected Member Functions inherited from autoware::common::helper_functions::crtp< Derived >
const Derived & impl () const
 
Derived & impl ()
 

Member Typedef Documentation

◆ Config

template<typename Derived , typename VoxelT >
using autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::Config = autoware::perception::filters::voxel_grid::Config

◆ Grid

template<typename Derived , typename VoxelT >
using autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::Grid = std::unordered_map<uint64_t, VoxelT>

◆ Point

template<typename Derived , typename VoxelT >
using autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::Point = Eigen::Vector3d

◆ TimePoint

template<typename Derived , typename VoxelT >
using autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::TimePoint = std::chrono::system_clock::time_point

◆ VoxelViewVector

template<typename Derived , typename VoxelT >
using autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::VoxelViewVector = std::vector<VoxelView<VoxelT> >

Constructor & Destructor Documentation

◆ NDTMapBase() [1/3]

template<typename Derived , typename VoxelT >
autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::NDTMapBase ( const Config voxel_grid_config)
inlineexplicit

Constructor

Parameters
voxel_grid_configVoxel grid config to configure the underlying voxel grid.

◆ NDTMapBase() [2/3]

template<typename Derived , typename VoxelT >
autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::NDTMapBase ( const NDTMapBase< Derived, VoxelT > &  )
delete

◆ NDTMapBase() [3/3]

template<typename Derived , typename VoxelT >
autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::NDTMapBase ( NDTMapBase< Derived, VoxelT > &&  )
default

Member Function Documentation

◆ begin() [1/2]

template<typename Derived , typename VoxelT >
Grid::const_iterator autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::begin ( ) const
inlinenoexcept

Returns an const iterator to the first element of the map.

Returns
Iterator

◆ begin() [2/2]

template<typename Derived , typename VoxelT >
Grid::iterator autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::begin ( )
inlinenoexcept

Returns an iterator to the first element of the map.

Returns
Iterator

◆ cbegin()

template<typename Derived , typename VoxelT >
Grid::const_iterator autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::cbegin ( ) const
inlinenoexcept

Returns a const iterator to the first element of the map.

Returns
Iterator

◆ cell() [1/2]

template<typename Derived , typename VoxelT >
const VoxelViewVector& autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::cell ( const Point pt) const
inline

Lookup the cell at location.

Parameters
ptpoint to lookup
Returns
A vector containing the cell at given coordinates. A vector is used to support near-neighbour cell queries in the future.

◆ cell() [2/2]

template<typename Derived , typename VoxelT >
const VoxelViewVector& autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::cell ( float32_t  x,
float32_t  y,
float32_t  z 
) const
inline

Lookup the cell at location.

Parameters
xx coordinate
yy coordinate
zz coordinate
Returns
A vector containing the cell at given coordinates. A vector is used to support near-neighbour cell queries in the future.

◆ cell_size()

template<typename Derived , typename VoxelT >
auto autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::cell_size ( ) const
inlinenoexcept

Get size of the cell.

Returns
A point representing the dimensions of the cell.

◆ cend()

template<typename Derived , typename VoxelT >
Grid::const_iterator autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::cend ( ) const
inlinenoexcept

Returns a const iterator to one past the last element of the map.

Returns
Iterator

◆ clear()

template<typename Derived , typename VoxelT >
void autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::clear ( )
inlinenoexcept

Clear all voxels in the map.

◆ emplace()

template<typename Derived , typename VoxelT >
auto autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::emplace ( uint64_t  key,
const VoxelT &&  vx 
)
inlineprotected

◆ end() [1/2]

template<typename Derived , typename VoxelT >
Grid::const_iterator autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::end ( ) const
inlinenoexcept

Returns a const iterator to one past the last element of the map.

Returns
Iterator

◆ end() [2/2]

template<typename Derived , typename VoxelT >
Grid::iterator autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::end ( )
inlinenoexcept

Returns an iterator to one past the last element of the map.

Returns
Iterator

◆ frame_id()

template<typename Derived , typename VoxelT >
const std::string& autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::frame_id ( ) const
inlinenoexcept

Get map's frame id.

Returns
Frame id of the map.

◆ index()

template<typename Derived , typename VoxelT >
auto autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::index ( const Point pt) const
inlineprotected

Get voxel index given a point.

Parameters
ptpoint
Returns
voxel index

◆ insert()

template<typename Derived , typename VoxelT >
void autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::insert ( const sensor_msgs::msg::PointCloud2 &  msg)
inline

Insert a point cloud to the map.

Parameters
msgPointCloud2 message to add.

◆ operator=() [1/2]

template<typename Derived , typename VoxelT >
NDTMapBase& autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::operator= ( const NDTMapBase< Derived, VoxelT > &  )
delete

◆ operator=() [2/2]

template<typename Derived , typename VoxelT >
NDTMapBase& autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::operator= ( NDTMapBase< Derived, VoxelT > &&  )
default

◆ size()

template<typename Derived , typename VoxelT >
uint64_t autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::size ( ) const
inlinenoexcept

Get size of the map

Returns
Number of voxels in the map. This number includes the voxels that do not have enough numbers to be used yet.

◆ stamp()

template<typename Derived , typename VoxelT >
TimePoint autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::stamp ( ) const
inlinenoexcept

Get map's time stamp.

Returns
map's time stamp.

◆ voxel()

template<typename Derived , typename VoxelT >
VoxelT& autoware::localization::ndt::NDTMapBase< Derived, VoxelT >::voxel ( uint64_t  idx)
inlineprotected

Get a reference to the voxel at the given index. If no voxel exists, a default constructed Voxel is inserted.

Parameters
idx
Returns

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