D:/Programmierung/C/TRTK/include/TRTK/Tools.hpp

Go to the documentation of this file.

/*
    Copyright (C) 2010 - 2019 Christoph Hänisch

    Chair of Medical Engineering (mediTEC)
    RWTH Aachen University
    Pauwelsstr. 20
    52074 Aachen
    Germany

    See license.txt for more information.

    Version 0.6.0 (2019-09-12)
*/

#ifndef TOOLS_H_9034987422
#define TOOLS_H_9034987422


#include <cassert>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <list>
#include <sstream>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include <Eigen/Core>

#include "Coordinate.hpp"

#ifdef QT_VERSION
    #include <QTransform>
#endif // QT_VERSION


namespace TRTK
{


template <class T> class Coordinate;


namespace Tools
{


#ifdef EIGEN_MACROS_H
#ifdef QT_VERSION

template <class EigenMatrix>
QTransform Eigen3x3_to_QTransfom(const EigenMatrix & eigenMatrix)
{
    QTransform qTransform(eigenMatrix(0, 0), eigenMatrix(0, 1), eigenMatrix(0, 2),
                          eigenMatrix(1, 0), eigenMatrix(1, 1), eigenMatrix(1, 2),
                          eigenMatrix(2, 0), eigenMatrix(2, 1), eigenMatrix(2, 2));

    return qTransform;
}

#endif // QT_VERSION
#endif // EIGEN_MACROS_H


template <class T>
std::pair<Eigen::Matrix<T, 3, 1>, T> axisAngleFromRotationMatrix(const Eigen::Matrix<T, 3, 3> & matrix)
{
    // The implementation is based on the lecture notes of Carlo Tomasi. See [1] for more information.
    // [1] https://www2.cs.duke.edu/courses/fall13/compsci527/notes/rodrigues.pdf

    using namespace Eigen;
    using Matrix3T = Matrix<T, 3, 3>;
    using Vector3T = Matrix<T, 3, 1>;

    const T eps = 1000 * std::numeric_limits<T>::epsilon();
    const T pi = 3.14159265359;

    Matrix3T A = (matrix - matrix.transpose()) / 2;
    Vector3T rho = Vector3T(A(2, 1), A(0, 2), A(1, 0));
    T s = rho.norm();
    T c = (matrix.trace() - 1) / 2;

    Vector3T axis;
    T angle;

    if (abs(s - 0) < eps && abs(c - 1) < eps)
    {
        axis = Vector3T(1, 0, 0);
        angle = 0;
    }
    else if (abs(s - 0) < eps && abs(c + 1) < eps)
    {
        angle = pi;
        Matrix3T B = matrix + Matrix3T::Identity();
        for (int i = 0; i < 3; ++i)
        {
            axis = B.col(i).normalized();
            if (axis.norm() > 0) break;
        }
        // Assure uniqueness by forcing the axis onto the half-hemisphere.
        if (abs(axis.x()) < eps && abs(axis.y()) < eps && axis.z() < 0 ||
            abs(axis.x()) < eps && axis.y() < 0 ||
            axis.x() < 0)
        {
            angle = -angle;
            axis = -axis;
        }
    }
    else
    {
        using ::atan2;
        axis = rho / s;
        angle = atan2(s, c);
    }

    return std::make_pair(axis, angle);
}


template <class T>
std::tuple<T, T, T> cartesian2Spherical(const T & x, const T & y, const T & z)
{
    const T pi = 3.1415926535897932384626433;
    using ::sqrt;
    auto r = sqrt(x * x + y * y + z * z);
    T theta;
    if (r == 0)
        theta = 0;
    else
        theta = acos(z / r);
    auto phi = atan2(y, x);

    assert(0 <= r);
    assert(0 <= theta && theta <= pi);
    assert(-pi <= phi && phi <= pi);

    return std::make_tuple(r, theta, phi);
}


template <class T>
Eigen::Matrix<T, 3, 1> cartesian2Spherical(const Eigen::Matrix<T, 3, 1> & point)
{
    Eigen::Matrix<T, 3, 1> result;
    std::tie(result.x(), result.y(), result.z()) = cartesian2Spherical(point.x(), point.y(), point.z());
    return result;
}


template <class T>
Coordinate<T> cartesian2Spherical(const Coordinate<T> & point)
{
    Coordinate<T> result(0, 0, 0);
    std::tie(result.x(), result.y(), result.z()) = cartesian2Spherical(point.x(), point.y(), point.z());
    return result;
}


bool fileExists(const char * file_name);


unsigned long long fileLength(const char * file_name);


unsigned long long fileLength(std::ifstream & file_stream);


std::string getCurrentDate();


std::string getCurrentTime();


template <class Derived, class Base>
inline bool isClass(Base * base_ptr)
{
    const Derived * derived_ptr = dynamic_cast<const Derived *>(base_ptr);
    return derived_ptr == NULL ? 0 : 1;
}


template <class Derived, class Base>
inline bool isClass(Base & base)
{
    return isClass<Derived>(&base);

    /*
    // This is an alternative implementation. However, the above one might be
    // better to be optimized by the compiler.

    // it might not be optimized

    try
    {
        Derived & derived = dynamic_cast<Derived &>(base);
        return 1;
    }
    catch (bad_cast)
    {
        return 0;
    }
    */
}


template <typename Base>
inline bool isDerivedFrom(const Base * base_ptr)
{
    return true;
}


template <typename Base>
inline bool isDerivedFrom(const void * base_ptr)
{
    return false;
}


template <class T>
inline bool isEqual(const T x, const T y)
{
    using std::abs;
    using std::numeric_limits;

    const T epsilon = 10 * numeric_limits<T>::epsilon() > 1e-13 ? 10 * numeric_limits<T>::epsilon() : 1e-13;
    return abs(x - y) < epsilon; // needs "cmath" and "limits" to be included
}

template <>
inline bool isEqual<int>(const int x, const int y)
{
    return x == y;
}



template <class T>
inline bool isZero(const T value)
{
    return isEqual<T>(value, T(0));
}


template <class T>
std::vector<T> listToVector(const std::list<T> & lst)
{
    int i = 0;
    std::vector<T> vec(lst.size());
    typename std::list<T>::const_iterator it;

    for (it = lst.begin(); it != lst.end(); ++it)
    {
        vec[i++] = *it;
    }

    return vec;
}


template <class Container, class ValueType> // TODO: C++11 --> class ValueType = Container::value_type
inline ValueType mean(const Container & container, ValueType null_value = ValueType())
{
    ValueType mean = null_value;

    for (typename Container::const_iterator it = container.begin(); it != container.end(); ++it)
    {
        mean = mean + *it;
    }

    return mean / container.size();
}


template <class WeightsContainer, class ValuesContainer, class ValueType = typename ValuesContainer::value_type>
inline ValueType weightedMean(const WeightsContainer & weights,
                              const ValuesContainer & values,
                              ValueType null_value = ValueType(0))
{
    using WeightType = typename WeightsContainer::value_type; // should be a scalar

    WeightType sum_of_weights = WeightType(0);
    ValueType weighted_sum = null_value;

    auto it_weights = weights.begin();
    auto it_values = values.begin();

    while (it_weights != weights.end() && it_values != values.end())
    {
        auto weight = *it_weights;
        auto value = *it_values;

        sum_of_weights += weight;
        weighted_sum += weight * value;

        ++it_weights;
        ++it_values;
    }

    return weighted_sum / sum_of_weights;
}


template <class Container>
inline auto median(const Container & container) -> std::decay_t<decltype(*begin(container))>
{
    using T = std::decay_t<decltype(*begin(container))>;
    using std::begin;
    using std::end;

    auto v = std::vector<T>(begin(container), end(container));
    auto size = v.size();

    if (size == 0) // undefined
    {
        using std::numeric_limits;
        return numeric_limits<T>::signaling_NaN();
    }
    else
    {
        std::sort(begin(v), end(v));
        if (size % 2 == 0)
        {
            auto it1 = begin(v);
            auto it2 = begin(v);
            std::advance(it1, size / 2 - 1);
            std::advance(it2, size / 2);
            return (*it1 + *it2) / 2;
        }
        else
        {
            auto it = begin(v);
            std::advance(it, size / 2);
            return *it;
        }
    }
}


template <class T>
TRTK::Coordinate<T> orthogonalMatrixToQuaternion(const Eigen::Matrix<T, 3, 3> & matrix)
{
    // References:
    //
    // [1] http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
    // [2] http://en.wikipedia.org/wiki/Rotation_matrix

    // Note: Coordinate<T> is of the form (x, y, z, w) whereas quaternions are often notated (w, x, y, z)!

    using std::sqrt;

    TRTK::Coordinate<T> quaternion(0, 0, 0, 0);

    T trace = matrix.trace();

    if (trace > -1) // check because of the computation of the root
    {
        double s = 0.5 / sqrt(trace + 1);
        quaternion.x() = 0.25 / s;
        quaternion.y() = (matrix(2,1) - matrix(1,2)) * s;
        quaternion.z() = (matrix(0,2) - matrix(2,0)) * s;
        quaternion.w() = (matrix(1,0) - matrix(0,1)) * s;
    }
    else if (matrix(0,0) > matrix(1,1) && matrix(0,0) > matrix(2,2))
    {
        double s = 2.0 * sqrt(1 + matrix(0,0) - matrix(1,1) - matrix(2,2));
        quaternion.x() = (matrix(2,1) - matrix(1,2)) / s;
        quaternion.y() = 0.25 * s;
        quaternion.z() = (matrix(0,1) + matrix(1,0)) / s;
        quaternion.w() = (matrix(0,2) + matrix(2,0)) / s;
    }
    else if (matrix(1,1) > matrix(2,2))
    {
        double s = 2 * sqrt(1 + matrix(1,1) - matrix(0,0) - matrix(2,2));
        quaternion.x() = (matrix(0,2) - matrix(2,0)) / s;
        quaternion.y() = (matrix(0,1) + matrix(1,0)) / s;
        quaternion.z() = 0.25 * s;
        quaternion.w() = (matrix(1,2) + matrix(2,1)) / s;
    }
    else
    {
        double s = 2 * sqrt(1 + matrix(2,2) - matrix(0,0) - matrix(1,1));
        quaternion.x() = (matrix(1,0) - matrix(0,1)) / s;
        quaternion.y() = (matrix(0,2) + matrix(2,0)) / s;
        quaternion.z() = (matrix(1,2) + matrix(2,1)) / s;
        quaternion.w() = 0.25 * s;
    }

    return quaternion.normalize();
}


template <class T>
TRTK::Coordinate<T> orthogonalMatrixToQuaternion2(const Eigen::Matrix<T, 3, 3> & matrix)
{
    // See [1] for more details about the implementation.

    // The matrix K3 is selfadjoint because it is real and symmetric. Thus we can use an
    // appropriate solver. The solver employed below only uses the lower triangular part
    // of the matrix hence we can leave the upper part uninitialized.

    typedef Eigen::Matrix<T, 4, 4> Matrix4T;
    typedef Eigen::Matrix<T, 4, 1> Vector4T;

    Matrix4T K3;

    K3(0,0) = matrix(0,0) - matrix(1,1) - matrix(2,2);
    K3(1,0) = matrix(1,0) + matrix(0,1);
    K3(2,0) = matrix(2,0) + matrix(0,2);
    K3(3,0) = matrix(1,2) - matrix(2,1);
    K3(1,1) = matrix(1,1) - matrix(0,0) - matrix(2,2);
    K3(2,1) = matrix(2,1) + matrix(1,2);
    K3(3,1) = matrix(2,0) - matrix(0,2);
    K3(2,2) = matrix(2,2) - matrix(0,0) - matrix(1,1);
    K3(3,2) = matrix(0,1) - matrix(1,0);
    K3(3,3) = matrix(0,0) + matrix(1,1) + matrix(2,2);

    K3 /= 3;

    Eigen::SelfAdjointEigenSolver<Matrix4T> solver(K3);

    Vector4T quaternion = solver.eigenvectors().col(3).normalized();

    return Coordinate<T>(quaternion);
}


template <class T>
Eigen::Matrix<T, 3, 3> quaternionToOrthogonalMatrix(T q0, T q1, T q2, T q3)
{
    // Revised code.

    // Identical to what you can find in the Wikipedia article
    // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
    // This matrix is a left-handed (post-multiplied) rotation matrix.

    assert(abs(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 - 1) < 1e-5); // check for unit quaternion

    Eigen::Matrix<T, 3, 3> R;

    R(0, 0) = 1 - 2 * (q2 * q2 + q3 * q3);
    R(0, 1) = 2 * q1 * q2 - 2 * q0 * q3;
    R(0, 2) = 2 * q1 * q3 + 2 * q0 * q2;

    R(1, 0) = 2 * q1 * q2 + 2 * q0 * q3;
    R(1, 1) = 1 - 2 * (q1 * q1  + q3 * q3);
    R(1, 2) = 2 * q2 * q3 - 2 * q0 * q1;

    R(2, 0) = 2 * q1 * q3 - 2 * q0 * q2;
    R(2, 1) = 2 * q2 * q3 + 2 * q0 * q1;
    R(2, 2) = 1 - 2 * (q1 * q1 + q2 * q2);

    return R;
}


template <class T>
inline Eigen::Matrix<T, 3, 3> quaternionToOrthogonalMatrix(const TRTK::Coordinate<T> & quaternion)
{
    assert(quaternion.size() == 4);
    return quaternionToOrthogonalMatrix<T>(quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w());
}


template <class T>
inline T rand()
{
    return static_cast<T>(std::rand()) / static_cast<T>(RAND_MAX);
}


template <class T>
T rand(T a, T b)
{
    assert(a <= b);
    return static_cast<T>(std::rand()) / static_cast<T>(RAND_MAX) * (b - a) + a;
}

template <>
inline int rand<int>(int a, int b)
{
    assert(a <= b);
    return int(rand<double>(a, b));
}

template <>
inline unsigned rand<unsigned>(unsigned a, unsigned b)
{
    assert(0 <= a && a <= b);
    return unsigned(rand<double>(a, b));
}


template <class T>
T randn()
{
    /*
    This algorithm uses the Marsaglia polar method. For more details, please
    have a look at "A convenient method for generating normal variables" by
    G. Marsaglia and T. A. Bray (SIAM Rev. 6, 260-264, 1964).
    */

    // The polar method yields two random variables per computation. So one
    // random variable is cached each time and returned on the next call.

    using std::log;
    using std::sqrt;

    static bool cache_is_valid = false;
    static T cached_random_variable = 0;

    if (!cache_is_valid)
    {
        T v1, v2, s;

        do
        {
            v1 = rand<T>(-1, 1);
            v2 = rand<T>(-1, 1);

            s = v1 * v1 + v2 * v2;
        }
        while (s >= 1 || s == 0);

        const T t = sqrt(-2 * log(s) / s);

        T random_variable = v1 * t;

        cached_random_variable = v2 * t;
        cache_is_valid = true;

        return random_variable;
    }
    else
    {
        cache_is_valid = false;

        return cached_random_variable;
    }
}

double randn(); // Convenience function for randn<double>().


template <class T>
inline T randn(T mu, T sigma)
{
    return sigma * randn<T>() + mu;
}

template <>
inline int randn(int mu, int sigma)
{
    return int(sigma * randn<double>() + mu);
}


template <class T> int sign(T value)
{
    return (T(0) < value) - (value < T(0));
}


template <class T>
Eigen::Matrix<T, 3, 3> rotationMatrix(const Eigen::Matrix<T, 3, 1> & axis_, double angle)
{
    using Vector3T = Eigen::Matrix<T, 3, 1>;
    using Matrix3T = Eigen::Matrix<T, 3, 3>;

    T axis_magnitude = axis_.norm();
    assert(axis_magnitude > 0); // Axis may not be a null vector.
    Vector3T axis = axis_ / axis_magnitude;

    // Find an orthogonal vector.

    const T eps = std::numeric_limits<T>::epsilon();

    auto isNullvector = [&] (Vector3T & vec) -> bool
    {
        return vec.norm() < 10 * eps;
    };

    auto isCollinear = [&] (Vector3T a, Vector3T b) -> bool
    {
        a.normalize();
        b.normalize();
        return abs(abs(a.dot(b)) - 1) < 10 * eps;
    };

    auto vec = Vector3T::Random().normalized();
    while (isNullvector(vec) || isCollinear(vec, axis))
    {
        vec = Vector3T::Random().normalized();
    }

    auto vec_orthonormal = (vec - vec.dot(axis) * axis).normalized(); // Cf. Gram-Schmidt process
    assert(abs(axis.dot(vec_orthonormal)) < 1000 * eps); // Axis and vec_orthonormal must be orthonormal. Implementation error.

    // Compute the rotation matrix

    // Make a change of basis such that the rotation axis is aligned
    // with the x-axis, rotate around this axis and revert the change of
    // basis.

    Matrix3T M;
    M.row(0) = axis;
    M.row(1) = vec_orthonormal;
    M.row(2) = axis.cross(vec_orthonormal);

    Matrix3T R_x;
    R_x << 1,          0,           0,
           0, cos(angle), -sin(angle),
           0, sin(angle),  cos(angle);

    return M.transpose() * R_x * M;
}


template <class T>
T round(const T number)
{
    using std::ceil;
    using std::floor;

    return (number < 0.0) ? ceil(number - 0.5) : floor(number + 0.5);
}

template <>
inline int round<int>(const int number)
{
    return number;
}


template <class T>
std::tuple<T, T, T> spherical2Cartesian(const T & r, const T & theta, const T & phi)
{
    using ::sin;
    using ::cos;
    double x = r * sin(theta) * cos(phi);
    double y = r * sin(theta) * sin(phi);
    double z = r * cos(theta);
    return std::make_tuple(x, y, z);
}


template <class T>
Coordinate<T> spherical2Cartesian(const Coordinate<T> & point)
{
    Coordinate<T> result(0, 0, 0);
    std::tie(result.x(), result.y(), result.z()) = spherical2Cartesian(point.x(), point.y(), point.z());
    return result;
}


template <class T>
Eigen::Matrix<T, 3, 1> spherical2Cartesian(const Eigen::Matrix<T, 3, 1> & point)
{
    Eigen::Matrix<T, 3, 1> result;
    std::tie(result.x(), result.y(), result.z()) = spherical2Cartesian(point.x(), point.y(), point.z());
    return result;
}


template <class Container, class ValueType> // TODO: C++11 --> class ValueType = Container::value_type
inline ValueType standardDeviation(const Container & container, ValueType null_value = ValueType())
{
    size_t n = container.size();
    assert (n >= 0);

    if (n == 0) return null_value;

    ValueType mean = Tools::mean(container, null_value);
    ValueType variance = null_value;

    for (typename Container::const_iterator it = container.begin(); it != container.end(); ++it)
    {
        variance = variance + (*it - mean) * (*it - mean);
    }

    double c4 = 1.0 - 1.0/(4.0*n) - 7.0/(32.0*n*n) - 19.0/(128.0*n*n*n);

    using std::sqrt;

    return sqrt(variance / (n - 1)) / c4;
}


template <class T>
inline std::string toString (const T & value)
{
    std::stringstream ss;
    ss << value;

    return ss.str();
}


template <class Container, class ValueType> // TODO: C++11 --> class ValueType = Container::value_type
inline ValueType variance(const Container & container, ValueType null_value = ValueType())
{
    assert(container.size() >= 0);

    if (container.size() == 0) return null_value;

    ValueType mean = Tools::mean(container, null_value);
    ValueType variance = null_value;

    for (typename Container::const_iterator it = container.begin(); it != container.end(); ++it)
    {
        variance = variance + (*it - mean) * (*it - mean);
    }

    return variance / (container.size() - 1);
}


template <class T>
std::list<T> vectorToList(const std::vector<T> & vec)
{
    typename std::list<T> lst;

    for (unsigned i = 0; i < vec.size(); ++i)
    {
        lst.push_back(vec[i]);
    }

    return lst;
}


template <class T1, class T2>
std::vector<std::pair<T1, T2> > zip(const std::vector<T1> & v1, const std::vector<T2> & v2)
{
    size_t size_v1 = v1.size();
    size_t size_v2 = v2.size();
    assert(size_v1 == size_v2);

    std::vector<std::pair<T1, T2> > zipped;
    zipped.reserve(size_v1);

    for (size_t i = 0; i < size_v1; ++i)
    {
        zipped.push_back(std::make_pair(v1[i], v2[i]));
    }

    return zipped;
}


} // namspace Tools


} // namespace TRTK


#endif // TOOLS_H_9034987422