Autoware.Auto
autoware::localization::ndt::DynamicNDTVoxel Class Reference

#include <ndt_voxel.hpp>

Public Types

using Point = Eigen::Vector3d
 
using Cov = Eigen::Matrix3d
 

Public Member Functions

 DynamicNDTVoxel ()
 
void add_observation (const Point &pt)
 
bool8_t try_stabilize ()
 
bool8_t usable () const noexcept
 
const Covcovariance () const
 
std::experimental::optional< Covinverse_covariance () const
 
const Pointcentroid () const
 
uint64_t count () const noexcept
 

Static Public Attributes

static constexpr uint32_t NUM_POINT_THRESHOLD = 3U
 

Detailed Description

Dynamic Voxel implementation for the NDT map. A dynamic voxel updates its state with each added observation and hence it is to be only used when a raw point cloud is being transformed into the ndt map representation.

Member Typedef Documentation

◆ Cov

◆ Point

Constructor & Destructor Documentation

◆ DynamicNDTVoxel()

autoware::localization::ndt::DynamicNDTVoxel::DynamicNDTVoxel ( )

Member Function Documentation

◆ add_observation()

void autoware::localization::ndt::DynamicNDTVoxel::add_observation ( const Point pt)

Add a point to the cell, update the centroid and covariance. This function Uses Welford's online algorithm: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_online_algorithm

Parameters
ptPoint to add to the voxel.

◆ centroid()

const Eigen::Vector3d & autoware::localization::ndt::DynamicNDTVoxel::centroid ( ) const

Returns the mean of the points in the cell. Throw if the cell does not have enough points.

Returns
centroid of the cell

◆ count()

uint64_t autoware::localization::ndt::DynamicNDTVoxel::count ( ) const
noexcept

Get number of points residing in the voxel.

Returns
Number of points.

◆ covariance()

const Eigen::Matrix3d & autoware::localization::ndt::DynamicNDTVoxel::covariance ( ) const

Returns the covariance of the points in the voxel. If not all points are used to update the covariance or the cell does not have enough points for covariance calculation, throws an error.

Returns
covariance of the cell

◆ inverse_covariance()

std::experimental::optional< Eigen::Matrix3d > autoware::localization::ndt::DynamicNDTVoxel::inverse_covariance ( ) const

Returns the inverse covariance calculated from the covariance. If the covariance is not invertible, throws an error

Returns
inverse covariance of the cell

◆ try_stabilize()

bool8_t autoware::localization::ndt::DynamicNDTVoxel::try_stabilize ( )

Try to stabilize the covariance

Returns
True if stabilization succeeds and covariance is invertible

◆ usable()

bool8_t autoware::localization::ndt::DynamicNDTVoxel::usable ( ) const
noexcept

Check if the cell contains enough points to be used in ndt matching

Returns
True if cell has more points than NUM_POINT_THRESHOLD

Member Data Documentation

◆ NUM_POINT_THRESHOLD

constexpr uint32_t autoware::localization::ndt::DynamicNDTVoxel::NUM_POINT_THRESHOLD = 3U
staticconstexpr

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