EmrysFilter 0.1.0
State estimation utilities in C++23 using Eigen
Loading...
Searching...
No Matches
TransformArgs.h
Go to the documentation of this file.
1#pragma once
2
11
12#include <algorithm>
13#include <array>
14#include <cstddef>
15#include <tuple>
16#include <type_traits>
17#include <utility>
18
19#include <unsupported/Eigen/AutoDiff>
20
33
34namespace Emrys
35{
36
37namespace internal
38{
39
41template <typename T>
42inline constexpr bool isWithJacobian = false;
43
45template <typename Value, typename Jacobian>
46inline constexpr bool isWithJacobian<WithJacobian<Value, Jacobian>>{true};
47
49
53template <typename Returned>
54[[nodiscard]] decltype(auto)
55valueOf(Returned && returned)
56{
57 if constexpr (isWithJacobian<std::remove_cvref_t<Returned>>)
58 {
59 return (std::forward<Returned>(returned).value);
60 }
61 else
62 {
63 return std::forward<Returned>(returned);
64 }
65}
66
72template <typename Blocks>
73struct ArgumentJacobians
74{
75 Blocks blocks;
76};
77
79template <typename T>
80inline constexpr bool isArgumentJacobians = false;
81
83template <typename Blocks>
84inline constexpr bool isArgumentJacobians<ArgumentJacobians<Blocks>>{true};
85
87
91template <std::size_t K, typename Jacobian>
92[[nodiscard]] auto const &
93jacobianBlock(Jacobian const & jacobian)
94{
95 if constexpr (requires { std::tuple_size<Jacobian>::value; })
96 {
97 return std::get<K>(jacobian);
98 }
99 else
100 {
101 static_assert(K == 0, "a function with more than one uncertain input must return a tuple of Jacobian blocks");
102 return jacobian;
103 }
104}
105
111template <int AffineSize, int AffineMaxSize, typename Points>
112struct FactoredAt
113{
114 Points points;
115 IndexVector<AffineSize, AffineMaxSize> affine;
116};
117
119template <typename T>
120inline constexpr bool isFactoredAt = false;
121
123template <int AffineSize, int AffineMaxSize, typename Points>
124inline constexpr bool isFactoredAt<FactoredAt<AffineSize, AffineMaxSize, Points>>{true};
126
128template <int... Sizes>
129inline constexpr int sizeSum = (false || ... || (Sizes == Eigen::Dynamic)) ? Eigen::Dynamic : (0 + ... + Sizes);
130
132template <int A, int B>
133inline constexpr int sizeProduct = A == Eigen::Dynamic || B == Eigen::Dynamic ? Eigen::Dynamic : A * B;
134
135} // namespace internal
136
141template <typename Input>
143{
144 Input const & input;
145};
146
154template <typename Reference>
156{
158 using ReferenceType = Reference;
159
164
165 Reference reference;
166
168 [[nodiscard]] decltype(auto)
169 mean() const
170 {
171 if constexpr (IsDistribution)
172 {
173 return reference.mean();
174 }
175 else
176 {
177 return (reference);
178 }
179 }
180};
181
182namespace internal
183{
184
186template <typename T>
187inline constexpr bool isLinearizedAbout = false;
188
190template <typename Reference>
191inline constexpr bool isLinearizedAbout<LinearizedAbout<Reference>>{true};
192
194
198template <typename... Args>
199inline constexpr bool leadsWithReference = false;
200
202template <typename First, typename... Rest>
203inline constexpr bool leadsWithReference<First, Rest...> = isLinearizedAbout<First>;
204
206
210struct NoReference
211{};
212
214template <typename T>
215concept IsSpace = Space<T>;
216
218template <typename Arg>
219struct ArgTraits
220{
222 using Input = Arg;
223
225 using Scalar = void;
226
227 static constexpr bool isUncertain = false;
228 static constexpr bool hasCross = false;
229 static constexpr int size = 0;
230 static constexpr int maxSize = 0;
231 static constexpr int differenceSize = 0;
232 static constexpr int differenceMaxSize = 0;
233};
234
236template <UncertainInput Arg>
237struct ArgTraits<Arg>
238{
240 using Input = Arg;
241
243 using Scalar = typename Arg::Scalar;
244
245 static constexpr bool isUncertain = true;
246 static constexpr bool hasCross = false;
247 static constexpr int size = Arg::ValueDimensionAtCompileTime;
248 static constexpr int maxSize = Arg::MaxValueDimensionAtCompileTime;
249 static constexpr int differenceSize = Arg::DifferenceDimensionAtCompileTime;
250 static constexpr int differenceMaxSize = Arg::MaxDifferenceDimensionAtCompileTime;
251};
252
254template <typename In>
255struct ArgTraits<CrossMarked<In>> : ArgTraits<In>
256{
257 static constexpr bool hasCross = true;
258};
259
261template <typename... Args>
262inline constexpr std::size_t crossCount = (std::size_t{ArgTraits<Args>::hasCross} + ... + 0);
263
265template <typename Arg>
266[[nodiscard]] auto const &
267argInput(Arg const & arg)
268{
269 if constexpr (ArgTraits<Arg>::hasCross)
270 {
271 return arg.input;
272 }
273 else
274 {
275 return arg;
276 }
277}
278
280template <typename First, typename... Rest>
281struct FirstNonVoidOf
282{
284 using type = std::conditional_t<std::is_void_v<First>, typename FirstNonVoidOf<Rest...>::type, First>;
285};
286
288template <typename First>
289struct FirstNonVoidOf<First>
290{
292 using type = First;
293};
294
296template <typename... T>
297using FirstNonVoid = typename FirstNonVoidOf<T...>::type;
298
300template <typename... Args>
301struct TransformArgs
302{
303 static constexpr std::size_t count = sizeof...(Args);
304 static constexpr std::array<bool, count> uncertain{ArgTraits<Args>::isUncertain...};
305
307 static constexpr std::size_t uncertainCount = (std::size_t{ArgTraits<Args>::isUncertain} + ... + 0);
308
309 static_assert(uncertainCount > 0, "transform() needs at least one uncertain input");
310 static_assert(crossCount<Args...> <= 1, "withCross() marks at most one input");
311
313 using Scalar = FirstNonVoid<typename ArgTraits<Args>::Scalar...>;
314
315 static_assert(
316 ((std::is_void_v<typename ArgTraits<Args>::Scalar> || std::is_same_v<typename ArgTraits<Args>::Scalar, Scalar>)
317 && ...),
318 "every uncertain input must have the same scalar type"
319 );
320
322 static constexpr int size = sizeSum<ArgTraits<Args>::size...>;
323
324 static constexpr int maxSize = sizeSum<ArgTraits<Args>::maxSize...>;
325
327 static constexpr int differenceSize = sizeSum<ArgTraits<Args>::differenceSize...>;
328
330 static constexpr int differenceMaxSize = sizeSum<ArgTraits<Args>::differenceMaxSize...>;
331
333 static constexpr std::array<std::size_t, count> ordinal{[] {
334 auto result = std::array<std::size_t, count>{};
335 std::size_t next = 0;
336 for (std::size_t i = 0; i < count; ++i)
337 {
338 result[i] = uncertain[i] ? next++ : 0;
339 }
340 return result;
341 }()};
342
344 static constexpr std::size_t crossPosition = [] {
345 constexpr auto marked = std::array<bool, count>{ArgTraits<Args>::hasCross...};
346 for (std::size_t i = 0; i < count; ++i)
347 {
348 if (marked[i])
349 {
350 return i;
351 }
352 }
353 return count;
354 }();
355};
356
364template <typename F, typename OutputSpace, typename... Args>
365class Transform
366{
367public:
368
370 using ArgInfo = TransformArgs<Args...>;
371
373 using Scalar = typename ArgInfo::Scalar;
374
376 using Index = Eigen::Index;
377
379 using OutputSpaceType = OutputSpace;
380
381 static constexpr int InputSize = ArgInfo::size;
382 static constexpr int InputMaxSize = ArgInfo::maxSize;
383 static constexpr int InputDifferenceSize = ArgInfo::differenceSize;
384 static constexpr int InputDifferenceMaxSize = ArgInfo::differenceMaxSize;
385 static constexpr int OutputSize = valueDimensionAtCompileTime<OutputSpace>;
386 static constexpr int OutputMaxSize = maxValueDimensionAtCompileTime<OutputSpace>;
387
389 static constexpr int OutputDifferenceSize = valueDimensionAtCompileTime<DifferenceSpace<OutputSpace>>;
390
391 static constexpr int OutputDifferenceMaxSize =
393
395 template <typename T>
396 using InputVector = Eigen::Matrix<T, InputSize, 1, Eigen::ColMajor, InputMaxSize, 1>;
397
399 template <typename T>
400 using InputDifferenceVector = Eigen::Matrix<T, InputDifferenceSize, 1, Eigen::ColMajor, InputDifferenceMaxSize, 1>;
401
403 template <typename T>
404 using OutputVector = Eigen::Matrix<T, OutputSize, 1, Eigen::ColMajor, OutputMaxSize, 1>;
405
407 template <typename T>
408 using OutputDifferenceVector =
409 Eigen::Matrix<T, OutputDifferenceSize, 1, Eigen::ColMajor, OutputDifferenceMaxSize, 1>;
410
412 using InputMatrix = Eigen::Matrix<
413 Scalar,
414 InputDifferenceSize,
415 InputDifferenceSize,
416 Eigen::ColMajor,
417 InputDifferenceMaxSize,
418 InputDifferenceMaxSize>;
419
421 using OutputMatrix = Eigen::Matrix<
422 Scalar,
423 OutputDifferenceSize,
424 OutputDifferenceSize,
425 Eigen::ColMajor,
426 OutputDifferenceMaxSize,
427 OutputDifferenceMaxSize>;
428
430 using JacobianMatrix =
431 MatrixOf<Scalar, OutputDifferenceSize, InputDifferenceSize, OutputDifferenceMaxSize, InputDifferenceMaxSize>;
432
434 using CrossMatrix =
435 MatrixOf<Scalar, InputDifferenceSize, OutputDifferenceSize, InputDifferenceMaxSize, OutputDifferenceMaxSize>;
436
440 template <int Points = InputDifferenceSize, int MaxPoints = InputDifferenceMaxSize>
441 using OutputSamples = MatrixOf<Scalar, OutputSize, Points, OutputMaxSize, MaxPoints>;
442
444 template <int Points = InputDifferenceSize, int MaxPoints = InputDifferenceMaxSize>
445 using OutputDeviations = MatrixOf<Scalar, OutputDifferenceSize, Points, OutputDifferenceMaxSize, MaxPoints>;
446
453 struct RootResult
454 {
455 OutputVector<Scalar> mean;
456 InputMatrix root;
457 CrossMatrix whitened;
458 OutputMatrix unexplained;
459 };
460
465 Transform(F & f, OutputSpace const & outputSpace, Args const &... args)
466 : m_f{f}
467 , m_outputSpace{outputSpace}
468 , m_args{args...}
469 {
470 forEachUncertain([this]<std::size_t I>(auto const & input) {
471 m_offsets[I] = m_inputSize;
472 m_differenceOffsets[I] = m_inputDifferenceSize;
473 m_inputSize += input.valueDimension();
474 m_inputDifferenceSize += input.differenceDimension();
475 });
476 // A resizable output space with one resizable block takes the size f returns.
477 if constexpr (OutputSize == Eigen::Dynamic && requires(Index n) { m_outputSpace.resize(n); })
478 {
479 Index const outputSize = OutputVector<Scalar>{valueOf(call<Scalar>(m_f, mean()))}.size();
480 if (valueDimension(m_outputSpace) != outputSize)
481 {
482 m_outputSpace.resize(outputSize);
483 }
484 }
485 else if constexpr (OutputSize == Eigen::Dynamic)
486 {
487 // Several resizable blocks: f's output can't say how to split, so the space must fit it.
488 EMRYS_FILTER_ASSERT(
489 OutputVector<Scalar>{valueOf(call<Scalar>(m_f, mean()))}.size() == valueDimension(m_outputSpace)
490 && "the function's output does not fit the output space"
491 );
492 }
493 }
494
496 [[nodiscard]] OutputSpace const &
497 outputSpace() const
498 {
499 return m_outputSpace;
500 }
501
503 [[nodiscard]] Index
504 inputSize() const
505 {
506 return m_inputSize;
507 }
508
510 [[nodiscard]] Index
511 inputDifferenceSize() const
512 {
513 return m_inputDifferenceSize;
514 }
515
517 [[nodiscard]] Index
518 outputDifferenceSize() const
519 {
520 return differenceDimension(m_outputSpace);
521 }
522
524 [[nodiscard]] InputVector<Scalar>
525 mean() const
526 {
527 InputVector<Scalar> result = InputVector<Scalar>::Zero(m_inputSize);
528 forEachUncertain([&]<std::size_t I>(auto const & input) {
529 result.segment(m_offsets[I], input.valueDimension()) = input.mean();
530 });
531 return result;
532 }
533
535 [[nodiscard]] InputMatrix
536 covarianceLower() const
537 {
538 InputMatrix result = InputMatrix::Zero(m_inputDifferenceSize, m_inputDifferenceSize);
539 forEachUncertain([&]<std::size_t I>(auto const & input) {
540 Index const d = input.differenceDimension();
541 result.block(m_differenceOffsets[I], m_differenceOffsets[I], d, d) = symmetricCov(input);
542 });
543 return result;
544 }
545
555 template <typename A = roots::DefaultAxes>
556 [[nodiscard]] InputMatrix
557 root(A const & axes = {}) const
558 {
559 internal::refuseFailingAxes<A>();
560 InputMatrix result = InputMatrix::Zero(m_inputDifferenceSize, m_inputDifferenceSize);
561 forEachUncertain([&]<std::size_t I>(auto const & input) {
562 Index const d = input.differenceDimension();
563 auto const take = [&](auto const & entry) {
564 if constexpr (requires { input.root(); })
565 {
566 return internal::rootValue(internal::rootFromFactor(entry, input.root()));
567 }
568 else
569 {
570 return internal::rootValue(entry.root(covarianceLowerOf(input)));
571 }
572 };
573 constexpr bool spread =
574 ArgInfo::crossPosition < ArgInfo::count ? I == ArgInfo::crossPosition : ArgInfo::ordinal[I] == 0;
575 if constexpr (spread)
576 {
577 result.block(m_differenceOffsets[I], m_differenceOffsets[I], d, d) = take(axes);
578 }
579 else
580 {
581 result.block(m_differenceOffsets[I], m_differenceOffsets[I], d, d) = take(roots::DefaultAxes{});
582 }
583 });
584 return result;
585 }
586
590 template <typename X, typename D>
591 [[nodiscard]] InputVector<typename X::Scalar>
592 plusInputs(Eigen::MatrixBase<X> const & x, Eigen::MatrixBase<D> const & d) const
593 {
594 InputVector<typename X::Scalar> result = x;
595 forEachUncertain([&]<std::size_t I>(auto const & input) {
596 using InputSpace = std::remove_cvref_t<decltype(input.space())>;
597 constexpr int Values = internal::rowsAtCompileTime<false, InputSpace>;
598 constexpr int Differences = internal::rowsAtCompileTime<true, InputSpace>;
599 internal::middleRowsOf<Values>(result, m_offsets[I], input.valueDimension()) = Emrys::plus(
600 input.space(),
601 internal::middleRowsOf<Values>(x, m_offsets[I], input.valueDimension()),
602 internal::middleRowsOf<Differences>(d, m_differenceOffsets[I], input.differenceDimension())
603 );
604 });
605 return result;
606 }
607
612 template <typename X, typename D>
613 [[nodiscard]] InputVector<typename X::Scalar>
614 stepInputs(Eigen::MatrixBase<X> const & x, Eigen::MatrixBase<D> const & d) const
615 {
616 InputVector<typename X::Scalar> result = x;
617 forEachUncertain([&]<std::size_t I>(auto const & input) {
618 using InputSpace = std::remove_cvref_t<decltype(input.space())>;
619 constexpr int Values = internal::rowsAtCompileTime<false, InputSpace>;
620 constexpr int Differences = internal::rowsAtCompileTime<true, InputSpace>;
621 auto values = internal::middleRowsOf<Values>(result, m_offsets[I], input.valueDimension());
622 auto const step =
623 internal::middleRowsOf<Differences>(d, m_differenceOffsets[I], input.differenceDimension());
624 if constexpr (HasPlus<InputSpace>)
625 {
626 values = Emrys::plus(
627 input.space(),
628 internal::middleRowsOf<Values>(x, m_offsets[I], input.valueDimension()),
629 step
630 );
631 }
632 else
633 {
634 values += step;
635 }
636 });
637 return result;
638 }
639
643 template <typename T>
644 [[nodiscard]] OutputVector<T>
645 evaluate(InputVector<T> const & x) const
646 {
647 OutputVector<T> y = valueOf(call<T>(m_f, x));
648 EMRYS_FILTER_ASSERT(y.size() == valueDimension(m_outputSpace) && "the output does not fit the output space");
649 return y;
650 }
651
654 static constexpr bool EvaluatesBatches = requires { requires std::remove_cvref_t<F>::EvaluatesBatches; };
655
657 template <typename T, int Points, int MaxPoints>
658 using InputSamples = MatrixOf<T, InputSize, Points, InputMaxSize, MaxPoints>;
659
666 template <typename Outputs, typename PointAt>
667 void
668 evaluateEach(Index count, PointAt const & pointAt, Outputs & outputs) const
669 {
670 using T = typename Outputs::Scalar;
671 outputs.resize(valueDimension(m_outputSpace), count);
672 if constexpr (EvaluatesBatches)
673 {
674 auto x = InputSamples<T, Outputs::ColsAtCompileTime, Outputs::MaxColsAtCompileTime>{};
675 x.resize(m_inputSize, count);
676 for (Index j = 0; j < count; ++j)
677 {
678 x.col(j) = pointAt(j);
679 }
680 [&]<std::size_t... I>(std::index_sequence<I...>) {
681 m_f.evaluateBatch(outputs, argumentColumns<I>(x)...);
682 }(std::index_sequence_for<Args...>{});
683 }
684 else
685 {
686 for (Index j = 0; j < count; ++j)
687 {
688 outputs.col(j) = evaluate<T>(pointAt(j));
689 }
690 }
691 }
692
699 template <typename Outputs, typename Jacobians, typename PointAt>
700 void
701 evaluateEachWithAffineJacobians(
702 Index count,
703 PointAt const & pointAt,
704 Outputs & outputs,
705 Jacobians & jacobians
706 ) const
707 requires EvaluatesBatches
708 {
709 outputs.resize(valueDimension(m_outputSpace), count);
710 auto x = InputSamples<Scalar, Outputs::ColsAtCompileTime, Outputs::MaxColsAtCompileTime>{};
711 x.resize(m_inputSize, count);
712 for (Index j = 0; j < count; ++j)
713 {
714 x.col(j) = pointAt(j);
715 }
716 [&]<std::size_t... I>(std::index_sequence<I...>) {
717 m_f.evaluateBatchWithAffineJacobians(outputs, jacobians, argumentColumns<I>(x)...);
718 }(std::index_sequence_for<Args...>{});
719 }
720
724 template <typename T>
725 [[nodiscard]] decltype(auto)
726 returned(InputVector<T> const & x) const
727 {
728 return call<T>(m_f, x);
729 }
730
732 template <typename T, typename G>
733 [[nodiscard]] decltype(auto)
734 call(G & g, InputVector<T> const & x) const
735 {
736 return [&]<std::size_t... I>(std::index_sequence<I...>) -> decltype(auto) {
737 return callWithScalar<T>(g, argument<T, I>(x)...);
738 }(std::index_sequence_for<Args...>{});
739 }
740
742 template <typename Fn>
743 void
744 forEachUncertain(Fn && fn) const
745 {
746 [&]<std::size_t... I>(std::index_sequence<I...>) {
747 (visitIfUncertain<I>(fn), ...);
748 }(std::index_sequence_for<Args...>{});
749 }
750
752 [[nodiscard]] Index
753 offset(std::size_t i) const
754 {
755 return m_offsets[i];
756 }
757
759 [[nodiscard]] Index
760 differenceOffset(std::size_t i) const
761 {
762 return m_differenceOffsets[i];
763 }
764
774 template <typename MethodResult>
775 [[nodiscard]] auto
776 assemble(MethodResult const & result) const
777 {
778 constexpr bool Root = std::is_same_v<MethodResult, RootResult>;
779 Index const m = outputDifferenceSize();
780 if constexpr (ArgInfo::crossPosition == ArgInfo::count)
781 {
782 OutputMatrix cov = [&] {
783 if constexpr (Root)
784 {
785 OutputMatrix parts = result.unexplained;
786 parts.noalias() += multiply(result.whitened.transpose(), result.whitened);
787 return parts;
788 }
789 else
790 {
791 CrossMatrix const cross = multiply(covarianceLower(), result.jacobian.transpose());
792 return OutputMatrix{multiply(result.jacobian, cross)};
793 }
794 }();
795 cov = (Scalar(0.5) * (cov + cov.transpose())).eval();
796 return GaussianParams<OutputSpace, Scalar>{m_outputSpace, result.mean, std::move(cov)};
797 }
798 else
799 {
800 constexpr std::size_t position = ArgInfo::crossPosition;
801 using InputSpace = typename ArgTraits<ArgAt<position>>::Input::SpaceType;
802 using Form = std::conditional_t<Root, RootForm, JacobianForm>;
803 using WithCrossResult = OutputWithCross<InputSpace, OutputSpace, Form, Scalar>;
804 Index const offset = m_differenceOffsets[position];
805 Index const rows = argInput(std::get<position>(m_args)).differenceDimension();
806 auto const symmetric = [](OutputMatrix & a) {
807 a = (Scalar(0.5) * (a + a.transpose())).eval();
808 };
809 // N: each other input's share, from its own columns of J or rows of Psi.
810 OutputMatrix inputs = OutputMatrix::Zero(m, m);
811 forEachUncertain([&]<std::size_t I>(auto const & input) {
812 if constexpr (I != position)
813 {
814 using Input = std::remove_cvref_t<decltype(input)>;
815 using Shared = MatrixOf<
816 Scalar,
817 Input::DifferenceDimensionAtCompileTime,
818 OutputDifferenceSize,
819 Input::MaxDifferenceDimensionAtCompileTime,
820 OutputDifferenceMaxSize>;
821 Index const at = m_differenceOffsets[I];
822 Index const d = input.differenceDimension();
823 if constexpr (Root)
824 {
825 Shared const block = result.whitened.middleRows(at, d);
826 inputs.noalias() += multiply(block.transpose(), block);
827 }
828 else
829 {
830 Shared const shared =
831 multiply(symmetricCov(input), result.jacobian.middleCols(at, d).transpose());
832 inputs.noalias() += multiply(result.jacobian.middleCols(at, d), shared);
833 }
834 }
835 });
836 symmetric(inputs);
837 if constexpr (Root)
838 {
839 typename WithCrossResult::CrossMatrix whitened = result.whitened.middleRows(offset, rows);
840 OutputMatrix explained = multiply(whitened.transpose(), whitened);
841 symmetric(explained);
842 return WithCrossResult{
843 m_outputSpace,
844 result.mean,
845 typename WithCrossResult::RootMatrix{result.root.block(offset, offset, rows, rows)},
846 std::move(whitened),
847 std::move(explained),
848 std::move(inputs),
849 result.unexplained
850 };
851 }
852 else
853 {
854 typename WithCrossResult::JacobianMatrix jacobian = result.jacobian.middleCols(offset, rows);
855 typename WithCrossResult::CrossMatrix cross =
856 multiply(symmetricCov(argInput(std::get<position>(m_args))), jacobian.transpose());
857 OutputMatrix explained = multiply(jacobian, cross);
858 symmetric(explained);
859 return WithCrossResult{
860 m_outputSpace,
861 result.mean,
862 std::move(jacobian),
863 std::move(cross),
864 std::move(explained),
865 std::move(inputs),
866 OutputMatrix::Zero(m, m)
867 };
868 }
869 }
870 }
871
872private:
873
875 template <std::size_t I>
876 using ArgAt = std::tuple_element_t<I, std::tuple<Args...>>;
877
879 template <typename Input>
880 [[nodiscard]] static auto
881 symmetricCov(Input const & input)
882 {
883 using Block = Eigen::Matrix<
884 Scalar,
885 Input::DifferenceDimensionAtCompileTime,
886 Input::DifferenceDimensionAtCompileTime,
887 Eigen::ColMajor,
888 Input::MaxDifferenceDimensionAtCompileTime,
889 Input::MaxDifferenceDimensionAtCompileTime>;
890 Block cov = covarianceLowerOf(input);
891 cov.template triangularView<Eigen::StrictlyUpper>() = cov.transpose();
892 return cov;
893 }
894
896 template <std::size_t I, typename Fn>
897 void
898 visitIfUncertain(Fn & fn) const
899 {
900 if constexpr (ArgInfo::uncertain[I])
901 {
902 fn.template operator()<I>(argInput(std::get<I>(m_args)));
903 }
904 }
905
907 template <typename T, std::size_t I>
908 [[nodiscard]] decltype(auto)
909 argument(InputVector<T> const & x) const
910 {
911 auto const & arg = std::get<I>(m_args);
912 if constexpr (ArgInfo::uncertain[I])
913 {
914 using Input = typename ArgTraits<ArgAt<I>>::Input;
915 using Block = Eigen::Matrix<
916 T,
917 Input::ValueDimensionAtCompileTime,
918 1,
919 Eigen::ColMajor,
920 Input::MaxValueDimensionAtCompileTime,
921 1>;
922 return Block{x.segment(m_offsets[I], argInput(arg).valueDimension())};
923 }
924 else
925 {
926 return arg;
927 }
928 }
929
933 template <std::size_t I, typename X>
934 [[nodiscard]] decltype(auto)
935 argumentColumns(X const & x) const
936 {
937 auto const & arg = std::get<I>(m_args);
938 if constexpr (ArgInfo::uncertain[I])
939 {
940 using Input = typename ArgTraits<ArgAt<I>>::Input;
941 return internal::middleRowsOf<Input::ValueDimensionAtCompileTime>(
942 x,
943 m_offsets[I],
944 argInput(arg).valueDimension()
945 );
946 }
947 else
948 {
949 return (arg);
950 }
951 }
952
956 template <typename T, std::size_t... I>
957 static auto returnedAt(std::index_sequence<I...>) -> decltype(std::declval<WithScalar<T, F> const &>()(
958 std::declval<Transform const &>().template argument<T, I>(std::declval<InputVector<T> const &>())...
959 ));
960
961public:
962
967 template <typename T>
968 using Returned = decltype(returnedAt<T>(std::index_sequence_for<Args...>{}));
969
970private:
971
972 F & m_f;
973 OutputSpace m_outputSpace;
974 std::tuple<Args const &...> m_args;
975 std::array<Index, sizeof...(Args)> m_offsets{};
976 std::array<Index, sizeof...(Args)> m_differenceOffsets{};
977 Index m_inputSize = 0;
978 Index m_inputDifferenceSize = 0;
979};
980
981} // namespace internal
982
983} // namespace Emrys
The coordinate types (decisions.md [no-bounded-coordinates]): Linear, plain coordinates,...
How a function that stores a matrix argument takes it (decisions.md [forwarded-matrices]): the concep...
The Gaussian: its parameters GaussianParams, the Gaussian that evaluates the density,...
What marginals and conditionals are built from: resolving Eigen index sets against a space; the share...
What a transform returns and what a measurement update reads from it (decisions.md [linearization-for...
The macros every header includes first.
Mathematical constants and scalar functions that work in every scalar type the library accepts,...
The transform methods, as policies made by their factories (decisions.md [policy-factories],...
Products of fixed-size matrices by Eigen's fixed-size product at every size: multiply() is by Eigen'...
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...
An argument of transform() that is uncertain: something with a mean, a space, and either a covariance...
Definition Linearization.h:43
constexpr AxisIndex< Path, sizeof...(I)> at(I... indices)
Definition Discrete.h:1071
constexpr auto symmetric(Fallbacks... fallbacks)
Definition Roots.h:1204
WithFallbacks< Cholesky, Pivoted > DefaultAxes
The axes every root the library takes uses unless told otherwise, cholesky(pivoted()): the Cholesky f...
Definition Roots.h:1254
constexpr Shared shared()
Definition Batched.h:63
Eigen::Index valueDimension(S const &space)
Definition Space.h:272
auto plus(S const &space, Eigen::MatrixBase< XDerived > const &x, Eigen::MatrixBase< DDerived > const &d)
Definition Space.h:675
auto block(Space auto const &space, X &&x)
Definition TagPath.h:979
Eigen::Index differenceDimension(S const &space)
Definition Space.h:284
constexpr int valueDimensionAtCompileTime
The value dimension of Space at compile time, or Eigen::Dynamic if it is resizable.
Definition Space.h:251
constexpr int maxValueDimensionAtCompileTime
The largest valid value dimension of Space, or Eigen::Dynamic if it is unbounded.
Definition Space.h:258
constexpr Spread< Rule, A > spread(Rule rule, A axes)
Definition Methods.h:750
An uncertain input marked by withCross().
Definition TransformArgs.h:143
Input const & input
the marked input
Definition TransformArgs.h:144
Where a step linearizes its input: a point, or a distribution, in place of the input's own mean and s...
Definition TransformArgs.h:156
Reference ReferenceType
The point's type, or a reference to the distribution.
Definition TransformArgs.h:158
decltype(auto) mean() const
Definition TransformArgs.h:169
static constexpr bool IsDistribution
Whether the reference is a distribution, which every method linearizes over, rather than a point,...
Definition TransformArgs.h:163
Reference reference
the point, or the distribution
Definition TransformArgs.h:165