utils.h

Go to the documentation of this file.

/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */
 
#ifndef UTILS_H
#define UTILS_H
 
#include <Eigen/Dense>
 
#include <chrono>
#include <cmath>
#include <memory>
#include <string>
 
namespace bfl
{
namespace utils
{
 
template<bool B, class T = void >
using enable_if_t = typename std::enable_if<B,T>::type;
 
 
template<typename T, typename ...Args>
std::unique_ptr<T> make_unique(Args&& ...args)
{
    return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
}
 
 
template<typename Derived>
double log_sum_exp(const Eigen::MatrixBase<Derived>& data)
{
    double max = static_cast<double>(data.maxCoeff());
 
    return max + std::log(static_cast<double>((data.array() - max).exp().sum()));
}
 
 
 
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)
{
    Eigen::Matrix<DerivedScalar, 3, Eigen::Dynamic> rotation_vectors(3, quaternion.cols());
 
    for (std::size_t i = 0; i < quaternion.cols(); ++i)
    {
        const DerivedScalar norm_n = quaternion.col(i).tail(3).norm();
        if (norm_n > 1e-4)
        {
            const DerivedScalar w = quaternion.col(i)(0);
            if (w < 0)
                rotation_vectors.col(i) = - 2.0 * std::acos(-w) * quaternion.col(i).tail(3) / norm_n;
            else
                rotation_vectors.col(i) = 2.0 * std::acos(w) * quaternion.col(i).tail(3) / norm_n;
        }
        else
            rotation_vectors.col(i) = Eigen::Matrix<DerivedScalar, 3, 1>::Zero();
    }
 
    return rotation_vectors;
}
 
 
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)
{
    Eigen::Matrix<DerivedScalar, 4, Eigen::Dynamic> quaternions(4, rotation_vector.cols());
    for (std::size_t i = 0; i < rotation_vector.cols(); ++i)
    {
        const DerivedScalar norm_r = rotation_vector.col(i).norm();
        if (norm_r > 1e-4)
        {
            quaternions.col(i)(0) = std::cos(norm_r / 2.0);
            quaternions.col(i).tail(3) = std::sin(norm_r / 2.0) * rotation_vector.col(i) / norm_r;
        }
        else
        {
            quaternions.col(i)(0) = 1.0;
            quaternions.col(i).tail(3) = Eigen::Matrix<DerivedScalar, 3, 1>::Zero();
        }
    }
 
    return quaternions;
}
 
 
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)
{
    /* Move rotation vectors to their quaternionic representation. */
    Eigen::Matrix<DerivedScalar, 4, Eigen::Dynamic> vector_as_quaternion = rotation_vector_to_quaternion(rotation_vector);
 
    Eigen::Quaternion<DerivedScalar> q_right(quaternion.col(0)(0), quaternion.col(0)(1), quaternion.col(0)(2), quaternion.col(0)(3));
 
    Eigen::Matrix<DerivedScalar, 4, Eigen::Dynamic> quaternions(4, rotation_vector.cols());
    for (std::size_t i = 0; i < rotation_vector.cols(); ++i)
    {
        Eigen::Quaternion<DerivedScalar> q_left(vector_as_quaternion.col(i)(0), vector_as_quaternion.col(i)(1), vector_as_quaternion.col(i)(2), vector_as_quaternion.col(i)(3));
        Eigen::Quaternion<DerivedScalar> sum = q_left * q_right;
 
        quaternions.col(i)(0) = sum.w();
        quaternions.col(i).tail(3) = sum.vec();
    }
 
    return quaternions;
}
 
 
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)
{
    Eigen::Matrix<DerivedScalar, 4, Eigen::Dynamic> products(4, quaternion_left.cols());
 
    Eigen::Quaternion<DerivedScalar> q_right(quaternion_right.col(0)(0), quaternion_right.col(0)(1), quaternion_right.col(0)(2), quaternion_right.col(0)(3));
    Eigen::Quaternion<DerivedScalar> q_right_conj = q_right.conjugate();
 
    /* Products between each left quaternion and the conjugated right quaternion. */
    for (std::size_t i = 0; i < quaternion_left.cols(); ++i)
    {
        Eigen::Quaternion<DerivedScalar> q_left(quaternion_left.col(i)(0), quaternion_left.col(i)(1), quaternion_left.col(i)(2), quaternion_left.col(i)(3));
        Eigen::Quaternion<DerivedScalar> product = q_left * q_right_conj;
 
        products.col(i)(0) = product.w();
        products.col(i).tail(3) = product.vec();
    }
 
    /* Express displacements in the tangent space. */
    return quaternion_to_rotation_vector(products);
}
 
 
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)
{
    /* Weighted outer product of quaternions. */
    Eigen::Matrix<DerivedScalar, 4, 4> outer_product_mean = Eigen::Matrix<DerivedScalar, 4, 4>::Zero();
    for (std::size_t i = 0; i < weight.rows(); ++i)
        outer_product_mean.noalias() += weight.col(0)(i) * quaternion.col(i) * quaternion.col(i).transpose();
 
    /* Take the weighted mean as the eigenvector corresponding to the maximum eigenvalue. */
    Eigen::EigenSolver<Eigen::Matrix<DerivedScalar, 4, 4>> eigen_solver(outer_product_mean);
    Eigen::Matrix<std::complex<DerivedScalar>, 4, 1> eigenvalues(eigen_solver.eigenvalues());
    int maximum_index;
    eigenvalues.real().maxCoeff(&maximum_index);
    return eigen_solver.eigenvectors().real().block(0, maximum_index, 4, 1);
}
 
 
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)
{
    const auto diff = input.colwise() - mean;
 
    Eigen::VectorXd values(diff.cols());
    for (std::size_t i = 0; i < diff.cols(); i++)
        values(i) = - 0.5 * (static_cast<double>(diff.rows()) * std::log(2.0 * M_PI) + std::log(covariance.determinant()) + (diff.col(i).transpose() * covariance.inverse() * diff.col(i)));
 
    return values;
}
 
 
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)
{
    std::size_t input_size = input.rows();
    std::size_t block_size = R.rows();
    std::size_t num_blocks = input_size / block_size;
 
    const auto diff = input.colwise() - mean;
 
    /* Evaluate inv(R) */
    Eigen::MatrixXd inv_R(block_size, input_size);
    if (R.cols() == block_size)
    {
        // The diagonal blocks of R are all equal
        Eigen::MatrixXd inv_R_single = R.inverse();
        for (std::size_t i = 0; i < num_blocks; i++)
        {
            inv_R.block(0, block_size * i, block_size, block_size) = inv_R_single;
        }
    }
    else
    {
        // The diagonal blocks of R are different
        for (std::size_t i = 0; i < num_blocks; i++)
        {
            inv_R.block(0, block_size * i, block_size, block_size) = R.block(0, block_size * i, block_size, block_size).inverse();
        }
    }
 
    /* Evaluate V * inv(R) */
    Eigen::MatrixXd V_inv_R(V.rows(), V.cols());
    for (std::size_t i = 0; i < V.cols() / block_size; i++)
    {
        V_inv_R.middleCols(i * block_size, block_size) = V.middleCols(i * block_size, block_size) * inv_R.block(0, block_size * i, block_size, block_size);
    }
 
    /* Evaluate diff^{T} * inv(R) */
    Eigen::MatrixXd diff_T_inv_R(input.cols(), input.rows());
    for (std::size_t i = 0; i < num_blocks; i++)
    {
        diff_T_inv_R.middleCols(i * block_size, block_size) = diff.middleRows(i * block_size, block_size).transpose() * inv_R.block(0, block_size * i, block_size, block_size);
    }
 
    /* Evaluate I + V * inv(R) * U */
    Eigen::MatrixXd I_V_inv_R_U = Eigen::MatrixXd::Identity(V.rows(), V.rows()) + V_inv_R * U;
 
    Eigen::VectorXd weighted_diffs(input.cols());
    for (std::size_t i = 0; i < weighted_diffs.size(); i++)
        weighted_diffs(i) = diff_T_inv_R.row(i) * (Eigen::MatrixXd::Identity(U.rows(), U.rows()) - U * I_V_inv_R_U.inverse() * V_inv_R) * diff.col(i);
 
    double det_S;
    double det_R = 1.0;
    if (R.cols() == block_size)
        det_R = std::pow(R.determinant(), num_blocks);
    else
        for (std::size_t i = 0; i < num_blocks; i++)
            det_R *= R.block(0, block_size * i, block_size, block_size).determinant();
 
    det_S = det_R * I_V_inv_R_U.determinant();
 
    /* Evaluate the full logarithm density */
    Eigen::VectorXd values(input.cols());
    for (std::size_t i = 0; i < input.cols(); i++)
        values(i) = - 0.5 * (static_cast<double>(diff.rows()) * std::log(2.0 * M_PI) + std::log(det_S) + weighted_diffs(i));
 
    return values;
}
 
 
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)
{
    return multivariate_gaussian_log_density(input, mean, covariance).array().exp();
}
 
 
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)
{
    return multivariate_gaussian_log_density_UVR(input, mean, U, V, R).array().exp();
}
 
 
template<typename timetype = std::chrono::milliseconds>
class CpuTimer
{
public:
    void start()
    {
        start_time_ = std::chrono::steady_clock::now();
 
        running_ = true;
    }
 
 
    void stop()
    {
        stop_time_ = std::chrono::steady_clock::now();
 
        running_ = false;
    }
 
 
    double elapsed()
    {
        std::chrono::steady_clock::duration time_span;
 
        if (!running_)
            time_span = stop_time_ - start_time_;
        else
            time_span = std::chrono::steady_clock::now() - start_time_;
 
        return static_cast<double>(time_span.count()) * std::chrono::steady_clock::period::num / std::chrono::steady_clock::period::den * timetype::period::den;
    }
 
 
    double now()
    {
        return static_cast<double>(std::chrono::steady_clock::now().time_since_epoch().count()) * std::chrono::steady_clock::period::num / std::chrono::steady_clock::period::den * timetype::period::den;
    }
 
 
    bool is_running()
    {
        return running_;
    }
 
private:
    std::chrono::steady_clock::time_point start_time_ = std::chrono::steady_clock::now();
 
 
    std::chrono::steady_clock::time_point stop_time_ = start_time_;
 
 
    bool running_ = false;
};
 
 
inline std::string throw_message(const std::string& from_where, const std::string& error_message, const std::string& data_log = "")
{
    if (from_where.empty() || error_message.empty())
        return "UTILS::THROW_MESSAGE::EMPTY_THROW_REPORT";
 
    std::string message;
 
    message = "ERROR::" + from_where + "\n";
    message += "MESSAGE:\n";
    message += "\t" + error_message + "\n";
 
    if (!data_log.empty())
    {
        message += "LOG:\n";
        message += "\t" + data_log + "\n";
    }
 
    return message;
}
 
}
}
 
 
#endif /* UTILS_H */