Utils.hpp

Go to the documentation of this file.

#ifndef AI_TOOLBOX_POMDP_UTILS_HEADER_FILE
#define AI_TOOLBOX_POMDP_UTILS_HEADER_FILE
 
#include <cstddef>
#include <iterator>
#include <numeric>
 
#include <AIToolbox/Utils/Core.hpp>
#include <AIToolbox/Utils/Probability.hpp>
#include <AIToolbox/Utils/Polytope.hpp>
#include <AIToolbox/POMDP/Types.hpp>
#include <AIToolbox/POMDP/TypeTraits.hpp>
 
#include <boost/functional/hash.hpp>
 
namespace AIToolbox::POMDP {
    std::strong_ordering operator<=>(const VEntry & lhs, const VEntry & rhs);
    bool operator==(const VEntry & lhs, const VEntry & rhs);
 
    inline VEntry makeVEntry(size_t S, size_t a, size_t O) {
        VEntry entry;
 
        entry.values.resize(S);
        entry.values.setZero();
        entry.action = a;
        entry.observations.resize(O);
 
        return entry;
    }
 
    inline size_t hash_value(const VEntry & v) {
        size_t seed = 0;
        boost::hash_combine(seed, v.action);
        boost::hash_combine(seed, v.observations);
        boost::hash_combine(seed, v.values);
        return seed;
    }
 
    inline const MDP::Values & unwrap(const VEntry & ve) {
        return ve.values;
    }
 
    ValueFunction makeValueFunction(size_t S);
 
    double weakBoundDistance(const VList & oldV, const VList & newV);
 
    template <IsModel M>
    auto makeSOSA(const M & m) {
        if constexpr(IsModelEigen<M>) {
            boost::multi_array<std::remove_cvref_t<decltype(m.getTransitionFunction(0))>, 2> retval( boost::extents[m.getA()][m.getO()] );
            for (size_t a = 0; a < m.getA(); ++a)
                for (size_t o = 0; o < m.getO(); ++o)
                    retval[a][o] = m.getTransitionFunction(a) * Vector(m.getObservationFunction(a).col(o)).asDiagonal();
            return retval;
        } else {
            Matrix4D retval( boost::extents[m.getA()][m.getO()] );
            for (size_t a = 0; a < m.getA(); ++a) {
                for (size_t o = 0; o < m.getO(); ++o) {
                    retval[a][o].resize(m.getS(), m.getS());
                    for (size_t s = 0; s < m.getS(); ++s)
                        for (size_t s1 = 0; s1 < m.getS(); ++s1)
                            retval[a][o](s, s1) = m.getTransitionProbability(s, a, s1) * m.getObservationProbability(s1, a, o);
                }
            }
            return retval;
        }
    }
 
    template <IsModel M>
    void updateBeliefUnnormalized(const M & model, const Belief & b, const size_t a, const size_t o, Belief * bRet) {
        if (!bRet) return;
 
        auto & br = *bRet;
 
        if constexpr(IsModelEigen<M>) {
            br = model.getObservationFunction(a).col(o).cwiseProduct((b.transpose() * model.getTransitionFunction(a)).transpose());
        } else {
            const size_t S = model.getS();
            for ( size_t s1 = 0; s1 < S; ++s1 ) {
                double sum = 0.0;
                for ( size_t s = 0; s < S; ++s )
                    sum += model.getTransitionProbability(s,a,s1) * b[s];
 
                br[s1] = model.getObservationProbability(s1,a,o) * sum;
            }
        }
    }
 
    template <IsModel M>
    Belief updateBeliefUnnormalized(const M & model, const Belief & b, const size_t a, const size_t o) {
        Belief br(model.getS());
        updateBeliefUnnormalized(model, b, a, o, &br);
        return br;
    }
 
    template <IsModel M>
    void updateBelief(const M & model, const Belief & b, const size_t a, const size_t o, Belief * bRet) {
        if (!bRet) return;
 
        updateBeliefUnnormalized(model, b, a, o, bRet);
 
        auto & br = *bRet;
        br /= br.sum();
    }
 
    template <IsModel M>
    Belief updateBelief(const M & model, const Belief & b, const size_t a, const size_t o) {
        Belief br(model.getS());
        updateBelief(model, b, a, o, &br);
        return br;
    }
 
    template <IsModel M>
    void updateBeliefPartial(const M & model, const Belief & b, const size_t a, Belief * bRet) {
        if (!bRet) return;
 
        auto & br = *bRet;
 
        if constexpr(IsModelEigen<M>) {
            br = (b.transpose() * model.getTransitionFunction(a)).transpose();
        } else {
            const size_t S = model.getS();
            for ( size_t s1 = 0; s1 < S; ++s1 ) {
                br[s1] = 0.0;
                for ( size_t s = 0; s < S; ++s )
                    br[s1] += model.getTransitionProbability(s,a,s1) * b[s];
            }
        }
    }
 
    template <IsModel M>
    Belief updateBeliefPartial(const M & model, const Belief & b, const size_t a) {
        Belief bRet(model.getS());
        updateBeliefPartial(model, b, a, &bRet);
        return bRet;
    }
 
    template <IsModel M>
    void updateBeliefPartialUnnormalized(const M & model, const Belief & b, const size_t a, const size_t o, Belief * bRet) {
        if (!bRet) return;
 
        auto & br = *bRet;
 
        if constexpr(IsModelEigen<M>) {
            br = model.getObservationFunction(a).col(o).cwiseProduct(b);
        } else {
            const size_t S = model.getS();
            for ( size_t s = 0; s < S; ++s )
                br[s] = model.getObservationProbability(s, a, o) * b[s];
        }
    }
 
    template <IsModel M>
    Belief updateBeliefPartialUnnormalized(const M & model, const Belief & b, const size_t a, const size_t o) {
        Belief bRet(model.getS());
        updateBeliefPartialUnnormalized(model, b, a, o, &bRet);
        return bRet;
    }
 
    template <IsModel M>
    void updateBeliefPartialNormalized(const M & model, const Belief & b, const size_t a, const size_t o, Belief * bRet) {
        if (!bRet) return;
 
        auto & br = *bRet;
 
        updateBeliefPartialUnnormalized(model, b, a, o, bRet);
 
        br /= br.sum();
    }
 
    template <IsModel M>
    Belief updateBeliefPartialNormalized(const M & model, const Belief & b, const size_t a, const size_t o) {
        auto newB = updateBeliefPartialUnnormalized(model, b, a, o);
        newB /= newB.sum();
        return newB;
    }
 
    template <IsModel M>
    double beliefExpectedReward(const M& model, const Belief & b, const size_t a) {
        if constexpr (IsModelEigen<M>) {
            return model.getRewardFunction().col(a).dot(b);
        } else {
            double rew = 0.0; const size_t S = model.getS();
            for ( size_t s = 0; s < S; ++s )
                for ( size_t s1 = 0; s1 < S; ++s1 )
                    rew += model.getTransitionProbability(s, a, s1) * model.getExpectedReward(s, a, s1) * b[s];
 
            return rew;
        }
    }
 
    template <typename ActionRow>
    void crossSumBestAtBelief(const Belief & b, const ActionRow & row, VEntry * outp, double * value = nullptr) {
        if (!outp) return;
 
        const size_t O = row.size();
        double v = 0.0, tmp;
 
        auto & out = *outp;
        out.values.setZero();
 
        // We compute the crossSum between each best vector for the belief.
        for ( size_t o = 0; o < O; ++o ) {
            const auto & r = row[o];
            auto begin = std::begin(r);
            auto end   = std::end(r);
 
            auto bestMatch = findBestAtPoint(b, begin, end, &tmp, unwrap).base();
 
            out.values += bestMatch->values;
            v += tmp;
 
            out.observations[o] = bestMatch->observations[0];
        }
        if (value) *value = v;
    }
 
    template <typename ActionRow>
    VEntry crossSumBestAtBelief(const Belief & b, const ActionRow & row, const size_t a, double * value = nullptr) {
        auto entry = makeVEntry(b.size(), a, row.size());
 
        crossSumBestAtBelief(b, row, &entry, value);
 
        return entry;
    }
 
    template <typename Projections>
    VEntry crossSumBestAtBelief(const Belief & b, const Projections & projs, double * value = nullptr) {
        const size_t A = projs.size();
 
        double bestValue, tmp;
        VEntry entry = crossSumBestAtBelief(b, projs[0], (size_t)0, &bestValue);
        VEntry helper = entry;
 
        for ( size_t a = 1; a < A; ++a ) {
            helper.action = a;
            crossSumBestAtBelief(b, projs[a], &helper, &tmp);
 
            if (tmp > bestValue) {
                bestValue = tmp;
                std::swap(entry, helper);
            }
        }
        if (value) *value = bestValue;
        return entry;
    }
 
    template <IsModel M>
    std::tuple<size_t, double> bestConservativeAction(const M & pomdp, MDP::QFunction immediateRewards, const Belief & initialBelief, const VList & lbVList, MDP::Values * alpha = nullptr) {
        // Note that we update inline the alphavectors in immediateRewards
        Vector bpAlpha(pomdp.getS());
        // Storage to avoid reallocations
        Belief intermediateBelief(pomdp.getS());
        Belief nextBelief(pomdp.getS());
 
        for (size_t a = 0; a < pomdp.getA(); ++a) {
            updateBeliefPartial(pomdp, initialBelief, a, &intermediateBelief);
 
            bpAlpha.setZero();
 
            for (size_t o = 0; o < pomdp.getO(); ++o) {
                updateBeliefPartialUnnormalized(pomdp, intermediateBelief, a, o, &nextBelief);
 
                const auto nextBeliefProbability = nextBelief.sum();
                if (checkEqualSmall(nextBeliefProbability, 0.0)) continue;
                // Now normalized
                nextBelief /= nextBeliefProbability;
 
                const auto it = findBestAtPoint(nextBelief, std::begin(lbVList), std::end(lbVList), nullptr, unwrap);
 
                bpAlpha += pomdp.getObservationFunction(a).col(o).cwiseProduct(it->values);
            }
            immediateRewards.col(a) += pomdp.getDiscount() * pomdp.getTransitionFunction(a) * bpAlpha;
        }
 
        size_t id;
        double v = (initialBelief.transpose() * immediateRewards).maxCoeff(&id);
 
        // Copy alphavector for selected action if needed
        if (alpha) *alpha = immediateRewards.col(id);
 
        return std::make_tuple(id, v);
    }
 
    template <bool useLP = true, IsModel M>
    std::tuple<size_t, double> bestPromisingAction(const M & pomdp, const MDP::QFunction & immediateRewards, const Belief & belief, const MDP::QFunction & ubQ, const UpperBoundValueFunction & ubV, Vector * vals = nullptr) {
        Vector storage;
        Vector & qvals = vals ? *vals : storage;
 
        qvals = belief.transpose() * immediateRewards;
 
        // Storage to avoid reallocations
        Belief intermediateBelief(pomdp.getS());
        Belief nextBelief(pomdp.getS());
 
        for (size_t a = 0; a < pomdp.getA(); ++a) {
            updateBeliefPartial(pomdp, belief, a, &intermediateBelief);
            double sum = 0.0;
            for (size_t o = 0; o < pomdp.getO(); ++o) {
                updateBeliefPartialUnnormalized(pomdp, intermediateBelief, a, o, &nextBelief);
 
                const auto prob = nextBelief.sum();
                if (checkEqualSmall(prob, 0.0)) continue;
                // Note that we do not normalize nextBelief since we'd also
                // have to multiply the result by the same probability. Instead
                // we don't normalize, and we don't multiply, so we save some
                // work.
                if constexpr (useLP)
                    sum += std::get<0>(LPInterpolation(nextBelief, ubQ, ubV));
                else
                    sum += std::get<0>(sawtoothInterpolation(nextBelief, ubQ, ubV));
            }
            qvals[a] += pomdp.getDiscount() * sum;
        }
        size_t bestAction;
        double bestValue = qvals.maxCoeff(&bestAction);
 
        return std::make_tuple(bestAction, bestValue);
    }
}
 
#endif