math_util.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa
 * SPDX-FileCopyrightText: 2021 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2021 Nils Funk
 * SPDX-FileCopyrightText: 2021 Sotiris Papatheodorou
 * SPDX-License-Identifier: BSD-3-Clause
 */

#ifndef SE_MATH_UTIL_HPP
#define SE_MATH_UTIL_HPP



#include <Eigen/Dense>
#include <vector>



namespace se {
namespace math {



template<typename T>
constexpr bool is_power_of_two(T);

template<>
constexpr bool is_power_of_two<unsigned>(const unsigned x);

constexpr int log2_const(int n);

static inline unsigned power_two_up(const float x);

template<typename T>
static inline T fracf(const T& v);

template<typename T>
static inline T floorf(const T& v);

template<typename T>
static inline T fabs(const T& v);

template<typename Scalar>
static constexpr inline Scalar sq(Scalar a);

template<typename Scalar>
static constexpr inline Scalar cu(Scalar a);

template<typename Scalar>
static inline bool in(const Scalar v, const Scalar a, const Scalar b);

static inline Eigen::Vector3f to_translation(const Eigen::Matrix4f& T);

static inline Eigen::Matrix3f to_rotation(const Eigen::Matrix4f& T);

static inline Eigen::Matrix4f to_transformation(const Eigen::Vector3f& t);

static inline Eigen::Matrix4f to_transformation(const Eigen::Matrix3f& R);

static inline Eigen::Matrix4f to_transformation(const Eigen::Matrix3f& R, const Eigen::Vector3f& t);

static inline Eigen::Vector3f to_inverse_translation(const Eigen::Matrix4f& T);

static inline Eigen::Matrix3f to_inverse_rotation(const Eigen::Matrix4f& T);

static inline Eigen::Matrix4f to_inverse_transformation(const Eigen::Matrix4f& T);

template<typename T>
static inline typename std::enable_if<std::is_arithmetic<T>::value, T>::type
clamp(const T& f, const T& a, const T& b);

static inline void clamp(Eigen::Ref<Eigen::VectorXf> res,
                         const Eigen::Ref<const Eigen::VectorXf> a,
                         const Eigen::Ref<Eigen::VectorXf> b);

template<typename R, typename A, typename B>
static inline void
clamp(Eigen::MatrixBase<R>& res, const Eigen::MatrixBase<A>& a, const Eigen::MatrixBase<B>& b);

static Eigen::Vector3f
plane_normal(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, const Eigen::Vector3f& p3);

template<typename T>
static T median(std::vector<T>& data);

template<typename T>
static T almost_median(std::vector<T>& data);

template<typename T>
static T median(const std::vector<T>& data);

static Eigen::Matrix3f hat(const Eigen::Vector3f& omega);

static Eigen::Matrix3f exp_and_theta(const Eigen::Vector3f& omega, float& theta);

static Eigen::Matrix4f exp(const Eigen::Matrix<float, 6, 1>& a);



} // namespace math
} // namespace se



#include "impl/math_util_impl.hpp"



#endif // SE_MATH_UTIL_HPP