Gaussian.h

Go to the documentation of this file.

#pragma once

 
#include <expected>
#include <optional>
#include <type_traits>
#include <utility>
 
#include <Eigen/Cholesky>
 
#include <EmrysFilter/Core/ChiSquared.h>
#include <EmrysFilter/Core/Forwarding.h>
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Print.h>
#include <EmrysFilter/Core/Solve.h>
#include <EmrysFilter/Core/Stamp.h>
#include <EmrysFilter/Distributions/Moments.h>
#include <EmrysFilter/Distributions/Roots.h>
#include <EmrysFilter/Spaces/Composite.h>
#include <EmrysFilter/Spaces/Indexing.h>
#include <EmrysFilter/Spaces/Space.h>
#include <EmrysFilter/Spaces/Subspace.h>
#include <EmrysFilter/Spaces/TagPath.h>
 
namespace Emrys
{
 
template <Space Space_, RealScalar Scalar_>
class Gaussian;
 
template <Space Space_, RealScalar Scalar_>
class GaussianParams;
 
template <typename Source, typename Selection, typename SubSpace>
class GaussianMarginal;

template <Space Space_, RealScalar Scalar_ = double>
struct GaussianConditional
{
    GaussianParams<Space_, Scalar_> conditional;  
    Scalar_ logEvidence;  
};
 
namespace internal
{

struct GaussianAccess
{
    template <typename G>
    [[nodiscard]] static typename G::MomentsType &
    moments(G & gaussian)
    {
        gaussian.m_stamp.touchValues();
        gaussian.m_factorValid = false;
        return gaussian.m_moments;
    }
};

struct FactorizedGaussian
{
    template <typename Space, typename Scalar>
    [[nodiscard]] static Scalar
    logDeterminant(GaussianParams<Space, Scalar> const & gaussian)
    {
        EMRYS_FILTER_ASSERT(gaussian.m_factorValid && "the Gaussian must be factorized");
        return gaussian.m_logDeterminant;
    }

    template <typename Space, typename Scalar>
    [[nodiscard]] static Scalar
    logPeak(GaussianParams<Space, Scalar> const & gaussian)
    {
        EMRYS_FILTER_ASSERT(gaussian.m_factorValid && "the Gaussian must be factorized");
        Scalar const log2Pi = Emrys::log2Pi<Scalar>();
        return Scalar(-0.5)
             * (internal::scalarFromCount<Scalar>(gaussian.differenceDimension()) * log2Pi + gaussian.m_logDeterminant);
    }

    template <typename Space, typename Scalar, typename Derived>
    [[nodiscard]] static Scalar
    squaredMahalanobisDistance(GaussianParams<Space, Scalar> const & gaussian, Eigen::MatrixBase<Derived> const & x)
    {
        EMRYS_FILTER_ASSERT(gaussian.m_factorValid && "the Gaussian must be factorized");
        typename GaussianParams<Space, Scalar>::DifferenceVectorType residual =
            Emrys::minus(gaussian.space(), x, gaussian.mean());
        gaussian.m_llt->matrixL().solveInPlace(residual);
        return residual.squaredNorm();
    }

    template <typename Space, typename Scalar, typename Derived>
    [[nodiscard]] static Scalar
    logDensity(GaussianParams<Space, Scalar> const & gaussian, Eigen::MatrixBase<Derived> const & x)
    {
        return logPeak(gaussian) - Scalar(0.5) * squaredMahalanobisDistance(gaussian, x);
    }

    template <typename Space, typename Scalar>
    [[nodiscard]] static Eigen::LLT<typename GaussianParams<Space, Scalar>::MatrixType> const &
    llt(GaussianParams<Space, Scalar> const & gaussian)
    {
        EMRYS_FILTER_ASSERT(gaussian.m_factorValid && "the Gaussian must be factorized");
        auto const & llt = gaussian.m_llt;
        return *llt;
    }
};
 
template <template <typename, typename, typename> typename Marginal, typename Path, typename Source>
[[nodiscard]] auto
tagMarginal(Source const & source);
 
template <template <typename, typename, typename> typename Marginal, typename Path, typename Source>
[[nodiscard]] auto
elementMarginal(Source const & source, Eigen::Index element);

template <
    bool WithEvidence,
    typename Source,
    typename Target,
    typename Given,
    typename Derived,
    typename TargetSpace,
    typename GivenSpace>
[[nodiscard]] auto
conditionalGaussianIn(
    Source const & source,
    Target const & target,
    Given const & given,
    Eigen::MatrixBase<Derived> const & values,
    TargetSpace const & targetSpace,
    GivenSpace const & givenSpace
)
{
    using Scalar = typename Source::Scalar;
    EMRYS_FILTER_ASSERT(distinctIndices(target.values, given.values) && "target and given indices must be distinct");
    if constexpr (WithEvidence)
    {
        using Result = std::expected<GaussianConditional<TargetSpace, Scalar>, GaussianError>;
        auto result = conditionMomentsWithEvidence(
            source.mean(),
            source.covarianceLower(),
            target,
            given,
            values,
            givenSpace,
            targetSpace
        );
        if (!result)
        {
            return Result{std::unexpect, GaussianError::NotPositiveDefinite};
        }
        return Result{GaussianConditional<TargetSpace, Scalar>{
            {targetSpace, std::move(result->moments.mean), std::move(result->moments.cov)},
            result->logLikelihood
        }};
    }
    else
    {
        auto result =
            conditionMoments(source.mean(), source.covarianceLower(), target, given, values, givenSpace, targetSpace);
        return GaussianParams<TargetSpace, Scalar>{targetSpace, std::move(result.mean), std::move(result.cov)};
    }
}

template <bool WithEvidence, typename GivenPath, typename Source, typename Derived>
[[nodiscard]] auto
tagConditionalGaussian(Source const & source, Eigen::MatrixBase<Derived> const & values)
{
    return tagConditional<GivenPath>(
        source.space(),
        [&](auto const & target, auto const & given, auto const & targetSpace, auto const & givenSpace) {
            return conditionalGaussianIn<WithEvidence>(source, target, given, values, targetSpace, givenSpace);
        }
    );
}
 
}  // namespace internal

template <Space Space_, RealScalar Scalar_ = double>
class GaussianParams
{
public:
 
    using MomentsType = Moments<Space_, Scalar_>;                             
    using Scalar = typename MomentsType::Scalar;                              
    using Index = typename MomentsType::Index;                                
    using VectorType = typename MomentsType::VectorType;                      
    using DifferenceVectorType = typename MomentsType::DifferenceVectorType;  
    using MatrixType = typename MomentsType::MatrixType;                      
    using SpaceType = Space_;                                                 
    using DensityType = Gaussian<Space_, Scalar_>;                            

    static constexpr int ValueDimensionAtCompileTime = MomentsType::ValueDimensionAtCompileTime;

    static constexpr int MaxValueDimensionAtCompileTime = MomentsType::MaxValueDimensionAtCompileTime;

    static constexpr int DifferenceDimensionAtCompileTime = MomentsType::DifferenceDimensionAtCompileTime;

    static constexpr int MaxDifferenceDimensionAtCompileTime = MomentsType::MaxDifferenceDimensionAtCompileTime;
 
    static constexpr bool IsParams = true;     
    static constexpr bool IsInfoForm = false;  
    static constexpr bool IsSqrtForm = false;  

    GaussianParams()
        : GaussianParams{Space_{}}
    {}

    explicit GaussianParams(Index n)
        requires internal::SizedByLength<Space_>
    {
        fitValueDimension(n);
        zeroMoments();
    }

    explicit GaussianParams(MomentsType moments)
        requires internal::SizedByLength<Space_>
        : m_moments{std::move(moments)}
    {
        fitSpace();
    }

    template <MatrixExpression Mean, CovarianceExpression Cov>
        requires internal::SizedByLength<Space_>
    GaussianParams(Mean && mean, Cov && cov)
        : m_moments{std::forward<Mean>(mean), std::forward<Cov>(cov)}
    {
        fitSpace();
    }

    explicit GaussianParams(Space_ space)
        : m_moments{space}
        , m_space{std::move(space)}
    {
        zeroMoments();
        fitSpace();
    }

    GaussianParams(Space_ space, Index n)
        : m_space{std::move(space)}
    {
        fitValueDimension(n);
        zeroMoments();
    }

    GaussianParams(Space_ space, MomentsType moments)
        : m_moments{std::move(moments)}
        , m_space{std::move(space)}
    {
        fitSpace();
    }

    template <MatrixExpression Mean, CovarianceExpression Cov>
    GaussianParams(Space_ space, Mean && mean, Cov && cov)
        : m_moments{std::forward<Mean>(mean), std::forward<Cov>(cov)}
        , m_space{std::move(space)}
    {
        fitSpace();
    }

    GaussianParams(GaussianParams const & other)
        : m_moments{other.m_moments}
        , m_space{other.m_space}
        , m_llt{other.m_factorValid ? other.m_llt : std::nullopt}
        , m_factorValid{other.m_factorValid}
    {
        if (m_factorValid)
        {
            m_logDeterminant = other.m_logDeterminant;
        }
    }

    GaussianParams(GaussianParams && other) noexcept
        : m_moments{std::move(other.m_moments)}
        , m_space{std::move(other.m_space)}
        , m_llt{other.m_factorValid ? std::move(other.m_llt) : std::nullopt}
        , m_factorValid{other.m_factorValid}
    {
        if (m_factorValid)
        {
            m_logDeterminant = other.m_logDeterminant;
        }
        other.m_stamp.touchShape();
        other.m_factorValid = false;
    }

    GaussianParams &
    operator=(GaussianParams const & other)
    {
        m_stamp.touchShape();
        m_moments = other.m_moments;
        m_space = other.m_space;
        if (other.m_factorValid)
        {
            m_llt = other.m_llt;
            m_logDeterminant = other.m_logDeterminant;
        }
        m_factorValid = other.m_factorValid;
        return *this;
    }

    GaussianParams &
    operator=(GaussianParams && other) noexcept
    {
        m_stamp.touchShape();
        other.m_stamp.touchShape();
        m_moments = std::move(other.m_moments);
        m_space = std::move(other.m_space);
        if (other.m_factorValid)
        {
            m_llt = std::move(other.m_llt);
            m_logDeterminant = other.m_logDeterminant;
        }
        m_factorValid = other.m_factorValid;
        other.m_factorValid = false;
        return *this;
    }

    [[nodiscard]] Index
    valueDimension() const
    {
        return Emrys::valueDimension(m_space);
    }

    [[nodiscard]] Index
    differenceDimension() const
    {
        return Emrys::differenceDimension(m_space);
    }

    void
    resize(Index n)
        requires internal::SizedByLength<Space_>
    {
        m_stamp.touchShape();
        fitValueDimension(n);
        m_factorValid = false;
    }

    template <typename Path, typename MeanDerived, typename CovDerived, typename CrossDerived>
    void
    insert(
        Index element,
        Eigen::MatrixBase<MeanDerived> const & mean,
        Eigen::MatrixBase<CovDerived> const & cov,
        Eigen::MatrixBase<CrossDerived> const & cross
    )
    {
        m_stamp.touchShape();
        EMRYS_FILTER_ASSERT(cross.cols() == differenceDimension());
        auto const rows = internal::growBlock<Path>(m_space, element, mean.size());
        EMRYS_FILTER_ASSERT(
            cov.rows() == rows.differenceCount && cov.cols() == rows.differenceCount
            && cross.rows() == rows.differenceCount
        );
        internal::insertRows(m_moments.mean(), rows.start, mean);
        internal::insertSymmetric(m_moments.covarianceLower(), rows.differenceStart, cov, cross);
        normalizeValues(m_space, m_moments.mean());
        m_factorValid = false;
    }

    template <typename Path, typename MeanDerived, typename CovDerived>
    void
    insert(Index element, Eigen::MatrixBase<MeanDerived> const & mean, Eigen::MatrixBase<CovDerived> const & cov)
    {
        using Cross = internal::MatrixOf<
            Scalar,
            Eigen::Dynamic,
            Eigen::Dynamic,
            CovDerived::MaxRowsAtCompileTime,
            MomentsType::MatrixType::MaxColsAtCompileTime>;
        insert<Path>(element, mean, cov, Cross::Zero(cov.rows(), differenceDimension()));
    }

    template <typename Path>
    void
    erase(Index element, Index count = 1)
    {
        m_stamp.touchShape();
        auto const rows = internal::shrinkBlock<Path>(m_space, element, count);
        internal::eraseRows(m_moments.mean(), rows.start, rows.count);
        internal::eraseSymmetric(m_moments.covarianceLower(), rows.differenceStart, rows.differenceCount);
        m_factorValid = false;
    }

    [[nodiscard]] Space_ const &
    space() const
    {
        return m_space;
    }

    [[nodiscard]] MomentsType const &
    moments() const
    {
        return m_moments;
    }

    [[nodiscard]] VectorType const &
    mean() const
    {
        return m_moments.mean();
    }

    [[nodiscard]] MatrixType const &
    covarianceLower() const
    {
        return m_moments.covarianceLower();
    }

    template <typename Path>
    [[nodiscard]] auto
    mean() const
    {
        return mean()(idx<Path>(m_space));
    }

    template <typename Path>
    [[nodiscard]] auto
    mean(Index element) const
    {
        return mean()(idx<Path>(m_space, element));
    }

    template <typename Path>
    [[nodiscard]] auto
    covariance() const
    {
        return internal::lowerSymmetricSelection<DifferenceDimensionAtCompileTime, MaxDifferenceDimensionAtCompileTime>(
            covarianceLower(),
            diffIdx<Path>(m_space)
        );
    }

    template <typename Path>
    [[nodiscard]] auto
    covariance(Index element) const
    {
        return internal::lowerSymmetricSelection<DifferenceDimensionAtCompileTime, MaxDifferenceDimensionAtCompileTime>(
            covarianceLower(),
            diffIdx<Path>(m_space, element)
        );
    }

    template <typename RowPath, typename ColPath>
    [[nodiscard]] auto
    covariance() const
    {
        return internal::lowerSymmetricCross<DifferenceDimensionAtCompileTime, MaxDifferenceDimensionAtCompileTime>(
            covarianceLower(),
            diffIdx<RowPath>(m_space),
            diffIdx<ColPath>(m_space)
        );
    }

    template <MatrixExpression Mean>
    void
    setMean(Mean && mean)
    {
        m_stamp.touchValues();
        EMRYS_FILTER_ASSERT(mean.size() == valueDimension());
        m_moments.mean() = std::forward<Mean>(mean);
        normalizeValues(m_space, m_moments.mean());
    }

    template <CovarianceExpression Cov>
    void
    setCovariance(Cov && cov)
    {
        m_stamp.touchValues();
        EMRYS_FILTER_ASSERT(cov.rows() == differenceDimension() && cov.cols() == differenceDimension());
        m_moments.covarianceLower() = std::forward<Cov>(cov);
        m_factorValid = false;
    }

    void
    scaleToConfidence(Scalar probability)
    {
        m_stamp.touchValues();
        EMRYS_FILTER_ASSERT(probability > 0 && probability < 1 && "the probability must be in (0, 1)");
        if (differenceDimension() == 0)
        {
            return;
        }
        m_moments.covarianceLower() *=
            chiSquaredQuantile<Scalar>(probability, internal::scalarFromCount<Scalar>(differenceDimension()));
        m_factorValid = false;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    covariance() const
    {
        MatrixType result = covarianceLower().template selfadjointView<Eigen::Lower>();
        return result;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    precision() const
    {
        if (auto factorized = factorize(); !factorized)
        {
            return std::unexpected{factorized.error()};
        }
        MatrixType result = MatrixType::Identity(differenceDimension(), differenceDimension());
        internal::solveInPlace(*m_llt, result);
        return result;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    covarianceRoot() const
    {
        if (auto factorized = factorize(); !factorized)
        {
            return std::unexpected{factorized.error()};
        }
        MatrixType result = m_llt->matrixL();
        return result;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    precisionRoot() const
    {
        if (auto factorized = factorize(); !factorized)
        {
            return std::unexpected{factorized.error()};
        }
        MatrixType result = MatrixType::Identity(differenceDimension(), differenceDimension());
        internal::solveInPlace(m_llt->matrixL(), result);
        return result;
    }

    template <typename Path>
    [[nodiscard]] auto
    marginal() const &
    {
        return internal::tagMarginal<GaussianMarginal, Path>(*this);
    }

    template <typename Path>
    void
    marginal() const && = delete;

    template <typename Path>
    [[nodiscard]] auto
    marginal(Index element) const &
    {
        return internal::elementMarginal<GaussianMarginal, Path>(*this, element);
    }

    template <typename Path>
    void
    marginal(Index element) const && = delete;

    template <typename GivenPath, typename Derived>
    [[nodiscard]] auto
    conditional(Eigen::MatrixBase<Derived> const & values) const
    {
        return internal::tagConditionalGaussian<false, GivenPath>(*this, values);
    }

    template <typename GivenPath, typename Derived>
    [[nodiscard]] auto
    conditionalWithEvidence(Eigen::MatrixBase<Derived> const & values) const
    {
        return internal::tagConditionalGaussian<true, GivenPath>(*this, values);
    }

    [[nodiscard]] std::expected<void, GaussianError>
    factorize() const
    {
        if (m_factorValid)
        {
            return {};
        }
        return factorizeCovariance(m_moments.covarianceLower());
    }

    template <typename R>
    [[nodiscard]] std::expected<void, GaussianError>
    factorize(R const & repair)
    {
        static_assert(
            Repair<R>,
            "a density takes a repair, such as roots::ridge(), not axes: its factor is the Cholesky factor "
            "(decisions.md [covariance-repair])"
        );
        auto result = std::as_const(*this).factorize();
        if (result || result.error() != GaussianError::NotPositiveDefinite)
        {
            return result;
        }
        auto repaired = repair.repaired(m_moments.covarianceLower());
        if (!repaired)
        {
            return std::unexpected{repaired.error()};
        }
        result = factorizeCovariance(*repaired);
        if (result)
        {
            m_stamp.touchValues();
            m_moments.covarianceLower() = std::move(*repaired);
        }
        return result;
    }

    [[nodiscard]] std::expected<DensityType, GaussianError>
    toDensity() const &
    {
        if (auto result = factorize(); !result)
        {
            return std::unexpected{result.error()};
        }
        return DensityType{*this};
    }

    void
    toDensity() const && = delete;

    template <typename R>
    [[nodiscard]] std::expected<DensityType, GaussianError>
    toDensity(R const & repair) &
    {
        if (auto result = factorize(repair); !result)
        {
            return std::unexpected{result.error()};
        }
        return DensityType{*this};
    }

    template <typename R>
    void
    toDensity(R const &) && = delete;
 
private:
 
    friend DensityType;                          
    friend struct internal::FactorizedGaussian;  

    template <typename>
    friend class internal::ValuesCheck;

    template <typename>
    friend class internal::ShapeCheck;
 
    friend struct internal::GaussianAccess;

    [[nodiscard]] std::expected<void, GaussianError>
    factorizeCovariance(MatrixType const & cov) const
    {
        if (!m_llt)
        {
            m_llt.emplace(cov.rows());
        }
        m_llt->compute(cov);
        if (m_llt->info() != Eigen::Success)
        {
            return std::unexpected{GaussianError::NotPositiveDefinite};
        }
        // NaN slips through LLT's positivity test but always reaches the diagonal of
        // L, so checking the log-determinant catches any non-finite lower-triangle input.
        Scalar const logDet = Scalar(2) * m_llt->matrixLLT().diagonal().array().log().sum();
        if (!isFinite(logDet))
        {
            return std::unexpected{GaussianError::NonFinite};
        }
        m_logDeterminant = logDet;
        m_factorValid = true;
        return {};
    }

    void
    fitSpace()
    {
        internal::fitValueDimension(m_space, m_moments.valueDimension());
        EMRYS_FILTER_ASSERT(
            m_moments.differenceDimension() == differenceDimension()
            && "the covariance must have a row per difference coordinate of the space"
        );
        normalizeValues(m_space, m_moments.mean());
    }

    void
    fitValueDimension(Index n)
    {
        internal::fitValueDimension(m_space, n);
        m_moments.resize(m_space);
    }

    void
    zeroMoments()
    {
        m_moments.mean().setZero();
        m_moments.covarianceLower().setZero();
    }
 
    MomentsType m_moments;                          
    EMRYS_FILTER_NO_UNIQUE_ADDRESS Space_ m_space;  
 
    // Factorization cache, filled lazily by the const factorize(), and so written through const
    // members (see factorize()).
    mutable std::optional<Eigen::LLT<MatrixType>> m_llt;
 
    mutable Scalar m_logDeterminant{};  
    mutable bool m_factorValid{false};  
    internal::Stamp m_stamp;            
};

template <Space Space_, RealScalar Scalar_ = double>
class Gaussian
{
public:
 
    using ParamsType = GaussianParams<Space_, Scalar_>;                      
    using SpaceType = Space_;                                                
    using MomentsType = typename ParamsType::MomentsType;                    
    using Scalar = typename ParamsType::Scalar;                              
    using Index = typename ParamsType::Index;                                
    using VectorType = typename ParamsType::VectorType;                      
    using DifferenceVectorType = typename ParamsType::DifferenceVectorType;  
    using MatrixType = typename ParamsType::MatrixType;                      
    using LltType = Eigen::LLT<MatrixType>;                                  

    static constexpr int ValueDimensionAtCompileTime = ParamsType::ValueDimensionAtCompileTime;

    static constexpr int MaxValueDimensionAtCompileTime = ParamsType::MaxValueDimensionAtCompileTime;

    static constexpr int DifferenceDimensionAtCompileTime = ParamsType::DifferenceDimensionAtCompileTime;

    static constexpr int MaxDifferenceDimensionAtCompileTime = ParamsType::MaxDifferenceDimensionAtCompileTime;
 
    static constexpr bool IsParams = false;  

    [[nodiscard]] Index
    valueDimension() const
    {
        return source().valueDimension();
    }

    [[nodiscard]] Index
    differenceDimension() const
    {
        return source().differenceDimension();
    }

    [[nodiscard]] Space_ const &
    space() const
    {
        return source().space();
    }

    [[nodiscard]] MomentsType const &
    moments() const
    {
        return source().moments();
    }

    [[nodiscard]] VectorType const &
    mean() const
    {
        return source().mean();
    }

    [[nodiscard]] MatrixType const &
    covarianceLower() const
    {
        return source().covarianceLower();
    }

    template <typename Path>
    [[nodiscard]] auto
    mean() const
    {
        return source().template mean<Path>();
    }

    template <typename Path>
    [[nodiscard]] auto
    mean(Index element) const
    {
        return source().template mean<Path>(element);
    }

    template <typename Path>
    [[nodiscard]] auto
    covariance() const
    {
        return source().template covariance<Path>();
    }

    template <typename Path>
    [[nodiscard]] auto
    covariance(Index element) const
    {
        return source().template covariance<Path>(element);
    }

    template <typename RowPath, typename ColPath>
    [[nodiscard]] auto
    covariance() const
    {
        return source().template covariance<RowPath, ColPath>();
    }

    [[nodiscard]] LltType const &
    llt() const
    {
        auto const & llt = source().m_llt;
        return *llt;
    }

    [[nodiscard]] Scalar
    logDeterminant() const
    {
        return source().m_logDeterminant;
    }

    [[nodiscard]] Scalar
    logPeak() const
    {
        return internal::FactorizedGaussian::logPeak(source());
    }

    template <typename Derived>
    [[nodiscard]] Scalar
    squaredMahalanobisDistance(Eigen::MatrixBase<Derived> const & x) const
    {
        static_assert(Derived::IsVectorAtCompileTime, "squaredMahalanobisDistance() expects a vector");
        EMRYS_FILTER_ASSERT(x.size() == valueDimension());
        return internal::FactorizedGaussian::squaredMahalanobisDistance(source(), x);
    }

    template <typename Derived>
    [[nodiscard]] Scalar
    confidence(Eigen::MatrixBase<Derived> const & x) const
    {
        if (differenceDimension() == 0)
        {
            return Scalar(0);
        }
        return chiSquaredCdf<Scalar>(
            squaredMahalanobisDistance(x),
            internal::scalarFromCount<Scalar>(differenceDimension())
        );
    }

    template <typename Derived>
    [[nodiscard]] bool
    contains(Eigen::MatrixBase<Derived> const & x, Scalar probability) const
    {
        EMRYS_FILTER_ASSERT(probability > 0 && probability < 1 && "the probability must be in (0, 1)");
        if (differenceDimension() == 0)
        {
            return true;
        }
        return squaredMahalanobisDistance(x)
            <= chiSquaredQuantile<Scalar>(probability, internal::scalarFromCount<Scalar>(differenceDimension()));
    }

    template <typename Derived>
    [[nodiscard]] Scalar
    logDensity(Eigen::MatrixBase<Derived> const & x) const
    {
        return logPeak() - Scalar(0.5) * squaredMahalanobisDistance(x);
    }

    template <typename Derived>
    [[nodiscard]] Scalar
    density(Eigen::MatrixBase<Derived> const & x) const
    {
        return Eigen::numext::exp(logDensity(x));
    }

    template <typename Path>
    [[nodiscard]] auto
    marginal() const
    {
        return internal::tagMarginal<GaussianMarginal, Path>(*this);
    }

    template <typename Path>
    [[nodiscard]] auto
    marginal(Index element) const
    {
        return internal::elementMarginal<GaussianMarginal, Path>(*this, element);
    }

    template <typename GivenPath, typename Derived>
    [[nodiscard]] auto
    conditional(Eigen::MatrixBase<Derived> const & values) const
    {
        return internal::tagConditionalGaussian<false, GivenPath>(*this, values);
    }

    template <typename GivenPath, typename Derived>
    [[nodiscard]] auto
    conditionalWithEvidence(Eigen::MatrixBase<Derived> const & values) const
    {
        return internal::tagConditionalGaussian<true, GivenPath>(*this, values);
    }
 
private:
 
    friend ParamsType;  

    explicit Gaussian(ParamsType const & source)
        : m_check{source}
    {}

    [[nodiscard]] ParamsType const &
    source() const
    {
        return m_check.get();
    }
 
    internal::ValuesCheck<ParamsType> m_check;  
};

template <typename Source, typename Selection, typename SubSpace>
class GaussianMarginal
{
public:
 
    using Scalar = typename Source::Scalar;  
    using Index = Eigen::Index;              
    using SpaceType = SubSpace;              

    static constexpr int ValueDimensionAtCompileTime = valueDimensionAtCompileTime<SubSpace>;

    static constexpr int MaxValueDimensionAtCompileTime = maxValueDimensionAtCompileTime<SubSpace>;

    static constexpr int DifferenceDimensionAtCompileTime = valueDimensionAtCompileTime<DifferenceSpace<SubSpace>>;

    static constexpr int MaxDifferenceDimensionAtCompileTime =
        maxValueDimensionAtCompileTime<DifferenceSpace<SubSpace>>;
 
    using ParamsType = GaussianParams<SubSpace, Scalar>;   
    using MomentsType = typename ParamsType::MomentsType;  
    using VectorType = typename ParamsType::VectorType;    
    using MatrixType = typename ParamsType::MatrixType;    
 
    static constexpr bool IsInfoForm = false;  
    static constexpr bool IsSqrtForm = false;  

    using ResolvedType = typename internal::SelectionHolder<Source, Selection, SubSpace>::ResolvedType;

    using IndexVectorType = decltype(ResolvedType::indices);

    using DifferenceIndexVectorType = decltype(ResolvedType::differenceIndices);

    GaussianMarginal(Source const & source, Selection const & selection, SubSpace space)
        requires(!internal::isSharedSelection<Selection>)
        : m_holder{source}
        , m_selection{source, selection, std::move(space)}
    {}

    GaussianMarginal(Source const & source, Selection shared)
        requires(internal::isSharedSelection<Selection>)
        : m_holder{source}
        , m_selection{shared}
    {}

    [[nodiscard]] Index
    valueDimension() const
    {
        return indices().size();
    }

    [[nodiscard]] Index
    differenceDimension() const
    {
        return differenceIndices().size();
    }

    [[nodiscard]] SubSpace const &
    space() const
    {
        return m_selection.get().space;
    }

    [[nodiscard]] IndexVectorType const &
    indices() const
    {
        return m_selection.get().indices;
    }

    [[nodiscard]] DifferenceIndexVectorType const &
    differenceIndices() const
    {
        return m_selection.get().differenceIndices;
    }

    [[nodiscard]] auto
    mean() const
    {
        return source().mean()(indices());
    }

    [[nodiscard]] auto
    covarianceLower() const
    {
        return internal::lowerSymmetricBlock(source().covarianceLower(), differenceIndices(), differenceIndices());
    }

    [[nodiscard]] MomentsType
    moments() const
    {
        return MomentsType{mean(), covarianceLower()};
    }

    [[nodiscard]] ParamsType
    copy() const
    {
        return ParamsType{space(), moments()};
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    covariance() const
    {
        MatrixType result = covarianceLower();
        return result;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    precision() const
    {
        auto const llt = factorization();
        if (!llt)
        {
            return std::unexpected{llt.error()};
        }
        MatrixType result = MatrixType::Identity(differenceDimension(), differenceDimension());
        internal::solveInPlace(*llt, result);
        return result;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    covarianceRoot() const
    {
        auto const llt = factorization();
        if (!llt)
        {
            return std::unexpected{llt.error()};
        }
        MatrixType result = llt->matrixL();
        return result;
    }

    [[nodiscard]] std::expected<MatrixType, GaussianError>
    precisionRoot() const
    {
        auto const llt = factorization();
        if (!llt)
        {
            return std::unexpected{llt.error()};
        }
        MatrixType result = MatrixType::Identity(differenceDimension(), differenceDimension());
        internal::solveInPlace(llt->matrixL(), result);
        return result;
    }
 
private:

    [[nodiscard]] Source const &
    source() const
    {
        return m_holder.get();
    }

    [[nodiscard]] std::expected<Eigen::LLT<MatrixType>, GaussianError>
    factorization() const
    {
        auto llt = Eigen::LLT<MatrixType>{MatrixType{covarianceLower()}};
        if (llt.info() != Eigen::Success)
        {
            return std::unexpected{GaussianError::NotPositiveDefinite};
        }
        // As in GaussianParams::factorize(): NaN passes LLT's test but reaches the diagonal.
        if (!allFinite(llt.matrixLLT().diagonal()))
        {
            return std::unexpected{GaussianError::NonFinite};
        }
        return llt;
    }
 
    internal::HolderFor<Source> m_holder;  
    internal::SelectionHolder<Source, Selection, SubSpace> m_selection;  
};
 
namespace internal
{

template <template <typename, typename, typename> typename Marginal, typename Path, typename Source>
[[nodiscard]] auto
tagMarginal(Source const & source)
{
    auto const selection =
        Coordinates{asIndexSet(idx<Path>(source.space())), asIndexSet(diffIdx<Path>(source.space()))};
    auto space = pathSubspace<Path>(source.space());
    return Marginal<Source, std::remove_cvref_t<decltype(selection)>, decltype(space)>{
        source,
        selection,
        std::move(space)
    };
}

template <template <typename, typename, typename> typename Marginal, typename Path, typename Source>
[[nodiscard]] auto
elementMarginal(Source const & source, Eigen::Index element)
{
    auto const selection =
        Coordinates{asIndexSet(idx<Path>(source.space(), element)), asIndexSet(diffIdx<Path>(source.space(), element))};
    auto space = pathElementSubspace<Path>(source.space());
    return Marginal<Source, std::remove_cvref_t<decltype(selection)>, decltype(space)>{
        source,
        selection,
        std::move(space)
    };
}
 
}  // namespace internal
 
}  // namespace Emrys
 
#ifndef EMRYS_FILTER_NO_IO
 
namespace Emrys
{
 
namespace internal
{

template <typename Space_, typename Scalar_>
void
describe(std::ostream & stream, GaussianParams<Space_, Scalar_> const & gaussian, Indent indent)
{
    stream << indent << "Gaussian";
    printDimension(stream, gaussian.valueDimension(), gaussian.differenceDimension());
    printMoments(stream, indent.deeper(), gaussian.moments());
}
 
}  // namespace internal
 
}  // namespace Emrys
 
#endif  // EMRYS_FILTER_NO_IO