Classes
class	CpuTimer
	This template class provides methods to keep track of time. More...

Typedefs
template<bool B, class T = void>
using	enable_if_t = typename std::enable_if< B, T >::type
	Helper type 'enable_if_t'. More...

Functions
template<typename T , typename ... Args>
std::unique_ptr< T >	make_unique (Args &&...args)
	Constructs an object of type T and wraps it in a std::unique_ptr. More...

template<typename Derived >
double	log_sum_exp (const Eigen::MatrixBase< Derived > &data)
	Return the element-wise logarithm of the sum of exponentials of the input data. More...

template<typename Derived , typename DerivedScalar = typename Derived::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value, bool > = true>
Eigen::Matrix< DerivedScalar, 3, Eigen::Dynamic >	quaternion_to_rotation_vector (const Eigen::MatrixBase< Derived > &quaternion)
	Convert a matrix of unit quaternions \( q_i \) to their rotation vector representation \( r_i \in \mathbb{R}^{3} \) in the tangent space at the identity. More...

template<typename Derived , typename DerivedScalar = typename Derived::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value, bool > = true>
Eigen::Matrix< DerivedScalar, 4, Eigen::Dynamic >	rotation_vector_to_quaternion (const Eigen::MatrixBase< Derived > &rotation_vector)
	Convert a matrix of rotation vectors \( r_i \in \mathbb{R}^{3} \) in the tangent space at the identity to their unitary quaternionic representation \( q_i \). More...

template<typename Derived , typename DerivedArg2 , typename DerivedScalar = typename Derived::Scalar, typename DerivedArg2Scalar = typename DerivedArg2::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value &&std::is_same< DerivedScalar, DerivedArg2Scalar >::value, bool > = true>
Eigen::Matrix< DerivedScalar, 4, Eigen::Dynamic >	sum_quaternion_rotation_vector (const Eigen::MatrixBase< Derived > &quaternion, const Eigen::MatrixBase< DerivedArg2 > &rotation_vector)
	Evaluate the colwise sum between a unit quaternion \( q \) and a set of rotation vectors \( r_i \in \mathbb{R}^{3} \) in the tangent space at the identity. More...

template<typename Derived , typename DerivedArg2 , typename DerivedScalar = typename Derived::Scalar, typename DerivedArg2Scalar = typename DerivedArg2::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value &&std::is_same< DerivedScalar, DerivedArg2Scalar >::value, bool > = true>
Eigen::Matrix< DerivedScalar, 3, Eigen::Dynamic >	diff_quaternion (const Eigen::MatrixBase< Derived > &quaternion_left, const Eigen::MatrixBase< DerivedArg2 > &quaternion_right)
	Evaluate the colwise difference between a set of unit quaternions \( q_i \) and a unit quaternion \( q \). More...

template<typename Derived , typename DerivedArg2 , typename DerivedScalar = typename Derived::Scalar, typename DerivedArg2Scalar = typename DerivedArg2::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value &&std::is_same< DerivedScalar, DerivedArg2Scalar >::value, bool > = true>
Eigen::Matrix< DerivedScalar, 4, 1 >	mean_quaternion (const Eigen::MatrixBase< Derived > &weight, const Eigen::MatrixBase< DerivedArg2 > &quaternion)
	Evaluate the weighted mean of a set of unit quaternions \( q_i \) given a set of weights \( w_i \). More...

template<typename Derived >
Eigen::VectorXd	multivariate_gaussian_log_density (const Eigen::MatrixBase< Derived > &input, const Eigen::Ref< const Eigen::VectorXd > &mean, const Eigen::Ref< const Eigen::MatrixXd > &covariance)
	Evaluate the logarithm of a multivariate Gaussian probability density function. More...

template<typename Derived >
Eigen::VectorXd	multivariate_gaussian_log_density_UVR (const Eigen::MatrixBase< Derived > &input, const Eigen::Ref< const Eigen::VectorXd > &mean, const Eigen::Ref< const Eigen::MatrixXd > &U, const Eigen::Ref< const Eigen::MatrixXd > &V, const Eigen::Ref< const Eigen::MatrixXd > &R)
	Evaluate the logarithm of a multivariate Gaussian probability density function using the Sherman–Morrison–Woodbury identity. More...

template<typename Derived >
Eigen::VectorXd	multivariate_gaussian_density (const Eigen::MatrixBase< Derived > &input, const Eigen::Ref< const Eigen::VectorXd > &mean, const Eigen::Ref< const Eigen::MatrixXd > &covariance)
	Evaluate a multivariate Gaussian probability density function. More...

template<typename Derived >
Eigen::VectorXd	multivariate_gaussian_density_UVR (const Eigen::MatrixBase< Derived > &input, const Eigen::Ref< const Eigen::VectorXd > &mean, const Eigen::Ref< const Eigen::MatrixXd > &U, const Eigen::Ref< const Eigen::MatrixXd > &V, const Eigen::Ref< const Eigen::MatrixXd > &R)
	Evaluate the logarithm of a multivariate Gaussian probability density function using the Sherman–Morrison–Woodbury formula. More...

std::string	throw_message (const std::string &from_where, const std::string &error_message, const std::string &data_log="")
	Utility function that returns a pre-formatted string for throwing exception reports. More...

Typedef Documentation

◆ enable_if_t

template<bool B, class T = void>

using bfl::utils::enable_if_t = typedef typename std::enable_if<B,T>::type

Helper type 'enable_if_t'.

Reference: https://en.cppreference.com/w/cpp/types/enable_if#Helper_types

Definition at line 29 of file utils.h.

Function Documentation

◆ diff_quaternion()

template<typename Derived , typename DerivedArg2 , typename DerivedScalar = typename Derived::Scalar, typename DerivedArg2Scalar = typename DerivedArg2::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value &&std::is_same< DerivedScalar, DerivedArg2Scalar >::value, bool > = true>

Eigen::Matrix<DerivedScalar, 3, Eigen::Dynamic> bfl::utils::diff_quaternion	(	const Eigen::MatrixBase< Derived > &	quaternion_left,
		const Eigen::MatrixBase< DerivedArg2 > &	quaternion_right
	)

Evaluate the colwise difference between a set of unit quaternions \( q_i \) and a unit quaternion \( q \).

The convention adopted for the quaternion representation is \( q = (a, b, c, d) \) where \( a \) is the real part.

Each subtraction takes the form \( r_{i, diff} = 2\,\mathrm{log}(q_{i}\,q^{*})^{\vee} \) expressed in the global frame (i.e. the left convention is used).

Taken from Chiella, A. C., Teixeira, B. O., & Pereira, G. A. (2019). Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. Sensors, 19(10), 2372.

Parameters

quaternion_left	a 4 x N matrix each column of which is a unit quaternion
quaternion_right	a 4 x 1 matrix representing a unit quaternion

Returns: a 3 x N matrix where the i-th column is the difference between the i-th left quaternion and the right quaternion

Definition at line 227 of file utils.h.

References quaternion_to_rotation_vector().

Referenced by bfl::sigma_point::unscented_transform().

Here is the call graph for this function:

◆ log_sum_exp()

template<typename Derived >

double bfl::utils::log_sum_exp ( const Eigen::MatrixBase< Derived > & data )

Return the element-wise logarithm of the sum of exponentials of the input data.

To learn more about logsumexp, read https://en.wikipedia.org/wiki/LogSumExp.

Parameters

data	Input numbers as a vector or a matrix.

Returns: The element-wise logsumexp of the input data.

Definition at line 72 of file utils.h.

Referenced by bfl::EstimatesExtraction::exponentialAverage(), bfl::SIS::filtering_step(), bfl::ResamplingWithPrior::resample(), and bfl::EstimatesExtraction::weightedAverage().

◆ make_unique()

template<typename T , typename ... Args>

std::unique_ptr<T> bfl::utils::make_unique ( Args &&... args )

Constructs an object of type T and wraps it in a std::unique_ptr.

Constructs a non-array type T. The arguments args are passed to the constructor of T. This overload only participates in overload resolution if T is not an array type. The function is equivalent to: unique_ptr<T>(new T(std::forward<Args>(args)...))

Note: Unlike std::make_shared (which has std::allocate_shared), std::make_unique does not have an allocator-aware counterpart. A hypothetical allocate_unique would be required to invent the deleter type D for the unique_ptr<T,D> it returns which would contain an allocator object and invoke both destroy and deallocate in its operator().

See https://en.cppreference.com/w/cpp/memory/unique_ptr/make_unique. See also: https://herbsutter.com/gotw/_102/.

Parameters

args	list of arguments with which an instance of T will be constructed.

@exeption May throw std::bad_alloc or any exception thrown by the constructor of T. If an exception is thrown, this function has no effect.

Returns: std::unique_ptr of an instance of type T.

Definition at line 56 of file utils.h.

◆ mean_quaternion()

template<typename Derived , typename DerivedArg2 , typename DerivedScalar = typename Derived::Scalar, typename DerivedArg2Scalar = typename DerivedArg2::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value &&std::is_same< DerivedScalar, DerivedArg2Scalar >::value, bool > = true>

Eigen::Matrix<DerivedScalar, 4, 1> bfl::utils::mean_quaternion	(	const Eigen::MatrixBase< Derived > &	weight,
		const Eigen::MatrixBase< DerivedArg2 > &	quaternion
	)

Evaluate the weighted mean of a set of unit quaternions \( q_i \) given a set of weights \( w_i \).

The convention adopted for the quaternion representation is \( q = (a, b, c, d) \) where \( a \) is the real part.

The mean quaternion is evaluated as the eigenvector of the matrix \( M = \sum_i w_i\, q_i\, q_i^{T} \) corresponding to the maximum eigenvalue.

Taken from Chiella, A. C., Teixeira, B. O., & Pereira, G. A. (2019). Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. Sensors, 19(10), 2372.

Parameters

weight	a N x 1 matrix containing N weights
quaternion	a 4 x N matrix each column of which is a unit quaternion

Returns: a 4-vector representing the weighted mean of the input quaternion set

Definition at line 271 of file utils.h.

Referenced by bfl::sigma_point::unscented_transform().

◆ multivariate_gaussian_density()

template<typename Derived >

Eigen::VectorXd bfl::utils::multivariate_gaussian_density	(	const Eigen::MatrixBase< Derived > &	input,
		const Eigen::Ref< const Eigen::VectorXd > &	mean,
		const Eigen::Ref< const Eigen::MatrixXd > &	covariance
	)

Evaluate a multivariate Gaussian probability density function.

Parameters

input	Input representing the argument of the function as a vector or matrix.
mean	The mean of the associated Gaussian distribution as a vector.
covariance	The covariance matrix of the associated Gaussian distribution as a matrix.

Returns: The value of the density function evaluated on the input data as a vector.

Definition at line 419 of file utils.h.

References multivariate_gaussian_log_density().

Referenced by bfl::GPFCorrection::evaluateProposal(), bfl::KFCorrection::getLikelihood(), bfl::UKFCorrection::getLikelihood(), bfl::WhiteNoiseAcceleration::getTransitionProbability(), and bfl::GaussianLikelihood::likelihood().

Here is the call graph for this function:

◆ multivariate_gaussian_density_UVR()

template<typename Derived >

Eigen::VectorXd bfl::utils::multivariate_gaussian_density_UVR	(	const Eigen::MatrixBase< Derived > &	input,
		const Eigen::Ref< const Eigen::VectorXd > &	mean,
		const Eigen::Ref< const Eigen::MatrixXd > &	U,
		const Eigen::Ref< const Eigen::MatrixXd > &	V,
		const Eigen::Ref< const Eigen::MatrixXd > &	R
	)

Evaluate the logarithm of a multivariate Gaussian probability density function using the Sherman–Morrison–Woodbury formula.

It is assumed that the covariance matrix S can be written in the form S = UV + R where R is an invertible block diagonal matrix. Each block R_{i} is square and has dimension M such that N * M is the size of the input for some positive integer N.

This version is much faster than the standard gaussian density evaluation if U.cols() << U.rows().

Parameters

input	Input representing the argument of the function as a vector or matrix.
mean	The mean of the associated Gaussian distribution as a vector.
U	The U factor within the covariance matrix S = UV + R of the associated Gaussian distribution as a matrix.
V	The V factor within the covariance matrix S = UV + R of the associated Gaussian distribution as a matrix.
R	The R summand within the covariance matrix S = UV + R of the associated Gaussian distribuion as a matrix. The matrix R has size M * (N * M) and consists in the concatenation of all the diagonal blocks R_{i}. If the diagonal blocks are all equal, it is possible to provide a single M * M block matrix.

Returns: The value of the density function evaluated on the input data as a vector.

Definition at line 447 of file utils.h.

References multivariate_gaussian_log_density_UVR().

Referenced by bfl::SUKFCorrection::getLikelihood().

Here is the call graph for this function:

◆ multivariate_gaussian_log_density()

template<typename Derived >

Eigen::VectorXd bfl::utils::multivariate_gaussian_log_density	(	const Eigen::MatrixBase< Derived > &	input,
		const Eigen::Ref< const Eigen::VectorXd > &	mean,
		const Eigen::Ref< const Eigen::MatrixXd > &	covariance
	)

Evaluate the logarithm of a multivariate Gaussian probability density function.

Parameters

input	Input representing the argument of the function as a vector or matrix.
mean	The mean of the associated Gaussian distribution as a vector.
covariance	The covariance matrix of the associated Gaussian distribution as a matrix.

Returns: The value of the logarithm of the density function evaluated on the input data as a vector.

Definition at line 297 of file utils.h.

Referenced by multivariate_gaussian_density().

◆ multivariate_gaussian_log_density_UVR()

template<typename Derived >

Eigen::VectorXd bfl::utils::multivariate_gaussian_log_density_UVR	(	const Eigen::MatrixBase< Derived > &	input,
		const Eigen::Ref< const Eigen::VectorXd > &	mean,
		const Eigen::Ref< const Eigen::MatrixXd > &	U,
		const Eigen::Ref< const Eigen::MatrixXd > &	V,
		const Eigen::Ref< const Eigen::MatrixXd > &	R
	)

Evaluate the logarithm of a multivariate Gaussian probability density function using the Sherman–Morrison–Woodbury identity.

It is assumed that the covariance matrix S can be written in the form S = UV + R where R is an invertible block diagonal matrix. Each block R_{i} is square and has dimension M such that N * M is the size of the input for some positive integer N.

This version is much faster than the standard gaussian logarithm density evaluation if U.cols() << U.rows().

Parameters

input	Input representing the argument of the function as a vector or matrix.
mean	The mean of the associated Gaussian distribution as a vector.
U	The U factor within the covariance matrix S = UV + R of the associated Gaussian distribution as a matrix.
V	The V factor within the covariance matrix S = UV + R of the associated Gaussian distribution as a matrix.
R	The R summand within the covariance matrix S = UV + R of the associated Gaussian distribuion as a matrix. The matrix R has size M * (N * M) and consists in the concatenation of all the diagonal blocks R_{i}. If the diagonal blocks are all equal, it is possible to provide a single M * M block matrix.

Returns: The value of the logarithm of the density function evaluated on the input data as a vector.

Definition at line 331 of file utils.h.

Referenced by multivariate_gaussian_density_UVR().

◆ quaternion_to_rotation_vector()

template<typename Derived , typename DerivedScalar = typename Derived::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value, bool > = true>

Eigen::Matrix<DerivedScalar, 3, Eigen::Dynamic> bfl::utils::quaternion_to_rotation_vector ( const Eigen::MatrixBase< Derived > & quaternion )

Convert a matrix of unit quaternions \( q_i \) to their rotation vector representation \( r_i \in \mathbb{R}^{3} \) in the tangent space at the identity.

The convention adopted for the quaternion representation is \( q = (a, b, c, d) \) where \( a \) is the real part.

Taken from Chiella, A. C., Teixeira, B. O., & Pereira, G. A. (2019). Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. Sensors, 19(10), 2372.

Parameters

quaternion a 4 x N matrix each column of which is a unit quaternion

Returns: a 3 x N matrix each column of which is the rotation vector associated to the input unit quaternion

Definition at line 99 of file utils.h.

Referenced by diff_quaternion().

◆ rotation_vector_to_quaternion()

template<typename Derived , typename DerivedScalar = typename Derived::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value, bool > = true>

Eigen::Matrix<DerivedScalar, 4, Eigen::Dynamic> bfl::utils::rotation_vector_to_quaternion ( const Eigen::MatrixBase< Derived > & rotation_vector )

Convert a matrix of rotation vectors \( r_i \in \mathbb{R}^{3} \) in the tangent space at the identity to their unitary quaternionic representation \( q_i \).

The convention adopted for the quaternion representation is \( q = (a, b, c, d) \) where \( a \) is the real part.

Taken from Chiella, A. C., Teixeira, B. O., & Pereira, G. A. (2019). Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. Sensors, 19(10), 2372.

Parameters

rotation_vector a 3 x N matrix each column of which is a rotation vector

Returns: a 4 x N matrix each column of which is the unit quaternion associated to the input rotation vector

Definition at line 139 of file utils.h.

Referenced by sum_quaternion_rotation_vector().

◆ sum_quaternion_rotation_vector()

template<typename Derived , typename DerivedArg2 , typename DerivedScalar = typename Derived::Scalar, typename DerivedArg2Scalar = typename DerivedArg2::Scalar, bfl::utils::enable_if_t< std::is_floating_point< DerivedScalar >::value &&std::is_same< DerivedScalar, DerivedArg2Scalar >::value, bool > = true>

Eigen::Matrix<DerivedScalar, 4, Eigen::Dynamic> bfl::utils::sum_quaternion_rotation_vector	(	const Eigen::MatrixBase< Derived > &	quaternion,
		const Eigen::MatrixBase< DerivedArg2 > &	rotation_vector
	)

Evaluate the colwise sum between a unit quaternion \( q \) and a set of rotation vectors \( r_i \in \mathbb{R}^{3} \) in the tangent space at the identity.

The convention adopted for the quaternion representation is \( q = (a, b, c, d) \) where \( a \) is the real part.

Each summation takes the form \( q_{i, sum} = \mathrm{exp}(\hat{\frac{r_i}{2}}) \, q \) where \( r_i = \omega t \in \mathbb{R}^{3} \) is the tangent increment expressed in the global frame (i.e. the left convention is used.)

Taken from Chiella, A. C., Teixeira, B. O., & Pereira, G. A. (2019). Quaternion-Based Robust Attitude Estimation Using an Adaptive Unscented Kalman Filter. Sensors, 19(10), 2372.

Parameters

quaternion	a 4 x 1 matrix representing a unit quaternion
rotation_vector	a 3 x N matrix each column of which is a rotation vector

Returns: a 4 x N matrix where the i-th column is the sum between the unit quaternion and the i-th rotation vector

Definition at line 184 of file utils.h.

References rotation_vector_to_quaternion().

Referenced by bfl::sigma_point::sigma_point().

Here is the call graph for this function:

◆ throw_message()

std::string bfl::utils::throw_message	(	const std::string &	from_where,
		const std::string &	error_message,
		const std::string &	data_log = `""`
	)

inline

Utility function that returns a pre-formatted string for throwing exception reports.

Carefully read the input parameter descriptions for string styling format.

Parameters

from_where	Name of the throwing function with namespace. Example: "MYCLASS::METHOD", "NAMESPACE::FUNCTION".
error_message	Error message about the throwing exception. Example: "Index out of bound."
data_log	Optional additional message reporting data causing the throwing exception. Example: "Provided index: 3. Index bound: 2".

Returns: The throwing exeption report or "UTILS::THROW_MESSAGE::EMPTY_THROW_REPORT" if from_where or error_message are empty.

Definition at line 558 of file utils.h.

Classes

Typedefs

Functions

Typedef Documentation

◆ enable_if_t

Function Documentation

◆ diff_quaternion()

◆ log_sum_exp()

◆ make_unique()

◆ mean_quaternion()

◆ multivariate_gaussian_density()

◆ multivariate_gaussian_density_UVR()

◆ multivariate_gaussian_log_density()

◆ multivariate_gaussian_log_density_UVR()

◆ quaternion_to_rotation_vector()

◆ rotation_vector_to_quaternion()

◆ sum_quaternion_rotation_vector()

◆ throw_message()