matrix_utilities.h

Go to the documentation of this file.

/*********************************************************************
 *
 *  Software License Agreement
 *
 *  Copyright (c) 2020,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 *  Authors: Christoph Rösmann
 *********************************************************************/

#ifndef SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_MATRIX_UTILITIES_H_
#define SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_MATRIX_UTILITIES_H_

#include <Eigen/Cholesky>
#include <Eigen/Core>

#include <type_traits>

namespace corbo {

template <typename Derived>
inline bool is_square(const Eigen::MatrixBase<Derived>& matrix)
{
    return matrix.rows() == matrix.cols();
}

inline bool is_square(int numel)
{
    double q = std::sqrt(numel);
    if (q * q != numel) return false;
    return true;
}

template <typename DerivedA, typename DerivedB>
inline bool have_equal_size(const Eigen::MatrixBase<DerivedA>& matrix1, const Eigen::MatrixBase<DerivedB>& matrix2)
{
    return matrix1.rows() == matrix2.rows() && matrix1.cols() == matrix2.cols();
}

template <typename DerivedA, typename DerivedB, typename... Derived>
inline bool have_equal_size(const Eigen::MatrixBase<DerivedA>& matrix1, const Eigen::MatrixBase<DerivedB>& matrix2,
                            const Eigen::MatrixBase<Derived>&... matrices_other)
{
    return have_equal_size(matrix1, matrix2) ? have_equal_size(matrix2, matrices_other...) : false;
}

template <typename Derived>
inline bool is_positive_definite(const Eigen::MatrixBase<Derived>& matrix)
{
    if (!is_square(matrix)) return false;
    Eigen::LLT<Eigen::MatrixXd> cholesky(matrix);
    return cholesky.info() != Eigen::NumericalIssue;
}

template <typename Derived>
inline double compute_condition_number_square_matrix(const Eigen::MatrixBase<Derived>& matrix)
{
    if (!is_square(matrix))
    {
        return -1.0;
    }
    auto& eigenvalues = matrix.eigenvalues();
    auto eig_min      = eigenvalues.minCoeff();
    auto eig_max      = eigenvalues.maxCoeff();
    // if (std::abs(eig_min) < 1e-15)
    //    return eig_max > 0 ? CORBO_INF_DBL : -CORBO_INF_DBL;

    return std::abs(eig_max / eig_min);
}

template <typename Derived>
inline double compute_condition_number(const Eigen::MatrixBase<Derived>& matrix)
{
    if (matrix.rows() == 0 || matrix.cols() == 0) return -1.0;

    const typename Eigen::JacobiSVD<typename Derived::PlainObject>::SingularValuesType& sing_vals = matrix.jacobiSvd().singularValues();
    // Singular values are always sorted in decreasing order.
    double sigma_max = sing_vals[0];
    double sigma_min = sing_vals[sing_vals.size() - 1];
    // if (sigma_min < 1e-15)
    //    return sigma_max > 0 ? CORBO_INF_DBL : -CORBO_INF_DBL;

    return sigma_max / sigma_min;
}

template <typename Derived>
void compute_pseudo_inverse(const Eigen::MatrixBase<Derived>& matrix, Eigen::MatrixBase<Derived>& pinvmat, double tolerance = 1e-6)
{
    Eigen::JacobiSVD<typename Derived::PlainObject> svd(matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
    // TODO(roesmann): do we need FullU and FullV, or is Thin sufficient?

    using SingularValuesType    = typename Eigen::JacobiSVD<typename Derived::PlainObject>::SingularValuesType;
    SingularValuesType sing_inv = svd.singularValues();

    for (int i = 0; i < sing_inv.size(); ++i)
    {
        if (sing_inv[i] > tolerance)
            sing_inv[i] = 1.0 / sing_inv[i];
        else
            sing_inv[i] = 0.0;
    }
    pinvmat.noalias() = svd.matrixV() * sing_inv.asDiagonal() * svd.matrixU().transpose();
}

}  // namespace corbo

#endif  // SRC_NUMERICS_INCLUDE_CORBO_NUMERICS_MATRIX_UTILITIES_H_