Reset.h

Go to the documentation of this file.

#pragma once

 
#include <cstddef>
#include <tuple>
#include <type_traits>
#include <utility>
 
#include <Eigen/LU>
 
#include <EmrysFilter/Core/Macros.h>
#include <EmrysFilter/Core/Math.h>
#include <EmrysFilter/Core/Multiply.h>
#include <EmrysFilter/Core/Solve.h>
#include <EmrysFilter/Spaces/Composite.h>
#include <EmrysFilter/Spaces/Space.h>
 
namespace Emrys
{
 
namespace internal
{

template <typename S>
concept HasResetJacobian = requires(S const & space, Eigen::MatrixXd & block) {
    space.resetJacobian(block.col(0), block.col(0), block.middleRows(0, 0));
};

template <typename S>
concept DeclaresIdentityReset = requires { requires bool(S::identityReset); };
 
template <typename S>
consteval bool
identityResetOf();

template <typename Blocks>
struct AllIdentityReset;

template <typename... Blocks>
struct AllIdentityReset<std::tuple<Blocks...>> : std::bool_constant<(identityResetOf<Blocks>() && ...)>
{};

template <typename S>
consteval bool
identityResetOf()
{
    if constexpr (DeclaresIdentityReset<S>)
    {
        return true;
    }
    else if constexpr (HasResetJacobian<S>)
    {
        return false;
    }
    else if constexpr (IsComposite<S>)
    {
        return AllIdentityReset<CompositeBlocks<S>>::value;
    }
    else if constexpr (IsRepeated<S>)
    {
        return identityResetOf<RepeatedBlock<S>>();
    }
    else
    {
        return !HasPlus<S>;
    }
}
 
}  // namespace internal

template <typename S>
inline constexpr bool hasIdentityReset = internal::identityResetOf<S>();
 
namespace internal
{

template <typename Block, typename X, typename D, typename Moved, typename Out>
void
blockResetJacobian(
    Block const & block,
    Eigen::MatrixBase<X> const & x,
    Eigen::MatrixBase<D> const & d,
    [[maybe_unused]] Eigen::MatrixBase<Moved> const & moved,
    Eigen::MatrixBase<Out> const & out
)
{
    if constexpr (HasResetJacobian<Block>)
    {
        block.resetJacobian(x, d, out);
    }
    else
    {
        using Scalar = typename Out::Scalar;
        constexpr int K = valueDimensionAtCompileTime<DifferenceSpace<Block>>;
        constexpr int KMax = maxValueDimensionAtCompileTime<DifferenceSpace<Block>>;
        constexpr int V = valueDimensionAtCompileTime<Block>;
        constexpr int VMax = maxValueDimensionAtCompileTime<Block>;
        using Dual = Eigen::AutoDiffScalar<Eigen::Matrix<Scalar, K, 1, Eigen::ColMajor, KMax, 1>>;
        using DualValue = Eigen::Matrix<Dual, V, 1, Eigen::ColMajor, VMax, 1>;
        using DualDifference = Eigen::Matrix<Dual, K, 1, Eigen::ColMajor, KMax, 1>;
        Eigen::Index const k = differenceDimension(block);
        auto step = DualDifference{};
        step.resize(k);
        for (Eigen::Index i = 0; i < k; ++i)
        {
            step(i) = Dual{Scalar(d(i)), int(k), int(i)};
        }
        DualValue const start = x.template cast<Dual>();
        DualValue const end = moved.template cast<Dual>();
        DualValue perturbed = DualValue::Zero(start.size());
        plusInto(block, start, step, perturbed);
        DualDifference error = DualDifference::Zero(k);
        minusInto(block, perturbed, end, error);
        auto & o = writable(out);
        for (Eigen::Index r = 0; r < k; ++r)
        {
            // Of a run-time size, the derivatives of a coordinate that does not depend on the step
            // would be empty; the Jacobian of a space whose minus() undoes its plus() is invertible.
            EMRYS_FILTER_ASSERT(
                error(r).derivatives().size() == k && "each coordinate of the difference must depend on the step"
            );
            o.row(r) = error(r).derivatives().transpose();
        }
    }
}

template <typename S, typename F>
void
forEachResetBlock(S const & space, F && f, Eigen::Index valueOffset = 0, Eigen::Index differenceOffset = 0)
{
    if constexpr (hasIdentityReset<S>)
    {}
    else if constexpr (IsComposite<S> && !HasResetJacobian<S>)
    {
        [&]<std::size_t... I>(std::index_sequence<I...>) {
            ((forEachResetBlock(space.template block<I>(), f, valueOffset, differenceOffset),
              valueOffset += valueDimension(space.template block<I>()),
              differenceOffset += differenceDimension(space.template block<I>())),
             ...);
        }(std::make_index_sequence<std::tuple_size_v<CompositeBlocks<S>>>{});
    }
    else if constexpr (IsRepeated<S> && !HasResetJacobian<S>)
    {
        auto const & block = space.block();
        Eigen::Index const values = valueDimension(block);
        Eigen::Index const differences = differenceDimension(block);
        for (Eigen::Index k = 0; k < space.count(); ++k)
        {
            forEachResetBlock(block, f, valueOffset + k * values, differenceOffset + k * differences);
        }
    }
    else
    {
        f(space, valueOffset, differenceOffset);
    }
}

template <typename Cov, typename G>
void
congruence(Eigen::MatrixBase<Cov> const & cov, Eigen::Index offset, G const & g)
{
    using Scalar = typename Cov::Scalar;
    constexpr int K = G::RowsAtCompileTime;
    constexpr int KMax = G::MaxRowsAtCompileTime;
    constexpr int N = Cov::ColsAtCompileTime;
    constexpr int NMax = Cov::MaxColsAtCompileTime;
    auto & p = writable(cov);
    Eigen::Index const k = g.rows();
    auto strip = MatrixOf<Scalar, K, N, KMax, NMax>{};
    strip.resize(k, p.cols());
    strip.noalias() = multiply(g, middleRowsOf<K>(p, offset, k));
    middleRowsOf<K>(p, offset, k) = strip;
    middleColsOf<K>(p, offset, k) = strip.transpose();
    auto diagonal = MatrixOf<Scalar, K, K, KMax, KMax>{};
    diagonal.resize(k, k);
    diagonal.noalias() = multiply(middleColsOf<K>(strip, offset, k), g.transpose());
    blockOf<K, K>(p, offset, offset, k, k) = Scalar(0.5) * (diagonal + diagonal.transpose());
}

template <bool Inverse = false, typename S, typename X, typename D, typename Moved, typename Cov>
void
resetCovarianceAt(
    S const & space,
    Eigen::MatrixBase<X> const & x,
    Eigen::MatrixBase<D> const & d,
    Eigen::MatrixBase<Moved> const & moved,
    Eigen::MatrixBase<Cov> const & cov
)
{
    if constexpr (!hasIdentityReset<S>)
    {
        EMRYS_FILTER_ASSERT(
            x.size() == valueDimension(space) && moved.size() == valueDimension(space)
            && d.size() == differenceDimension(space) && cov.rows() == differenceDimension(space)
            && cov.cols() == differenceDimension(space) && "the value, step, and covariance must fit the space"
        );
        forEachResetBlock(space, [&](auto const & block, Eigen::Index values, Eigen::Index differences) {
            using Block = std::remove_cvref_t<decltype(block)>;
            using Scalar = typename Cov::Scalar;
            constexpr int K = valueDimensionAtCompileTime<DifferenceSpace<Block>>;
            constexpr int V = valueDimensionAtCompileTime<Block>;
            constexpr int KMax = maxValueDimensionAtCompileTime<DifferenceSpace<Block>>;
            Eigen::Index const v = valueDimension(block);
            Eigen::Index const k = differenceDimension(block);
            MatrixOf<Scalar, K, K, KMax, KMax> g = MatrixOf<Scalar, K, K, KMax, KMax>::Zero(k, k);
            blockResetJacobian(
                block,
                middleRowsOf<V>(x, values, v),
                middleRowsOf<K>(d, differences, k),
                middleRowsOf<V>(moved, values, v),
                g
            );
            if constexpr (Inverse)
            {
                using Matrix = MatrixOf<Scalar, K, K, KMax, KMax>;
                g = inverseFrom<Matrix>(Eigen::PartialPivLU<Matrix>{g});
            }
            congruence(cov, differences, g);
        });
    }
}

template <typename S, typename X, typename D, typename Moved, typename F>
void
forEachInverseResetJacobian(
    S const & space,
    Eigen::MatrixBase<X> const & x,
    Eigen::MatrixBase<D> const & d,
    Eigen::MatrixBase<Moved> const & moved,
    F && f
)
{
    if constexpr (!hasIdentityReset<S>)
    {
        EMRYS_FILTER_ASSERT(
            x.size() == valueDimension(space) && moved.size() == valueDimension(space)
            && d.size() == differenceDimension(space) && "the value and step must fit the space"
        );
        forEachResetBlock(space, [&](auto const & block, Eigen::Index values, Eigen::Index differences) {
            using Block = std::remove_cvref_t<decltype(block)>;
            using Scalar = typename X::Scalar;
            constexpr int K = valueDimensionAtCompileTime<DifferenceSpace<Block>>;
            constexpr int V = valueDimensionAtCompileTime<Block>;
            constexpr int KMax = maxValueDimensionAtCompileTime<DifferenceSpace<Block>>;
            Eigen::Index const v = valueDimension(block);
            Eigen::Index const k = differenceDimension(block);
            MatrixOf<Scalar, K, K, KMax, KMax> g = MatrixOf<Scalar, K, K, KMax, KMax>::Zero(k, k);
            blockResetJacobian(
                block,
                middleRowsOf<V>(x, values, v),
                middleRowsOf<K>(d, differences, k),
                middleRowsOf<V>(moved, values, v),
                g
            );
            using Matrix = MatrixOf<Scalar, K, K, KMax, KMax>;
            g = inverseFrom<Matrix>(Eigen::PartialPivLU<Matrix>{g});
            f(differences, g);
        });
    }
}

template <typename S, typename X, typename D, typename Moved, typename Info, typename Vector>
void
resetInfoAt(
    S const & space,
    Eigen::MatrixBase<X> const & x,
    Eigen::MatrixBase<D> const & d,
    Eigen::MatrixBase<Moved> const & moved,
    Eigen::MatrixBase<Info> const & info,
    Eigen::MatrixBase<Vector> const & vector
)
{
    forEachInverseResetJacobian(space, x, d, moved, [&](Eigen::Index differences, auto const & gInverse) {
        using GInverse = std::remove_cvref_t<decltype(gInverse)>;
        GInverse const gInverseTransposed = gInverse.transpose();
        congruence(info, differences, gInverseTransposed);
        auto & eta = writable(vector);
        Eigen::Index const k = gInverse.rows();
        middleRowsOf<GInverse::RowsAtCompileTime>(eta, differences, k) =
            gInverseTransposed * middleRowsOf<GInverse::RowsAtCompileTime>(eta, differences, k);
    });
}
 
}  // namespace internal

template <Space S, typename XDerived, typename DDerived>
[[nodiscard]] auto
resetJacobian(S const & space, Eigen::MatrixBase<XDerived> const & x, Eigen::MatrixBase<DDerived> const & d)
{
    using Scalar = typename XDerived::Scalar;
    constexpr int N = valueDimensionAtCompileTime<DifferenceSpace<S>>;
    constexpr int NMax = maxValueDimensionAtCompileTime<DifferenceSpace<S>>;
    Eigen::Index const n = differenceDimension(space);
    auto result = internal::MatrixOf<Scalar, N, N, NMax, NMax>{};
    result.setIdentity(n, n);
    if constexpr (!hasIdentityReset<S>)
    {
        auto const moved = Emrys::plus(space, x, d);
        internal::forEachResetBlock(space, [&](auto const & block, Eigen::Index values, Eigen::Index differences) {
            using Block = std::remove_cvref_t<decltype(block)>;
            constexpr int K = valueDimensionAtCompileTime<DifferenceSpace<Block>>;
            constexpr int V = valueDimensionAtCompileTime<Block>;
            Eigen::Index const v = valueDimension(block);
            Eigen::Index const k = differenceDimension(block);
            internal::blockResetJacobian(
                block,
                internal::middleRowsOf<V>(x, values, v),
                internal::middleRowsOf<K>(d, differences, k),
                internal::middleRowsOf<V>(moved, values, v),
                internal::blockOf<K, K>(result, differences, differences, k, k)
            );
        });
    }
    return result;
}

template <Space S, typename XDerived, typename DDerived, typename CovDerived>
void
resetCovariance(
    S const & space,
    Eigen::MatrixBase<XDerived> const & x,
    Eigen::MatrixBase<DDerived> const & d,
    Eigen::MatrixBase<CovDerived> const & cov
)
{
    if constexpr (!hasIdentityReset<S>)
    {
        auto const moved = Emrys::plus(space, x, d);
        internal::resetCovarianceAt(space, x, d, moved, cov);
    }
}
 
}  // namespace Emrys