Autoware.Auto
|
|
#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 Cov & | covariance () const |
std::experimental::optional< Cov > | inverse_covariance () const |
const Point & | centroid () const |
uint64_t | count () const noexcept |
Static Public Attributes | |
static constexpr uint32_t | NUM_POINT_THRESHOLD = 3U |
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.
using autoware::localization::ndt::DynamicNDTVoxel::Cov = Eigen::Matrix3d |
using autoware::localization::ndt::DynamicNDTVoxel::Point = Eigen::Vector3d |
autoware::localization::ndt::DynamicNDTVoxel::DynamicNDTVoxel | ( | ) |
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
pt | Point to add to the voxel. |
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.
|
noexcept |
Get number of points residing in the voxel.
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.
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
bool8_t autoware::localization::ndt::DynamicNDTVoxel::try_stabilize | ( | ) |
Try to stabilize the covariance
|
noexcept |
Check if the cell contains enough points to be used in ndt matching
|
staticconstexpr |