xrt::auxiliary::math Namespace Reference

C++-only functionality in the Math helper library. More...

Data Structures
struct	FourthRootMachineEps
struct	FourthRootMachineEps< ceres::Jet< T, N > >
struct	FourthRootMachineEps< double >
struct	FourthRootMachineEps< float >
class	IntegerLowPassIIRFilter
	A very simple integer low-pass filter, using a "one-pole infinite impulse response" design (one-pole IIR), also known as an exponential filter. More...
class	LowPassIIRFilter
	A very simple low-pass filter, using a "one-pole infinite impulse response" design (one-pole IIR). More...
class	LowPassIIRVectorFilter
	A very simple low-pass filter, using a "one-pole infinite impulse response" design (one-pole IIR). More...
struct	Rational
	A rational (fractional) number type. More...

Functions
static Eigen::Map< const Eigen::Quaternionf >	map_quat (const struct xrt_quat &q)
	Wrap an internal quaternion struct in an Eigen type, const overload.
static Eigen::Map< Eigen::Quaternionf >	map_quat (struct xrt_quat &q)
	Wrap an internal quaternion struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Vector2f >	map_vec2 (const struct xrt_vec2 &v)
	Wrap an internal 2D vector struct in an Eigen type, const overload.
static Eigen::Map< Eigen::Vector2f >	map_vec2 (struct xrt_vec2 &v)
	Wrap an internal 2D vector struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Vector3f >	map_vec3 (const struct xrt_vec3 &v)
	Wrap an internal 3D vector struct in an Eigen type, const overload.
static Eigen::Map< Eigen::Vector3f >	map_vec3 (struct xrt_vec3 &v)
	Wrap an internal 3D vector struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Vector3d >	map_vec3_f64 (const struct xrt_vec3_f64 &v)
	Wrap an internal 3D vector struct in an Eigen type, non-const overload.
static Eigen::Map< Eigen::Vector3d >	map_vec3_f64 (struct xrt_vec3_f64 &v)
	Wrap an internal 3D vector struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Matrix3f >	map_matrix_3x3 (const struct xrt_matrix_3x3 &m)
	Wrap an internal 3x3 matrix struct in an Eigen type, const overload.
static Eigen::Map< Eigen::Matrix3f >	map_matrix_3x3 (struct xrt_matrix_3x3 &m)
	Wrap an internal 3x3 matrix struct in an Eigen type, non-const overload.
static Eigen::Map< Eigen::Matrix3d >	map_matrix_3x3_f64 (struct xrt_matrix_3x3_f64 &m)
	Wrap an internal 3x3 matrix struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Matrix4f >	map_matrix_4x4 (const struct xrt_matrix_4x4 &m)
	Wrap an internal 4x4 matrix struct in an Eigen type, const overload.
static Eigen::Map< Eigen::Matrix4f >	map_matrix_4x4 (struct xrt_matrix_4x4 &m)
	Wrap an internal 4x4 matrix struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Matrix4d >	map_matrix_4x4_f64 (const struct xrt_matrix_4x4_f64 &m)
	Wrap an internal 4x4 matrix f64 struct in an Eigen type, const overload.
static Eigen::Map< Eigen::Matrix4d >	map_matrix_4x4_f64 (struct xrt_matrix_4x4_f64 &m)
	Wrap an internal 4x4 matrix struct in an Eigen type, non-const overload.
static Eigen::Map< const Eigen::Quaternionf >	orientation (const struct xrt_pose &pose)
	Return a Eigen type wrapping a pose's orientation (const).
static Eigen::Map< Eigen::Quaternionf >	orientation (struct xrt_pose &pose)
	Return a Eigen type wrapping a pose's orientation.
static Eigen::Map< const Eigen::Vector3f >	position (const struct xrt_pose &pose)
	Return a Eigen type wrapping a pose's position (const).
static Eigen::Map< Eigen::Vector3f >	position (struct xrt_pose &pose)
	Return a Eigen type wrapping a pose's position.
template<typename Scalar >
Scalar	sinc_sq (Scalar theta_sq)
	Computes the "historical" (un-normalized) sinc(Theta), using theta^2 to allow us to handle sqrt(0) better for differentiation.
template<typename Scalar >
Scalar	cos_sq (Scalar theta_sq)
	Computes cos(theta) given theta^2, staying differentiable at theta = 0.
template<typename Derived >
Eigen::Quaternion< typename Derived::Scalar >	quat_exp (Eigen::MatrixBase< Derived > const &vec)
	Fully-templated free function for quaternion exponentiation.
template<typename Derived >
Eigen::Quaternion< typename Derived::Scalar >	quat_exp_so3 (Eigen::MatrixBase< Derived > const &vec)
	Fully-templated free function for quaternion exponentiation, SO(3) version as described by Grassia.
template<typename Scalar >
Scalar	cscTaylorExpansion (Scalar theta)
	Taylor series expansion of theta over sin(theta), also known as cosecant, for use near 0 when you want continuity and validity at 0.
template<typename Scalar >
Eigen::Matrix< Scalar, 3, 1 >	quat_ln (Eigen::Quaternion< Scalar > const &quat)
	Fully-templated free function for quaternion log map.
template<typename Scalar >
Eigen::Matrix< Scalar, 3, 1 >	quat_ln_so3 (const Eigen::Quaternion< Scalar > &quat)
	Fully-templated free function for the quaternion log map, SO(3) version as described by Grassia.

Variables
constexpr double	quat_small_sqr_vecnorm = 1.e-8
	Squared vector norm below which the quat maps switch to a series expansion, i.e.

Detailed Description

C++-only functionality in the Math helper library.

Function Documentation

cos_sq()

template<typename Scalar >

Scalar xrt::auxiliary::math::cos_sq ( Scalar  theta_sq )

inline

Computes cos(theta) given theta^2, staying differentiable at theta = 0.

References quat_small_sqr_vecnorm.

Referenced by quat_exp(), and quat_exp_so3().

cscTaylorExpansion()

template<typename Scalar >

Scalar xrt::auxiliary::math::cscTaylorExpansion ( Scalar  theta )

inline

Taylor series expansion of theta over sin(theta), also known as cosecant, for use near 0 when you want continuity and validity at 0.

map_matrix_3x3() [1/2]

static Eigen::Map< const Eigen::Matrix3f > xrt::auxiliary::math::map_matrix_3x3 ( const struct xrt_matrix_3x3 &  m )

inlinestatic

Wrap an internal 3x3 matrix struct in an Eigen type, const overload.

Permits zero-overhead manipulation of xrt_matrix_3x3& by Eigen routines as if it were a Eigen::Matrix3f&.

Referenced by math_matrix_3x3_identity(), xrt_matrix_3x3::math_matrix_3x3_inverse(), xrt_matrix_4x4::math_matrix_3x3_rotation_from_isometry(), xrt_matrix_3x3::math_matrix_3x3_transpose(), and xrt_matrix_4x4::math_matrix_4x4_isometry_from_rt().

map_matrix_3x3() [2/2]

static Eigen::Map< Eigen::Matrix3f > xrt::auxiliary::math::map_matrix_3x3 ( struct xrt_matrix_3x3 &  m )

inlinestatic

Wrap an internal 3x3 matrix struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_matrix_3x3& by Eigen routines as if it were a Eigen::Matrix3f&.

map_matrix_3x3_f64()

static Eigen::Map< Eigen::Matrix3d > xrt::auxiliary::math::map_matrix_3x3_f64 ( struct xrt_matrix_3x3_f64 &  m )

inlinestatic

Wrap an internal 3x3 matrix struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_matrix_3x3& by Eigen routines as if it were a Eigen::Matrix3_f64&.

Referenced by math_matrix_3x3_f64_identity().

map_matrix_4x4() [1/2]

static Eigen::Map< const Eigen::Matrix4f > xrt::auxiliary::math::map_matrix_4x4 ( const struct xrt_matrix_4x4 &  m )

inlinestatic

Wrap an internal 4x4 matrix struct in an Eigen type, const overload.

Permits zero-overhead manipulation of xrt_matrix_4x4& by Eigen routines as if it were a Eigen::Matrix4f&.

Referenced by xrt_matrix_4x4::math_matrix_3x3_rotation_from_isometry(), xrt_matrix_4x4::math_matrix_4x4_identity(), xrt_matrix_4x4::math_matrix_4x4_inverse(), xrt_matrix_4x4::math_matrix_4x4_inverse_view_projection(), xrt_matrix_4x4::math_matrix_4x4_isometry_from_pose(), xrt_matrix_4x4::math_matrix_4x4_isometry_from_rt(), xrt_matrix_4x4::math_matrix_4x4_isometry_inverse(), xrt_matrix_4x4::math_matrix_4x4_model(), xrt_matrix_4x4::math_matrix_4x4_multiply(), xrt_matrix_4x4::math_matrix_4x4_transpose(), xrt_matrix_4x4::math_matrix_4x4_view_from_pose(), xrt_pose::math_pose_from_isometry(), and xrt_vec3::math_vec3_translation_from_isometry().

map_matrix_4x4() [2/2]

static Eigen::Map< Eigen::Matrix4f > xrt::auxiliary::math::map_matrix_4x4 ( struct xrt_matrix_4x4 &  m )

inlinestatic

Wrap an internal 4x4 matrix struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_matrix_4x4& by Eigen routines as if it were a Eigen::Matrix4f&.

map_matrix_4x4_f64() [1/2]

static Eigen::Map< const Eigen::Matrix4d > xrt::auxiliary::math::map_matrix_4x4_f64 ( const struct xrt_matrix_4x4_f64 &  m )

inlinestatic

Wrap an internal 4x4 matrix f64 struct in an Eigen type, const overload.

Permits zero-overhead manipulation of const xrt_matrix_4x4_f64& by Eigen routines as if it were a const Eigen::Matrix4d&.

Referenced by xrt_matrix_4x4_f64::m_mat4_f64_identity(), xrt_matrix_4x4_f64::m_mat4_f64_invert(), xrt_matrix_4x4_f64::m_mat4_f64_model(), xrt_matrix_4x4_f64::m_mat4_f64_multiply(), and xrt_matrix_4x4_f64::m_mat4_f64_orientation().

map_matrix_4x4_f64() [2/2]

static Eigen::Map< Eigen::Matrix4d > xrt::auxiliary::math::map_matrix_4x4_f64 ( struct xrt_matrix_4x4_f64 &  m )

inlinestatic

Wrap an internal 4x4 matrix struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_matrix_4x4_f64& by Eigen routines as if it were a Eigen::Matrix4d&.

map_quat() [1/2]

static Eigen::Map< const Eigen::Quaternionf > xrt::auxiliary::math::map_quat ( const struct xrt_quat &  q )

inlinestatic

Wrap an internal quaternion struct in an Eigen type, const overload.

Permits zero-overhead manipulation of const xrt_quat& by Eigen routines as if it were a const Eigen::Quaternionf&.

Referenced by xrt::tracking::hand::mercury::back_project(), imu_fusion::imu_fusion_get_prediction(), xrt_quat::math_quat_ensure_normalized(), xrt_quat::math_quat_exp(), xrt_quat::math_quat_finite_difference(), xrt_quat::math_quat_from_angle_vector(), xrt_quat::math_quat_from_euler_angles(), xrt_quat::math_quat_from_matrix_3x3(), xrt_quat::math_quat_from_vec_a_to_vec_b(), xrt_quat::math_quat_integrate_velocity(), xrt_quat::math_quat_invert(), xrt_quat::math_quat_is_identity(), xrt_quat::math_quat_len(), xrt_quat::math_quat_ln(), xrt_quat::math_quat_normalize(), xrt_quat::math_quat_rotate(), xrt_quat::math_quat_slerp(), xrt_quat::math_quat_to_swing_twist(), xrt_quat::math_quat_unrotate(), orientation(), orientation(), and xrt::tracking::hand::mercury::run_keypoint_estimation_unsafe().

map_quat() [2/2]

static Eigen::Map< Eigen::Quaternionf > xrt::auxiliary::math::map_quat ( struct xrt_quat &  q )

inlinestatic

Wrap an internal quaternion struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_quat& by Eigen routines as if it were a Eigen::Quaternionf&.

map_vec2() [1/2]

static Eigen::Map< const Eigen::Vector2f > xrt::auxiliary::math::map_vec2 ( const struct xrt_vec2 &  v )

inlinestatic

Wrap an internal 2D vector struct in an Eigen type, const overload.

Permits zero-overhead manipulation of const xrt_vec2& by Eigen routines as if it were a const Eigen::Vector2f&.

map_vec2() [2/2]

static Eigen::Map< Eigen::Vector2f > xrt::auxiliary::math::map_vec2 ( struct xrt_vec2 &  v )

inlinestatic

Wrap an internal 2D vector struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_vec2& by Eigen routines as if it were a Eigen::Vector2f&.

map_vec3() [1/2]

static Eigen::Map< const Eigen::Vector3f > xrt::auxiliary::math::map_vec3 ( const struct xrt_vec3 &  v )

inlinestatic

Wrap an internal 3D vector struct in an Eigen type, const overload.

Permits zero-overhead manipulation of const xrt_vec3& by Eigen routines as if it were a const Eigen::Vector3f&.

Referenced by xrt::tracking::hand::mercury::back_project(), imu_fusion::imu_fusion_get_prediction(), imu_fusion::imu_fusion_get_prediction_rotation_vec(), imu_fusion::imu_fusion_incorporate_accelerometer(), imu_fusion::imu_fusion_incorporate_gyros(), imu_fusion::imu_fusion_incorporate_gyros_and_accelerometer(), math_matrix_3x3_transform_vec3(), xrt_matrix_4x4::math_matrix_4x4_isometry_from_rt(), xrt_pose::math_pose_transform_point(), xrt_quat::math_quat_exp(), xrt_quat::math_quat_finite_difference(), xrt_quat::math_quat_integrate_velocity(), xrt_quat::math_quat_ln(), xrt_quat::math_quat_rotate_vec3(), xrt_vec3::math_vec3_accum(), xrt_vec3::math_vec3_cross(), xrt_vec3::math_vec3_normalize(), xrt_vec3::math_vec3_scalar_mul(), xrt_vec3::math_vec3_subtract(), xrt_vec3::math_vec3_translation_from_isometry(), xrt_vec3::math_vec3_validate(), position(), position(), xrt::tracking::hand::mercury::run_keypoint_estimation_unsafe(), and transform_point().

map_vec3() [2/2]

static Eigen::Map< Eigen::Vector3f > xrt::auxiliary::math::map_vec3 ( struct xrt_vec3 &  v )

inlinestatic

Wrap an internal 3D vector struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_vec3& by Eigen routines as if it were a Eigen::Vector3f&.

map_vec3_f64() [1/2]

static Eigen::Map< const Eigen::Vector3d > xrt::auxiliary::math::map_vec3_f64 ( const struct xrt_vec3_f64 &  v )

inlinestatic

Wrap an internal 3D vector struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_vec3& by Eigen routines as if it were a const Eigen::Vector3_f64&.

Referenced by math_matrix_3x3_f64_transform_vec3_f64(), xrt_vec3_f64::math_vec3_f64_cross(), and xrt_vec3_f64::math_vec3_f64_normalize().

map_vec3_f64() [2/2]

static Eigen::Map< Eigen::Vector3d > xrt::auxiliary::math::map_vec3_f64 ( struct xrt_vec3_f64 &  v )

inlinestatic

Wrap an internal 3D vector struct in an Eigen type, non-const overload.

Permits zero-overhead manipulation of xrt_vec3& by Eigen routines as if it were a Eigen::Vector3_f64&.

orientation() [1/2]

static Eigen::Map< const Eigen::Quaternionf > xrt::auxiliary::math::orientation ( const struct xrt_pose &  pose )

inlinestatic

Return a Eigen type wrapping a pose's orientation (const).

References map_quat().

Referenced by xrt_matrix_4x4_f64::m_mat4_f64_model(), xrt_matrix_4x4::math_matrix_4x4_isometry_from_pose(), xrt_matrix_4x4::math_matrix_4x4_model(), xrt_matrix_4x4::math_matrix_4x4_view_from_pose(), xrt_pose::math_pose_from_isometry(), xrt_pose::math_pose_invert(), transform_point(), and transform_pose().

orientation() [2/2]

static Eigen::Map< Eigen::Quaternionf > xrt::auxiliary::math::orientation ( struct xrt_pose &  pose )

inlinestatic

Return a Eigen type wrapping a pose's orientation.

References map_quat().

position() [1/2]

static Eigen::Map< const Eigen::Vector3f > xrt::auxiliary::math::position ( const struct xrt_pose &  pose )

inlinestatic

Return a Eigen type wrapping a pose's position (const).

References map_vec3().

Referenced by xrt_matrix_4x4_f64::m_mat4_f64_model(), xrt_matrix_4x4::math_matrix_4x4_isometry_from_pose(), xrt_matrix_4x4::math_matrix_4x4_model(), xrt_matrix_4x4::math_matrix_4x4_view_from_pose(), xrt_pose::math_pose_from_isometry(), xrt_pose::math_pose_invert(), transform_point(), and transform_pose().

position() [2/2]

static Eigen::Map< Eigen::Vector3f > xrt::auxiliary::math::position ( struct xrt_pose &  pose )

inlinestatic

Return a Eigen type wrapping a pose's position.

References map_vec3().

quat_exp()

template<typename Derived >

Eigen::Quaternion< typename Derived::Scalar > xrt::auxiliary::math::quat_exp ( Eigen::MatrixBase< Derived > const &  vec )

inline

Fully-templated free function for quaternion exponentiation.

Implementation inspired by Grassia, F. S. (1998). Practical Parameterization of Rotations Using the Exponential Map. Journal of Graphics Tools, 3(3), 29–48. http://doi.org/10.1080/10867651.1998.10487493

Note

This is not the SO(3) version of the exponential map as defined by Grassia, this is the standard form of quaternion exponentiation, which is why we do not include the factor of 1/2.

References cos_sq(), and sinc_sq().

Referenced by xrt_quat::math_quat_exp(), and xrt_quat::math_quat_integrate_velocity().

quat_exp_so3()

template<typename Derived >

Eigen::Quaternion< typename Derived::Scalar > xrt::auxiliary::math::quat_exp_so3 ( Eigen::MatrixBase< Derived > const &  vec )

inline

Fully-templated free function for quaternion exponentiation, SO(3) version as described by Grassia.

Implementation inspired by Grassia, F. S. (1998). Practical Parameterization of Rotations Using the Exponential Map. Journal of Graphics Tools, 3(3), 29–48. http://doi.org/10.1080/10867651.1998.10487493

References cos_sq(), and sinc_sq().

quat_ln()

template<typename Scalar >

Eigen::Matrix< Scalar, 3, 1 > xrt::auxiliary::math::quat_ln ( Eigen::Quaternion< Scalar > const &  quat )

inline

Fully-templated free function for quaternion log map.

Assumes a unit quaternion.

Note

This is the log of the quaternion as given, not of the shortest rotation it represents: a quaternion with a negative w takes the long way round (phi > pi/2). Negate the coefficients before calling if you want the minimal result, as is usually wanted when the output feeds an optimizer residual.

References quat_small_sqr_vecnorm.

Referenced by xrt_quat::math_quat_finite_difference(), xrt_quat::math_quat_ln(), and quat_ln_so3().

quat_ln_so3()

template<typename Scalar >

Eigen::Matrix< Scalar, 3, 1 > xrt::auxiliary::math::quat_ln_so3

(

const Eigen::Quaternion< Scalar > &

quat

)

Fully-templated free function for the quaternion log map, SO(3) version as described by Grassia.

Assumes a unit quaternion. Inverse of quat_exp_so3().

Note

See quat_ln() for the handling of a negative w.

References quat_ln().

sinc_sq()

template<typename Scalar >

Scalar xrt::auxiliary::math::sinc_sq ( Scalar  theta_sq )

inline

Computes the "historical" (un-normalized) sinc(Theta), using theta^2 to allow us to handle sqrt(0) better for differentiation.

(sine(theta)/theta for theta != 0, defined as the limit value of 0 at theta = 0)

Referenced by quat_exp(), and quat_exp_so3().

Variable Documentation

quat_small_sqr_vecnorm

constexpr double xrt::auxiliary::math::quat_small_sqr_vecnorm = 1.e-8

constexpr

Squared vector norm below which the quat maps switch to a series expansion, i.e.

a vector norm of 1e-4.

The truncated series is good to O(theta^6) there, well inside float precision, and the direct computation is still exact at that magnitude, so one constant serves both float and double.

Referenced by cos_sq(), and quat_ln().