Autoware.Auto
autoware::common::helper_functions Namespace Reference

Namespaces

 comparisons
 
 detail
 
 message_field_adapters
 

Classes

class  ByteReader
 A utility class to read byte vectors in big-endian order. More...
 
class  crtp
 
struct  expression_valid
 
struct  expression_valid< ExpressionTemplate, T, types::void_t< ExpressionTemplate< T > > >
 
struct  expression_valid_with_return
 
struct  expression_valid_with_return< ExpressionTemplate, T, ReturnT, std::enable_if_t< std::is_same< ReturnT, ExpressionTemplate< T > >::value > >
 
class  LookupTable1D
 

Functions

template<typename T >
constexpr T wrap_angle (T angle) noexcept
 Wrap angle to the [-pi, pi] range. More...
 
template<typename T , std::int32_t kNumOfStates>
types::float32_t calculate_squared_mahalanobis_distance (const Eigen::Matrix< T, kNumOfStates, 1 > &sample, const Eigen::Matrix< T, kNumOfStates, 1 > &mean, const Eigen::Matrix< T, kNumOfStates, kNumOfStates > &covariance_factor)
 Calculate square of mahalanobis distance. More...
 
template<typename T , std::int32_t kNumOfStates>
types::float32_t calculate_mahalanobis_distance (const Eigen::Matrix< T, kNumOfStates, 1 > &sample, const Eigen::Matrix< T, kNumOfStates, 1 > &mean, const Eigen::Matrix< T, kNumOfStates, kNumOfStates > &covariance_factor)
 Calculate mahalanobis distance. More...
 
template<typename T >
lookup_1d (const std::vector< T > &domain, const std::vector< T > &range, const T value)
 

Function Documentation

◆ calculate_mahalanobis_distance()

template<typename T , std::int32_t kNumOfStates>
types::float32_t autoware::common::helper_functions::calculate_mahalanobis_distance ( const Eigen::Matrix< T, kNumOfStates, 1 > &  sample,
const Eigen::Matrix< T, kNumOfStates, 1 > &  mean,
const Eigen::Matrix< T, kNumOfStates, kNumOfStates > &  covariance_factor 
)

Calculate mahalanobis distance.

Template Parameters
TType of elements in the matrix
kNumOfStatesNumber of states
Parameters
sampleSingle column matrix containing sample whose distance needs to be computed
meanSingle column matrix containing mean of samples received so far
covariance_factorCovariance matrix
Returns
Mahalanobis distance

◆ calculate_squared_mahalanobis_distance()

template<typename T , std::int32_t kNumOfStates>
types::float32_t autoware::common::helper_functions::calculate_squared_mahalanobis_distance ( const Eigen::Matrix< T, kNumOfStates, 1 > &  sample,
const Eigen::Matrix< T, kNumOfStates, 1 > &  mean,
const Eigen::Matrix< T, kNumOfStates, kNumOfStates > &  covariance_factor 
)

Calculate square of mahalanobis distance.

Template Parameters
TType of elements in the matrix
kNumOfStatesNumber of states
Parameters
sampleSingle column matrix containing sample whose distance needs to be computed
meanSingle column matrix containing mean of samples received so far
covariance_factorCovariance matrix
Returns
Square of mahalanobis distance

◆ lookup_1d()

template<typename T >
T autoware::common::helper_functions::lookup_1d ( const std::vector< T > &  domain,
const std::vector< T > &  range,
const T  value 
)

Do a 1D table lookup: Does some semi-expensive O(N) error checking first. If query value fall out of the domain, then the value at the corresponding edge of the domain is returned.

Parameters
[in]domainThe domain, or set of x values
[in]rangeThe range, or set of y values
[in]valueThe point in the domain to query, x
Returns
A linearly interpolated value y, corresponding to the query, x
Exceptions
std::domain_errorIf domain or range is empty
std::domain_errorIf range is not the same size as domain
std::domain_errorIf domain is not sorted
std::domain_errorIf value is not finite (NAN or INF)
Template Parameters
TThe type of the function, must be interpolatable

◆ wrap_angle()

template<typename T >
constexpr T autoware::common::helper_functions::wrap_angle ( angle)
constexprnoexcept

Wrap angle to the [-pi, pi] range.

This method uses the formula suggested in the paper On wrapping the Kalman filter and estimating with the SO(2) group and implements the following formula: \(\mathrm{mod}(\alpha + \pi, 2 \pi) - \pi\).

Parameters
[in]angleThe input angle
Template Parameters
TType of scalar
Returns
Angle wrapped to the chosen range.