EmrysFilter 0.1.0
State estimation utilities in C++23 using Eigen
Loading...
Searching...
No Matches
Product.h
Go to the documentation of this file.
1#pragma once
2
9
10#include <algorithm>
11#include <array>
12#include <concepts>
13#include <cstddef>
14#include <expected>
15#include <optional>
16#include <random>
17#include <tuple>
18#include <type_traits>
19#include <utility>
20
31
32namespace Emrys
33{
34
39template <typename T>
40concept HasLogDensity = requires(
41 T const & density,
42 Eigen::Matrix<
43 typename T::Scalar,
44 T::ValueDimensionAtCompileTime,
45 1,
46 Eigen::ColMajor,
47 T::MaxValueDimensionAtCompileTime,
48 1> const & x
49) {
50 typename T::SpaceType;
51 { Emrys::valueDimension(density.space()) } -> std::convertible_to<Eigen::Index>;
52 { density.logDensity(x) } -> std::convertible_to<typename T::Scalar>;
53};
54
55namespace internal
56{
57
61template <typename T>
62concept CheckedParams = requires(T const & params) {
63 { params.toDensity().has_value() } -> std::convertible_to<bool>;
64 requires HasLogDensity<typename decltype(params.toDensity())::value_type>;
65};
66
68template <typename T>
69concept HasMoments = requires(T const & distribution) { distribution.moments(); };
70
71} // namespace internal
72
78template <typename T>
79concept ProductFactor = (HasLogDensity<T> && internal::HasMoments<T>)
80 || (internal::CheckedParams<T>
81 && internal::HasMoments<typename decltype(std::declval<T const &>().toDensity())::value_type>);
82
83namespace internal
84{
85
89template <typename Own, typename Block>
90inline constexpr bool factorFits = std::is_same_v<Own, Block> || std::derived_from<Block, Own>
91 || std::derived_from<Own, Block> || (isLinear<Own> && isLinear<Block>);
92
97template <typename Placed>
98struct PlacedFactor
99{};
100
102template <ProductFactor F, typename Path>
103struct PlacedFactor<OnPath<F, Path>>
104{
106 template <typename Space>
107 using TypeIn = F;
108
110 template <typename Space>
111 [[nodiscard]] static F
112 factorIn(OnPath<F, Path> && placed, Space const &)
113 {
114 return std::move(placed.value);
115 }
116
118 template <typename Space>
119 [[nodiscard]] static auto
120 indicesIn(OnPath<F, Path> const &, Space const & space)
121 {
122 return asIndexSet(idx<Path>(space));
123 }
124
126 template <typename Space>
127 static constexpr bool fitsIn = factorFits<typename F::SpaceType, PathSubspaceType<Space, Path>>;
128};
129
133template <ProductFactor F, typename Indices>
134struct FactorAt
135{
136 F factor;
137 Indices indices;
138};
139
141template <ProductFactor F, typename Indices>
142struct PlacedFactor<FactorAt<F, Indices>>
143{
145 template <typename Space>
146 using TypeIn = F;
147
149 template <typename Space>
150 [[nodiscard]] static F
151 factorIn(FactorAt<F, Indices> && placed, Space const &)
152 {
153 return std::move(placed.factor);
154 }
155
157 template <typename Space>
158 [[nodiscard]] static auto
159 indicesIn(FactorAt<F, Indices> const & placed, Space const &)
160 {
161 return asIndexSet(placed.indices);
162 }
163
165 template <typename Space>
166 static constexpr bool fitsIn = true;
167};
168
170template <typename Indices, ProductFactor F>
171[[nodiscard]] FactorAt<F, Indices>
172placedAt(Indices indices, F factor)
173{
174 return {std::move(factor), std::move(indices)};
175}
176
178template <typename F>
179[[nodiscard]] decltype(auto)
180densityOf(F const & factor)
181{
182 if constexpr (HasLogDensity<F>)
183 {
184 return (factor);
185 }
186 else
187 {
188 auto const density = factor.toDensity();
189 auto view = *density;
190 return view;
191 }
192}
193
195template <typename F>
196inline constexpr bool momentsMayFail =
197 isExpected<std::remove_cvref_t<decltype(densityOf(std::declval<F const &>()).moments())>>;
198
200template <typename... T>
201struct TypeList
202{};
203
205template <std::size_t N, typename... T>
206struct DropFrontImpl
207{
209 using type = TypeList<T...>;
210};
211
213template <std::size_t N, typename First, typename... Rest>
214 requires(N > 0)
215struct DropFrontImpl<N, First, Rest...> : DropFrontImpl<N - 1, Rest...>
216{};
217
219template <std::size_t N, typename... T>
220using DropFront = typename DropFrontImpl<N, T...>::type;
221
223template <std::size_t N, typename Done, typename... T>
224struct TakeFrontImpl
225{
227 using type = Done;
228};
229
231template <std::size_t N, typename... Done, typename First, typename... Rest>
232 requires(N > 0)
233struct TakeFrontImpl<N, TypeList<Done...>, First, Rest...> : TakeFrontImpl<N - 1, TypeList<Done..., First>, Rest...>
234{};
235
237template <std::size_t N, typename... T>
238using TakeFront = typename TakeFrontImpl<N, TypeList<>, T...>::type;
239
243template <int Size, int MaxSize, typename Indices, typename Target>
244[[nodiscard]] IndexVector<Size, MaxSize>
245positionsIn(Indices const & indices, Target const & target)
246{
247 auto result = IndexVector<Size, MaxSize>{};
248 result.resize(Eigen::Index(indices.size()));
249 for (Eigen::Index i = 0; i < result.size(); ++i)
250 {
251 auto const found = std::ranges::find(target, indices[i]);
252 EMRYS_FILTER_ASSERT(found != std::ranges::end(target) && "a factor's coordinate is missing from the result");
253 result(i) = Eigen::Index(found - std::ranges::begin(target));
254 }
255 return result;
256}
257
259template <typename R>
260struct ConditionalValueOf
261{
263 using type = R;
264};
265
267template <typename T, typename E>
268struct ConditionalValueOf<std::expected<T, E>>
269{
271 using type = T;
272};
273
277template <typename R>
278using ConditionalValue = typename ConditionalValueOf<R>::type;
279
283template <typename R>
284struct ConditionalErrorOf
285{
287 using type = void;
288};
289
291template <typename T, typename E>
292struct ConditionalErrorOf<std::expected<T, E>>
293{
295 using type = E;
296};
297
299template <CheckedParams R>
300struct ConditionalErrorOf<R>
301{
303 using type = typename decltype(std::declval<R const &>().toDensity())::error_type;
304};
305
307template <typename R>
308using ConditionalError = typename ConditionalErrorOf<R>::type;
309
310} // namespace internal
311
312template <Space Space_, RealScalar Scalar_, ProductFactor... Factors_>
313class Product;
314
315namespace internal
316{
317
319template <
320 typename Scalar,
321 typename Space,
322 std::size_t K,
323 typename Replacement,
324 typename Factors_,
325 typename Sequence = std::make_index_sequence<std::tuple_size_v<Factors_>>>
326struct ReplacedProductOf;
327
329template <typename Scalar, typename Space, std::size_t K, typename Replacement, typename... Factors_, std::size_t... J>
330struct ReplacedProductOf<Scalar, Space, K, Replacement, std::tuple<Factors_...>, std::index_sequence<J...>>
331{
334};
335
337template <typename Scalar, typename Space, std::size_t K, typename Replacement, typename Factors_>
338using ReplacedProduct = typename ReplacedProductOf<Scalar, Space, K, Replacement, Factors_>::type;
339
340} // namespace internal
341
356template <ProductFactor F, Space BlockSpace>
358{
359 static_assert(
360 requires { typename BlockSpace::BlockType; } && BlockSpace::FixedValueDimension == 0,
361 "onEach<Path>() places a factor on a repeated block"
362 );
363
365 static constexpr int Step = BlockSpace::ValueDimensionStep;
366
368 static constexpr bool MomentsMayFail = internal::momentsMayFail<F>;
369
370public:
371
372 using Scalar = typename F::Scalar;
373 using Index = Eigen::Index;
374 using SpaceType = BlockSpace;
377
380
383
386 : m_factor{std::move(factor)}
387 , m_space{std::move(space)}
388 {
389 EMRYS_FILTER_ASSERT(
390 Emrys::valueDimension(m_factor.space()) == Step && "the factor must cover one repetition of the block"
391 );
392 if constexpr (!HasLogDensity<F>)
393 {
394 [[maybe_unused]] bool const checked = m_factor.toDensity().has_value();
395 EMRYS_FILTER_ASSERT(checked && "a factor failed its check; call its toDensity() first to handle this");
396 }
397 }
398
400 [[nodiscard]] F const &
401 factor() const
402 {
403 return m_factor;
404 }
405
407 [[nodiscard]] BlockSpace const &
408 space() const
409 {
410 return m_space;
411 }
412
414 [[nodiscard]] Index
415 count() const
416 {
417 return m_space.count();
418 }
419
421 [[nodiscard]] Index
423 {
424 return Emrys::valueDimension(m_space);
425 }
426
430 template <typename Derived>
431 [[nodiscard]] Scalar
432 logDensity(Eigen::MatrixBase<Derived> const & x) const
433 {
434 static_assert(Derived::IsVectorAtCompileTime, "logDensity() expects a vector");
435 EMRYS_FILTER_ASSERT(x.size() == valueDimension());
436 decltype(auto) density = internal::densityOf(m_factor);
437 Scalar result = 0;
438 for (Index k = 0; k < count(); ++k)
439 {
440 result += Scalar(density.logDensity(x.derived().template segment<Step>(k * Step)));
441 }
442 return result;
443 }
444
448 [[nodiscard]] auto
449 moments() const -> std::conditional_t<MomentsMayFail, std::expected<MomentsType, MomentsError>, MomentsType>
450 {
451 decltype(auto) density = internal::densityOf(m_factor);
452 auto const & one = density.moments();
453 auto const place = [&](auto const & moments) {
454 auto result = MomentsType{valueDimension(), valueDimension()};
455 result.mean().setZero();
456 result.covarianceLower().setZero();
457 for (Index k = 0; k < count(); ++k)
458 {
459 result.mean().template segment<Step>(k * Step) = moments.mean();
460 result.covarianceLower().template block<Step, Step>(k * Step, k * Step) =
461 moments.covarianceLower().template selfadjointView<Eigen::Lower>();
462 }
463 return result;
464 };
465 if constexpr (MomentsMayFail)
466 {
467 if (!one)
468 {
469 return std::unexpected{one.error()};
470 }
471 return place(*one);
472 }
473 else
474 {
475 return place(one);
476 }
477 }
478
479private:
480
481 F m_factor;
482 EMRYS_FILTER_NO_UNIQUE_ADDRESS BlockSpace m_space;
483};
484
489template <typename F, typename BlockSpace, std::uniform_random_bit_generator G>
491drawSamples(EachRepetition<F, BlockSpace> const & each, Eigen::Index n, G & rng)
492{
493 EMRYS_FILTER_ASSERT(n >= 0);
494 constexpr int Step = BlockSpace::ValueDimensionStep;
496 decltype(auto) density = internal::densityOf(each.factor());
497 for (Eigen::Index k = 0; k < each.count(); ++k)
498 {
499 samples.template middleRows<Step>(k * Step) = drawSamples(density, n, rng);
500 }
501 return samples;
502}
503
507template <typename F, typename BlockSpace, std::uniform_random_bit_generator G>
510{
511 constexpr int Step = BlockSpace::ValueDimensionStep;
513 sample.resize(each.valueDimension());
514 decltype(auto) density = internal::densityOf(each.factor());
515 for (Eigen::Index k = 0; k < each.count(); ++k)
516 {
517 sample.template segment<Step>(k * Step) = drawSample(density, rng);
518 }
519 return sample;
520}
521
526template <typename F, typename BlockSpace, std::uniform_random_bit_generator G>
527[[nodiscard]] SamplesWithLogDensity<BlockSpace, typename F::Scalar>
528drawSamplesWithLogDensity(EachRepetition<F, BlockSpace> const & each, Eigen::Index n, G & rng)
529{
530 EMRYS_FILTER_ASSERT(n >= 0);
531 using Scalar = typename F::Scalar;
532 constexpr int Step = BlockSpace::ValueDimensionStep;
535 Eigen::Array<Scalar, Eigen::Dynamic, 1>::Zero(n)
536 };
537 decltype(auto) density = internal::densityOf(each.factor());
538 for (Eigen::Index k = 0; k < each.count(); ++k)
539 {
540 auto draws = drawSamplesWithLogDensity(density, n, rng);
541 result.samples.template middleRows<Step>(k * Step) = draws.samples;
542 result.logDensity += draws.logDensity;
543 }
544 return result;
545}
546
551template <typename F, typename BlockSpace, std::uniform_random_bit_generator G>
552[[nodiscard]] SampleWithLogDensity<BlockSpace, typename F::Scalar>
554{
555 constexpr int Step = BlockSpace::ValueDimensionStep;
557 result.sample.resize(each.valueDimension());
558 decltype(auto) density = internal::densityOf(each.factor());
559 for (Eigen::Index k = 0; k < each.count(); ++k)
560 {
561 auto const draw = drawSampleWithLogDensity(density, rng);
562 result.sample.template segment<Step>(k * Step) = draw.sample;
563 result.logDensity += draw.logDensity;
564 }
565 return result;
566}
567
568namespace internal
569{
570
572template <typename Space, typename Path>
573struct EachBlockOf
574{
576 using type = PathSubspaceType<Space, Path>;
577
579 [[nodiscard]] static type
580 in(Space const & space)
581 {
582 return pathSubspace<Path>(space);
583 }
584
586 [[nodiscard]] static auto
587 indicesIn(Space const & space)
588 {
589 return asIndexSet(idx<Path>(space));
590 }
591};
592
594template <typename Space>
595struct EachBlockOf<Space, WholeSpace>
596{
598 using type = Space;
599
601 [[nodiscard]] static Space const &
602 in(Space const & space)
603 {
604 return space;
605 }
606
608 [[nodiscard]] static auto
609 indicesIn(Space const & space)
610 {
611 return asIndexSet(Eigen::seqN(Eigen::Index{0}, valueDimension(space)));
612 }
613};
614
618template <ProductFactor F, typename Path>
619struct PlacedFactor<OnEach<F, Path>>
620{
622 template <typename Space>
623 using TypeIn = EachRepetition<F, typename EachBlockOf<Space, Path>::type>;
624
626 template <typename Space>
627 [[nodiscard]] static TypeIn<Space>
628 factorIn(OnEach<F, Path> && placed, Space const & space)
629 {
630 return {std::move(placed.value), EachBlockOf<Space, Path>::in(space)};
631 }
632
634 template <typename Space>
635 [[nodiscard]] static auto
636 indicesIn(OnEach<F, Path> const &, Space const & space)
637 {
638 return EachBlockOf<Space, Path>::indicesIn(space);
639 }
640
644 template <typename Space>
645 static constexpr bool fitsIn = [] {
646 using Block = typename EachBlockOf<Space, Path>::type;
647 if constexpr (requires { typename Block::BlockType; })
648 {
649 return factorFits<typename F::SpaceType, typename Block::BlockType>;
650 }
651 else
652 {
653 return false;
654 }
655 }();
656};
657
659template <typename T>
660inline constexpr bool isPlacedFactor = false;
661
663template <ProductFactor F, typename Path>
664inline constexpr bool isPlacedFactor<OnPath<F, Path>>{true};
665
666template <ProductFactor F, typename Path>
667inline constexpr bool isPlacedFactor<OnEach<F, Path>>{true};
669
670} // namespace internal
671
691template <typename... Placed>
693{
694 static_assert(sizeof...(Placed) > 0, "a product needs at least one factor");
695 static_assert(
696 (internal::isPlacedFactor<Placed> && ...),
697 "each factor is placed by tag path, with on<Path>() or onEach<Path>()"
698 );
699
700public:
701
703 explicit Factors(Placed... placed)
704 : m_placed{std::move(placed)...}
705 {}
706
708 [[nodiscard]] std::tuple<Placed...> const &
709 placed() const
710 {
711 return m_placed;
712 }
713
714private:
715
716 std::tuple<Placed...> m_placed;
717};
718
719namespace internal
720{
721
723template <typename T>
724inline constexpr bool isFactors = false;
725
727template <typename... Placed>
728inline constexpr bool isFactors<Factors<Placed...>>{true};
730
731} // namespace internal
732
770template <Space Space_, RealScalar Scalar_, ProductFactor... Factors_>
772{
773 static_assert(sizeof...(Factors_) > 0, "a product needs at least one factor");
774 static_assert(
775 (std::is_same_v<typename Factors_::Scalar, Scalar_> && ...),
776 "every factor must have the same scalar"
777 );
778
780 static constexpr bool MomentsMayFail = (internal::momentsMayFail<Factors_> || ...);
781
782public:
783
784 using Scalar = Scalar_;
785 using Index = Eigen::Index;
786 using SpaceType = Space_;
790
793
796
799
802
803 static constexpr std::size_t FactorCount = sizeof...(Factors_);
804
805 static_assert(
806 internal::hasEqualSizes<Space_>,
807 "a product places its factors by coordinate, so its space's values and differences must have the "
808 "same coordinates"
809 );
810
812 template <std::size_t K>
813 using FactorType = std::tuple_element_t<K, std::tuple<Factors_...>>;
814
818 template <typename... Placed>
819 requires(sizeof...(Placed) == sizeof...(Factors_)
820 && (std::is_same_v<typename internal::PlacedFactor<Placed>::template TypeIn<Space_>, Factors_> && ...))
821 explicit Product(Space_ space, Placed... placed)
822 : m_space{std::move(space)}
823 , m_indices{resolve<Factors_>(placed)...}
824 , m_factors{internal::PlacedFactor<Placed>::factorIn(std::move(placed), m_space)...}
825 {
826 static_assert(
827 (internal::PlacedFactor<Placed>::template fitsIn<Space_> && ...),
828 "a factor's space must be the space of the block it covers"
829 );
830 EMRYS_FILTER_ASSERT(sizesMatch() && "a factor's dimension must match the number of coordinates it covers");
831 EMRYS_FILTER_ASSERT(partitionsSpace() && "the factors' indices must partition the space");
832 std::apply(
833 [](auto const &... factors) {
834 (checkParams(factors), ...);
835 },
836 m_factors
837 );
838 }
839
841 template <typename... Placed>
842 explicit Product(Space_ space, Factors<Placed...> const & factors)
843 : Product{std::move(space), factors, std::index_sequence_for<Placed...>{}}
844 {}
845
847 [[nodiscard]] Index
849 {
850 return Emrys::valueDimension(m_space);
851 }
852
854 [[nodiscard]] Index
856 {
857 return Emrys::differenceDimension(m_space);
858 }
859
861 [[nodiscard]] Space_ const &
862 space() const
863 {
864 return m_space;
865 }
866
868 template <std::size_t K>
869 [[nodiscard]] FactorType<K> const &
870 factor() const
871 {
872 return std::get<K>(m_factors);
873 }
874
876 template <std::size_t K>
877 [[nodiscard]] auto const &
878 indices() const
879 {
880 return std::get<K>(m_indices);
881 }
882
886 template <typename Fn>
887 void
888 forEachFactor(Fn && fn) const
889 {
890 [&]<std::size_t... K>(std::index_sequence<K...>) {
891 (fn(internal::densityOf(std::get<K>(m_factors)), std::get<K>(m_indices)), ...);
892 }(std::index_sequence_for<Factors_...>{});
893 }
894
896 template <typename Derived>
897 [[nodiscard]] Scalar
898 logDensity(Eigen::MatrixBase<Derived> const & x) const
899 {
900 static_assert(Derived::IsVectorAtCompileTime, "logDensity() expects a vector");
901 EMRYS_FILTER_ASSERT(x.size() == valueDimension());
902 Scalar result = 0;
903 forEachFactor([&](auto const & density, auto const & indices) {
904 result += Scalar(density.logDensity(x.derived()(indices)));
905 });
906 return result;
907 }
908
913 template <typename Derived>
914 [[nodiscard]] Scalar
915 density(Eigen::MatrixBase<Derived> const & x) const
916 {
917 return Eigen::numext::exp(logDensity(x));
918 }
919
924 [[nodiscard]] auto
925 moments() const -> std::conditional_t<MomentsMayFail, std::expected<MomentsType, MomentsError>, MomentsType>
926 {
928 result.mean().setZero();
929 result.covarianceLower().setZero();
930 auto error = std::optional<MomentsError>{};
931 auto const place = [&](auto const & moments, auto const & indices) {
932 using FactorMatrix = std::remove_cvref_t<decltype(moments.covarianceLower())>;
933 result.mean()(indices) = moments.mean();
934 result.covarianceLower()(indices, indices) =
935 FactorMatrix{moments.covarianceLower().template selfadjointView<Eigen::Lower>()};
936 };
937 forEachFactor([&](auto const & density, auto const & indices) {
938 if (error)
939 {
940 return;
941 }
942 auto const & factorMoments = density.moments();
943 if constexpr (internal::isExpected<std::remove_cvref_t<decltype(factorMoments)>>)
944 {
945 if (!factorMoments)
946 {
947 error = factorMoments.error();
948 }
949 else
950 {
951 place(*factorMoments, indices);
952 }
953 }
954 else
955 {
956 place(factorMoments, indices);
957 }
958 });
959 if constexpr (MomentsMayFail)
960 {
961 if (error)
962 {
963 return std::unexpected{*error};
964 }
965 }
966 return result;
967 }
968
977 template <typename Path>
978 [[nodiscard]] decltype(auto)
979 marginal() const &
980 {
981 return internal::withPathTags<Path>([&]<typename... Tags>() -> decltype(auto) {
982 return marginalAlong<Tags...>();
983 });
984 }
985
987 template <typename Path>
988 void
989 marginal() const && = delete;
990
1003 template <typename GivenPath, typename Derived>
1004 [[nodiscard]] auto
1005 conditional(Eigen::MatrixBase<Derived> const & values) const
1006 {
1007 return internal::withPathTags<GivenPath>([&]<typename... Tags>() {
1008 return conditionalAlong<Tags...>(values);
1009 });
1010 }
1011
1012private:
1013
1015 template <typename... Placed, std::size_t... K>
1016 Product(Space_ space, Factors<Placed...> const & factors, std::index_sequence<K...>)
1017 : Product{std::move(space), std::get<K>(factors.placed())...}
1018 {}
1019
1021 template <typename... Path>
1022 [[nodiscard]] decltype(auto)
1023 marginalAlong() const
1024 {
1025 using Match = MatchType<Path...>;
1026 if constexpr (Match::value.factor != None)
1027 {
1028 assertOnOwnBlock<Match::value.factor>(OwnBlockPath<Match::value, Path...>{});
1029 if constexpr (Match::value.whole)
1030 {
1031 return std::get<Match::value.factor>(m_factors);
1032 }
1033 else
1034 {
1035 return factorMarginal(
1036 std::get<Match::value.factor>(m_factors),
1037 internal::DropFront<Match::value.start, Path...>{}
1038 );
1039 }
1040 }
1041 else
1042 {
1043 static_assert(
1044 std::ranges::count(withinOf<Path...>(), true) > 0,
1045 "the path neither reaches into a factor nor contains one"
1046 );
1047 auto const target = internal::resolveIndices<ValueDimensionAtCompileTime, MaxValueDimensionAtCompileTime>(
1049 internal::asIndexSet(idx<TagPath<Path...>>(m_space))
1050 );
1051 EMRYS_FILTER_ASSERT(
1052 coversExactly(withinOf<Path...>(), target) && "the path's block must be made of whole factors"
1053 );
1054 return keepOnly<withinOf<Path...>()>(target, internal::subspace<Space_, Path...>(m_space));
1055 }
1056 }
1057
1059 template <typename... GivenPath, typename Derived>
1060 [[nodiscard]] auto
1061 conditionalAlong(Eigen::MatrixBase<Derived> const & values) const
1062 {
1063 return internal::tagConditional<TagPath<GivenPath...>>(
1064 m_space,
1065 [&](auto const & targetCoordinates, auto const & givenCoordinates, auto targetSpace, auto const &) {
1066 // The values and the differences have the same coordinates.
1067 auto const & target = targetCoordinates.values;
1068 auto const & given = givenCoordinates.values;
1069 EMRYS_FILTER_ASSERT(values.size() == given.size());
1070 using Match = MatchType<GivenPath...>;
1071 if constexpr (Match::value.factor != None && !Match::value.whole)
1072 {
1073 assertOnOwnBlock<Match::value.factor>(OwnBlockPath<Match::value, GivenPath...>{});
1074 return replaceFactor<Match::value.factor>(
1075 target,
1076 given,
1077 std::move(targetSpace),
1078 internal::DropFront<Match::value.start, GivenPath...>{},
1079 values
1080 );
1081 }
1082 else
1083 {
1084 static_assert(
1085 std::ranges::count(droppedOf<GivenPath...>(), true) > 0,
1086 "the path neither reaches into a factor nor contains one"
1087 );
1088 static_assert(
1089 std::ranges::count(droppedOf<GivenPath...>(), true) < std::ptrdiff_t(FactorCount),
1090 "conditioning on every factor leaves nothing"
1091 );
1092 EMRYS_FILTER_ASSERT(
1093 coversExactly(droppedOf<GivenPath...>(), given)
1094 && "the given block must be made of whole factors"
1095 );
1096 return keepOnly<complement(droppedOf<GivenPath...>())>(target, std::move(targetSpace));
1097 }
1098 }
1099 );
1100 }
1101
1102 static constexpr std::size_t None = FactorCount;
1103
1107 struct PathMatch
1108 {
1109 std::size_t factor = None;
1110 std::size_t start = 0;
1111 bool whole = false;
1112 bool named = false;
1113 };
1114
1118 template <PathMatch Match, typename... Path>
1119 using OwnBlockPath = std::conditional_t<
1120 Match.named,
1121 internal::TakeFront<Match.start + 1, Path...>,
1122 internal::TypeList<typename FactorType<Match.factor>::SpaceType>>;
1123
1125 template <typename T>
1126 static consteval std::size_t
1127 factorNamed()
1128 {
1129 constexpr auto same = std::array<bool, FactorCount>{std::is_same_v<T, typename Factors_::SpaceType>...};
1130 static_assert(std::ranges::count(same, true) <= 1, "two factors share this space type, so it names neither");
1131 auto const found = std::ranges::find(same, true);
1132 return found == same.end() ? None : std::size_t(found - same.begin());
1133 }
1134
1136 template <typename T>
1137 static consteval std::size_t
1138 factorContaining()
1139 {
1140 constexpr auto inside =
1141 std::array<bool, FactorCount>{(internal::countTag<T, typename Factors_::SpaceType>() > 0)...};
1142 static_assert(std::ranges::count(inside, true) <= 1, "this tag lies in more than one factor's space");
1143 auto const found = std::ranges::find(inside, true);
1144 return found == inside.end() ? None : std::size_t(found - inside.begin());
1145 }
1146
1148 template <typename T>
1149 static consteval std::array<bool, FactorCount>
1150 factorsWithin()
1151 {
1152 return {(internal::countTag<typename Factors_::SpaceType, T>() > 0)...};
1153 }
1154
1158 template <typename... Path>
1159 static consteval PathMatch
1160 matchPath()
1161 {
1162 constexpr auto named = std::array<std::size_t, sizeof...(Path)>{factorNamed<Path>()...};
1163 for (std::size_t j = 0; j < named.size(); ++j)
1164 {
1165 if (named[j] != None)
1166 {
1167 return {named[j], j, j + 1 == named.size(), true};
1168 }
1169 }
1170 return {factorContaining<std::tuple_element_t<0, std::tuple<Path...>>>(), 0, false};
1171 }
1172
1176 template <PathMatch Match, typename Last>
1177 static consteval std::array<bool, FactorCount>
1178 droppedBy()
1179 {
1180 if constexpr (Match.factor != None)
1181 {
1182 auto result = std::array<bool, FactorCount>{};
1183 result[Match.factor] = true;
1184 return result;
1185 }
1186 else
1187 {
1188 return factorsWithin<Last>();
1189 }
1190 }
1191
1192 // The path analyses are types and consteval functions, not local constexpr variables: GCC can
1193 // conflate a local used as a template argument across instantiations of a generic lambda.
1194
1196 template <typename... Path>
1197 using MatchType = std::integral_constant<PathMatch, matchPath<Path...>()>;
1198
1200 template <typename... Path>
1201 using LastTag = std::tuple_element_t<sizeof...(Path) - 1, std::tuple<Path...>>;
1202
1204 template <typename... Path>
1205 static consteval std::array<bool, FactorCount>
1206 withinOf()
1207 {
1208 return factorsWithin<LastTag<Path...>>();
1209 }
1210
1212 template <typename... Path>
1213 static consteval std::array<bool, FactorCount>
1214 droppedOf()
1215 {
1216 return droppedBy<MatchType<Path...>::value, LastTag<Path...>>();
1217 }
1218
1220 static consteval std::array<bool, FactorCount>
1221 complement(std::array<bool, FactorCount> mask)
1222 {
1223 for (bool & b : mask)
1224 {
1225 b = !b;
1226 }
1227 return mask;
1228 }
1229
1231 template <std::array<bool, FactorCount> Mask>
1232 static consteval auto
1233 maskedFactors()
1234 {
1235 auto result = std::array<std::size_t, std::size_t(std::ranges::count(Mask, true))>{};
1236 std::size_t out = 0;
1237 for (std::size_t k = 0; k < FactorCount; ++k)
1238 {
1239 if (Mask[k])
1240 {
1241 result[out++] = k;
1242 }
1243 }
1244 return result;
1245 }
1246
1250 template <typename Factor>
1251 static void
1252 checkParams(Factor const & factor)
1253 {
1254 if constexpr (!HasLogDensity<Factor>)
1255 {
1256 [[maybe_unused]] bool const checked = factor.toDensity().has_value();
1257 EMRYS_FILTER_ASSERT(checked && "a factor failed its check; call its toDensity() first to handle this");
1258 }
1259 }
1260
1264 template <typename Factor, typename Placed>
1265 [[nodiscard]] internal::IndexVector<Factor::ValueDimensionAtCompileTime, Factor::MaxValueDimensionAtCompileTime>
1266 resolve(Placed const & placed) const
1267 {
1268 return internal::IndexVector<Factor::ValueDimensionAtCompileTime, Factor::MaxValueDimensionAtCompileTime>{
1269 internal::resolveIndices<ValueDimensionAtCompileTime, MaxValueDimensionAtCompileTime>(
1271 internal::PlacedFactor<Placed>::indicesIn(placed, m_space)
1272 )
1273 };
1274 }
1275
1277 template <typename Fn>
1278 void
1279 forEachStored(Fn && fn) const
1280 {
1281 [&]<std::size_t... K>(std::index_sequence<K...>) {
1282 (fn(std::get<K>(m_factors), std::get<K>(m_indices)), ...);
1283 }(std::index_sequence_for<Factors_...>{});
1284 }
1285
1287 [[nodiscard]] bool
1288 sizesMatch() const
1289 {
1290 bool result = true;
1291 forEachStored([&](auto const & factor, auto const & indices) {
1292 result = result && Emrys::valueDimension(factor.space()) == indices.size();
1293 });
1294 return result;
1295 }
1296
1298 [[nodiscard]] bool
1299 partitionsSpace() const
1300 {
1301 Index total = 0;
1302 forEachStored([&](auto const &, auto const & indices) {
1303 total += indices.size();
1304 });
1305 bool const distinct = std::apply(
1306 [](auto const &... indices) {
1307 return internal::distinctIndices(indices...);
1308 },
1309 m_indices
1310 );
1311 return total == valueDimension() && distinct;
1312 }
1313
1317 template <std::size_t K, typename... Path>
1318 void
1319 assertOnOwnBlock(internal::TypeList<Path...>) const
1320 {
1321 [[maybe_unused]] auto const block =
1322 internal::resolveIndices<ValueDimensionAtCompileTime, MaxValueDimensionAtCompileTime>(
1324 internal::asIndexSet(idx<TagPath<Path...>>(m_space))
1325 );
1326 EMRYS_FILTER_ASSERT(
1327 std::equal(block.begin(), block.end(), std::get<K>(m_indices).begin(), std::get<K>(m_indices).end())
1328 && "a factor reached by a tag path must be placed on that tag's block"
1329 );
1330 }
1331
1335 template <typename Block>
1336 [[nodiscard]] bool
1337 coversExactly(std::array<bool, FactorCount> const & mask, Block const & block) const
1338 {
1339 std::size_t k = 0;
1340 std::size_t covered = 0;
1341 bool inBlock = true;
1342 forEachStored([&](auto const &, auto const & indices) {
1343 if (mask[k++])
1344 {
1345 covered += std::size_t(indices.size());
1346 for (Index const i : indices)
1347 {
1348 inBlock = inBlock && std::ranges::find(block, i) != std::ranges::end(block);
1349 }
1350 }
1351 });
1352 return inBlock && covered == std::size_t(std::ranges::size(block));
1353 }
1354
1358 template <typename Factor, typename... Path>
1359 [[nodiscard]] static auto
1360 factorMarginal(Factor const & factor, internal::TypeList<Path...>)
1361 {
1362 static_assert(
1363 requires { factor.template marginal<TagPath<Path...>>(); },
1364 "this factor has no marginal<Path>()"
1365 );
1366 return factor.template marginal<TagPath<Path...>>();
1367 }
1368
1372 template <std::array<bool, FactorCount> Keep, typename Target, typename SubSpace>
1373 [[nodiscard]] decltype(auto)
1374 keepOnly(Target const & target, SubSpace space) const
1375 {
1376 constexpr auto kept = maskedFactors<Keep>();
1377 if constexpr (kept.size() == 1)
1378 {
1379 return std::get<kept[0]>(m_factors);
1380 }
1381 else
1382 {
1383 return [&]<std::size_t... J>(std::index_sequence<J...>) {
1385 std::move(space),
1386 internal::placedAt(positionsOf<kept[J]>(target), std::get<kept[J]>(m_factors))...
1387 };
1388 }(std::make_index_sequence<kept.size()>{});
1389 }
1390 }
1391
1393 template <std::size_t K, typename Target>
1394 [[nodiscard]] auto
1395 positionsOf(Target const & target) const
1396 {
1397 using Factor = FactorType<K>;
1398 return internal::positionsIn<Factor::ValueDimensionAtCompileTime, Factor::MaxValueDimensionAtCompileTime>(
1399 std::get<K>(m_indices),
1400 target
1401 );
1402 }
1403
1407 template <std::size_t K, typename Target, typename Given, typename SubSpace, typename... Path, typename Derived>
1408 [[nodiscard]] auto
1409 replaceFactor(
1410 Target const & target,
1411 Given const & given,
1412 SubSpace space,
1413 internal::TypeList<Path...>,
1414 Eigen::MatrixBase<Derived> const & values
1415 ) const
1416 {
1417 auto const & factor = std::get<K>(m_factors);
1418 static_assert(
1419 requires { factor.template conditional<TagPath<Path...>>(values); },
1420 "this factor has no conditional<Path>()"
1421 );
1422 if constexpr (FactorCount == 1)
1423 {
1424 return factor.template conditional<TagPath<Path...>>(values);
1425 }
1426 else
1427 {
1428 using Raw = decltype(factor.template conditional<TagPath<Path...>>(values));
1429 using Replacement = internal::ConditionalValue<Raw>;
1430 using Error = internal::ConditionalError<Raw>;
1431 static_assert(ProductFactor<Replacement>, "this factor's conditional cannot be a factor of a product");
1432 using Result = internal::ReplacedProduct<Scalar, SubSpace, K, Replacement, std::tuple<Factors_...>>;
1433
1434 auto const build = [&](Replacement replacement) {
1435 // The replacement covers factor K's coordinates that were not given, in order.
1436 auto const & own = std::get<K>(m_indices);
1437 auto const isRemaining = [&](Index i) {
1438 return std::ranges::find(given, i) == std::ranges::end(given);
1439 };
1440 auto remaining = internal::IndexVector<
1441 Replacement::ValueDimensionAtCompileTime,
1442 Replacement::MaxValueDimensionAtCompileTime>{};
1443 remaining.resize(Index(std::ranges::count_if(own, isRemaining)));
1444 Index r = 0;
1445 for (Index const i : own)
1446 {
1447 if (isRemaining(i))
1448 {
1449 remaining(r++) = i;
1450 }
1451 }
1452 auto positions = internal::positionsIn<
1453 Replacement::ValueDimensionAtCompileTime,
1454 Replacement::MaxValueDimensionAtCompileTime>(remaining, target);
1455 return [&]<std::size_t... J>(std::index_sequence<J...>) {
1456 return Result{std::move(space), placeFactor<J, K>(target, positions, replacement)...};
1457 }(std::index_sequence_for<Factors_...>{});
1458 };
1459
1460 auto raw = factor.template conditional<TagPath<Path...>>(values);
1461 if constexpr (std::is_void_v<Error>)
1462 {
1463 return build(std::move(raw));
1464 }
1465 else
1466 {
1467 using Return = std::expected<Result, Error>;
1468 if constexpr (internal::isExpected<Raw>)
1469 {
1470 if (!raw)
1471 {
1472 return Return{std::unexpect, raw.error()};
1473 }
1474 return Return{build(std::move(*raw))};
1475 }
1476 else
1477 {
1478 if (auto checked = raw.toDensity(); !checked)
1479 {
1480 return Return{std::unexpect, checked.error()};
1481 }
1482 return Return{build(std::move(raw))};
1483 }
1484 }
1485 }
1486 }
1487
1491 template <std::size_t J, std::size_t K, typename Target, typename Positions, typename Replacement>
1492 [[nodiscard]] auto
1493 placeFactor(Target const & target, Positions const & positions, Replacement & replacement) const
1494 {
1495 if constexpr (J == K)
1496 {
1497 return internal::placedAt(positions, std::move(replacement));
1498 }
1499 else
1500 {
1501 return internal::placedAt(positionsOf<J>(target), std::get<J>(m_factors));
1502 }
1503 }
1504
1505 EMRYS_FILTER_NO_UNIQUE_ADDRESS Space_ m_space;
1506
1508 std::tuple<
1509 internal::IndexVector<Factors_::ValueDimensionAtCompileTime, Factors_::MaxValueDimensionAtCompileTime>...>
1510 m_indices;
1511
1512 std::tuple<Factors_...> m_factors;
1513};
1514
1516template <typename Space, typename First, typename... Rest>
1517Product(Space, First, Rest...) -> Product<
1518 Space,
1519 typename internal::PlacedFactor<First>::template TypeIn<Space>::Scalar,
1520 typename internal::PlacedFactor<First>::template TypeIn<Space>,
1521 typename internal::PlacedFactor<Rest>::template TypeIn<Space>...>;
1522
1523template <typename Space, typename First, typename... Rest>
1524Product(Space, Factors<First, Rest...>) -> Product<
1525 Space,
1526 typename internal::PlacedFactor<First>::template TypeIn<Space>::Scalar,
1527 typename internal::PlacedFactor<First>::template TypeIn<Space>,
1528 typename internal::PlacedFactor<Rest>::template TypeIn<Space>...>;
1529
1531
1538template <typename Scalar, typename Space, typename... Factors_, std::uniform_random_bit_generator G>
1539[[nodiscard]] SampleSet<Space, Scalar>
1540drawSamples(Product<Space, Scalar, Factors_...> const & product, Eigen::Index n, G & rng)
1541{
1542 EMRYS_FILTER_ASSERT(n >= 0);
1543 auto samples = SampleSet<Space, Scalar>{product.valueDimension(), n};
1544 product.forEachFactor([&](auto const & density, auto const & indices) {
1545 samples(indices, Eigen::placeholders::all) = drawSamples(density, n, rng);
1546 });
1547 return samples;
1548}
1549
1554template <typename Scalar, typename Space, typename... Factors_, std::uniform_random_bit_generator G>
1555[[nodiscard]] typename Moments<Space, Scalar>::VectorType
1557{
1559 sample.resize(product.valueDimension());
1560 product.forEachFactor([&](auto const & density, auto const & indices) {
1561 sample(indices) = drawSample(density, rng);
1562 });
1563 return sample;
1564}
1565
1572template <typename Scalar, typename Space, typename... Factors_, std::uniform_random_bit_generator G>
1573[[nodiscard]] SamplesWithLogDensity<Space, Scalar>
1575{
1576 EMRYS_FILTER_ASSERT(n >= 0);
1579 Eigen::Array<Scalar, Eigen::Dynamic, 1>::Zero(n)
1580 };
1581 product.forEachFactor([&](auto const & density, auto const & indices) {
1582 auto draws = drawSamplesWithLogDensity(density, n, rng);
1583 result.samples(indices, Eigen::placeholders::all) = draws.samples;
1584 result.logDensity += draws.logDensity;
1585 });
1586 return result;
1587}
1588
1594template <typename Scalar, typename Space, typename... Factors_, std::uniform_random_bit_generator G>
1595[[nodiscard]] SampleWithLogDensity<Space, Scalar>
1597{
1599 result.sample.resize(product.valueDimension());
1600 product.forEachFactor([&](auto const & density, auto const & indices) {
1601 auto const draw = drawSampleWithLogDensity(density, rng);
1602 result.sample(indices) = draw.sample;
1603 result.logDensity += draw.logDensity;
1604 });
1605 return result;
1606}
1607
1608} // namespace Emrys
What marginals and conditionals are built from: resolving Eigen index sets against a space; the share...
The macros every header includes first.
Mathematical constants and scalar functions that work in every scalar type the library accepts,...
Moments: the mean and covariance that a Gaussian carries and that a mixture or a sample set is moment...
NumericError, the reasons a factorization or a normalization can fail, and its two names,...
SampleSet: a matrix whose columns are samples in a space, which the samplers return and the unscented...
Drawing samples: SamplesWithLogDensity and SampleWithLogDensity, the Gaussian draws drawSamples(),...
The Space concept and what every continuous space shares (decisions.md [sizes-and-dimensions]): the t...
Subspaces, and growing and shrinking blocks (decisions.md [several-resizable-blocks]).
Tag paths and the index utilities (decisions.md [tag-addressing], [index-names]): TagPath and TagPath...
One factor on every repetition of a repeated block: the product of independent copies of a density,...
Definition Product.h:358
Eigen::Index Index
Eigen's index type.
Definition Product.h:373
BlockSpace SpaceType
the space the values live in
Definition Product.h:374
Moments< BlockSpace, Scalar > MomentsType
a mean and covariance together
Definition Product.h:375
typename MomentsType::VectorType VectorType
a value
Definition Product.h:376
EachRepetition(F factor, BlockSpace space)
factor on each repetition of space.
Definition Product.h:385
Scalar logDensity(Eigen::MatrixBase< Derived > const &x) const
Definition Product.h:432
auto moments() const -> std::conditional_t< MomentsMayFail, std::expected< MomentsType, MomentsError >, MomentsType >
Definition Product.h:449
typename F::Scalar Scalar
the scalar type
Definition Product.h:372
static constexpr int MaxValueDimensionAtCompileTime
Definition Product.h:382
static constexpr int ValueDimensionAtCompileTime
Definition Product.h:379
Index valueDimension() const
Definition Product.h:422
Index count() const
Definition Product.h:415
BlockSpace const & space() const
Definition Product.h:408
The factors of a product, each placed by tag path with on() or onEach(), in no space yet: a Product o...
Definition Product.h:693
Factors(Placed... placed)
The factors placed.
Definition Product.h:703
std::tuple< Placed... > const & placed() const
Definition Product.h:709
First and second moments (mean and covariance) of a distribution.
Definition Moments.h:55
DifferenceMatrix< Space_, Space_, Scalar > MatrixType
Definition Moments.h:76
ValueVector< Space_, Scalar > VectorType
Definition Moments.h:74
static constexpr int MaxDifferenceDimensionAtCompileTime
Definition Moments.h:72
void resize(Index valueDimension, Index differenceDimension)
Resizes the mean to valueDimension coordinates and the covariance to differenceDimension rows and col...
Definition Moments.h:126
static constexpr int DifferenceDimensionAtCompileTime
Definition Moments.h:69
static constexpr int MaxValueDimensionAtCompileTime
Definition Moments.h:66
static constexpr int ValueDimensionAtCompileTime
Definition Moments.h:63
The product of independent densities, each on its own coordinates of a space.
Definition Product.h:772
auto moments() const -> std::conditional_t< MomentsMayFail, std::expected< MomentsType, MomentsError >, MomentsType >
Definition Product.h:925
auto conditional(Eigen::MatrixBase< Derived > const &values) const
Definition Product.h:1005
auto const & indices() const
Definition Product.h:878
Index differenceDimension() const
Definition Product.h:855
typename MomentsType::MatrixType MatrixType
Definition Product.h:789
void forEachFactor(Fn &&fn) const
Calls fn(density, indices) for each factor in order, with its density (a view, for parameters) and th...
Definition Product.h:888
Scalar logDensity(Eigen::MatrixBase< Derived > const &x) const
Definition Product.h:898
Product(Space_ space, Placed... placed)
The product of the factors placed, made by on() or onEach(), in space.
Definition Product.h:821
std::tuple_element_t< K, std::tuple< Factors_... > > FactorType
Definition Product.h:813
decltype(auto) marginal() const &
Definition Product.h:979
typename MomentsType::VectorType VectorType
Definition Product.h:788
Scalar density(Eigen::MatrixBase< Derived > const &x) const
Definition Product.h:915
Index valueDimension() const
Definition Product.h:848
FactorType< K > const & factor() const
Definition Product.h:870
void marginal() const &&=delete
A marginal of a temporary would outlive it.
Product(Space_ space, Factors< Placed... > const &factors)
The product of factors placed in space (see Factors).
Definition Product.h:842
A density: a type that evaluates its log-density at a value in its space, such as a Gaussian,...
Definition Product.h:40
What a Product takes as a factor: a density (HasLogDensity), or parameters that toDensity() checks,...
Definition Product.h:79
A type usable as the scalar of a distribution: a real number type such as float, double,...
Definition Math.h:80
A type usable as the space of a distribution.
Definition Space.h:186
#define EMRYS_FILTER_NO_UNIQUE_ADDRESS
Assertion used for programmer errors (misuse of the API).
Definition Macros.h:38
MomentsError
Reasons a distribution's moments() can fail.
Definition Moments.h:32
SampleWithLogDensity< Space, Scalar > drawSampleWithLogDensity(Gamma< Space, Scalar > const &gamma, G &rng)
Definition Gamma.h:546
Eigen::Matrix< Scalar, valueDimensionAtCompileTime< Space >, Eigen::Dynamic,(maxValueDimensionAtCompileTime< Space >==1 ? Eigen::RowMajor :Eigen::ColMajor), maxValueDimensionAtCompileTime< Space >, Eigen::Dynamic > SampleSet
A matrix whose columns are samples in Space (see Spaces), with the space's size and storage bound.
Definition SampleSet.h:33
Eigen::Index drawSample(Categorical< Space, Scalar > const &distribution, G &rng)
Definition Categorical.h:1111
std::vector< Eigen::Index > drawSamples(Categorical< Space, Scalar > const &distribution, Eigen::Index n, G &rng)
Definition Categorical.h:1087
SamplesWithLogDensity< Space, Scalar > drawSamplesWithLogDensity(Gamma< Space, Scalar > const &gamma, Eigen::Index n, G &rng)
Definition Gamma.h:529
GivenBlock< Space, P, Value > given(Space const &space, Value const &value)
Definition Measurement.h:917
Sample< std::remove_cvref_t< G > > sample(G &&function)
Definition Policies.h:665
Eigen::Index valueDimension(S const &space)
Definition Space.h:272
auto block(Space auto const &space, X &&x)
Definition TagPath.h:979
Eigen::Index differenceDimension(S const &space)
Definition Space.h:284
constexpr bool isLinear
Whether S is linear: its values and its differences are both plain vectors.
Definition Space.h:321
constexpr auto idx()
Definition TagPath.h:658
One sample in Space, in a vector sized by the space, together with the log of the density it was draw...
Definition Sampling.h:50
Moments< Space, Scalar >::VectorType sample
the sample
Definition Sampling.h:51
Samples in Space together with the log of the density they were drawn from, evaluated at each.
Definition Sampling.h:36
A path of tags, each naming a block inside the one before: TagPath<Ownship, Position> is the Position...
Definition TagPath.h:353