Triangularization.h

Go to the documentation of this file.

#pragma once

 
#include <algorithm>
#include <concepts>
#include <type_traits>
 
#include <Eigen/Core>
#include <Eigen/Householder>
 
#include <EmrysFilter/Core/Macros.h>
 
namespace Emrys
{

template <typename M, typename Derived = std::remove_cvref_t<M>>
    requires std::derived_from<Derived, Eigen::MatrixBase<Derived>> && (!std::is_const_v<std::remove_reference_t<M>>)
          && ((Derived::Flags & Eigen::LvalueBit) != 0)
void
triangularize(M && m)
{
    Derived & matrix = m;
    using Scalar = typename Derived::Scalar;
    using RealScalar = typename Eigen::NumTraits<Scalar>::Real;
    using Workspace =
        Eigen::Matrix<Scalar, Derived::ColsAtCompileTime, 1, Eigen::ColMajor, Derived::MaxColsAtCompileTime, 1>;
    Eigen::Index const rows = matrix.rows();
    Eigen::Index const cols = matrix.cols();
    Workspace workspace(cols);
    for (Eigen::Index k = 0; k < std::min(rows, cols); ++k)
    {
        Eigen::Index const below = rows - k - 1;
        Eigen::Index const right = cols - k - 1;
        if (below > 0)
        {
            auto tau = Scalar{};
            auto beta = RealScalar{};
            matrix.col(k).tail(below + 1).makeHouseholderInPlace(tau, beta);
            if (right > 0)
            {
                matrix.bottomRightCorner(below + 1, right)
                    .applyHouseholderOnTheLeft(matrix.col(k).tail(below), tau, workspace.data());
            }
            matrix(k, k) = beta;
            matrix.col(k).tail(below).setZero();
        }
        if (matrix(k, k) < Scalar(0))
        {
            matrix.row(k).tail(cols - k) *= Scalar(-1);
        }
    }
}
 
namespace internal
{

template <typename Derived>
[[nodiscard]] Eigen::Matrix<
    typename Derived::Scalar,
    Derived::RowsAtCompileTime,
    Derived::RowsAtCompileTime,
    Eigen::ColMajor,
    Derived::MaxRowsAtCompileTime,
    Derived::MaxRowsAtCompileTime>
lowerTriangularRoot(Eigen::MatrixBase<Derived> const & a)
{
    using Scalar = typename Derived::Scalar;
    using Transposed = Eigen::Matrix<
        Scalar,
        Derived::ColsAtCompileTime,
        Derived::RowsAtCompileTime,
        Eigen::ColMajor,
        Derived::MaxColsAtCompileTime,
        Derived::MaxRowsAtCompileTime>;
    using Result = Eigen::Matrix<
        Scalar,
        Derived::RowsAtCompileTime,
        Derived::RowsAtCompileTime,
        Eigen::ColMajor,
        Derived::MaxRowsAtCompileTime,
        Derived::MaxRowsAtCompileTime>;
    Transposed buffer = a.transpose();
    triangularize(buffer);
    Eigen::Index const n = a.rows();
    Eigen::Index const k = std::min(a.rows(), a.cols());
    Result factor = Result::Zero(n, n);
    factor.leftCols(k) = buffer.topRows(k).transpose();
    return factor;
}
 
}  // namespace internal
 
}  // namespace Emrys