19#include <unsupported/Eigen/AutoDiff>
42inline constexpr bool isWithJacobian =
false;
45template <
typename Value,
typename Jacobian>
46inline constexpr bool isWithJacobian<WithJacobian<Value, Jacobian>>{
true};
53template <
typename Returned>
54[[nodiscard]]
decltype(
auto)
55valueOf(Returned && returned)
57 if constexpr (isWithJacobian<std::remove_cvref_t<Returned>>)
59 return (std::forward<Returned>(returned).value);
63 return std::forward<Returned>(returned);
72template <
typename Blocks>
73struct ArgumentJacobians
80inline constexpr bool isArgumentJacobians =
false;
83template <
typename Blocks>
84inline constexpr bool isArgumentJacobians<ArgumentJacobians<Blocks>>{
true};
91template <std::
size_t K,
typename Jacobian>
92[[nodiscard]]
auto const &
93jacobianBlock(Jacobian
const & jacobian)
95 if constexpr (
requires { std::tuple_size<Jacobian>::value; })
97 return std::get<K>(jacobian);
101 static_assert(K == 0,
"a function with more than one uncertain input must return a tuple of Jacobian blocks");
111template <
int AffineSize,
int AffineMaxSize,
typename Po
ints>
115 IndexVector<AffineSize, AffineMaxSize> affine;
120inline constexpr bool isFactoredAt =
false;
123template <
int AffineSize,
int AffineMaxSize,
typename Po
ints>
124inline constexpr bool isFactoredAt<FactoredAt<AffineSize, AffineMaxSize, Points>>{
true};
128template <
int... Sizes>
129inline constexpr int sizeSum = (
false || ... || (Sizes == Eigen::Dynamic)) ? Eigen::Dynamic : (0 + ... + Sizes);
132template <
int A,
int B>
133inline constexpr int sizeProduct = A == Eigen::Dynamic || B == Eigen::Dynamic ? Eigen::Dynamic : A * B;
141template <
typename Input>
154template <
typename Reference>
168 [[nodiscard]]
decltype(
auto)
187inline constexpr bool isLinearizedAbout =
false;
190template <
typename Reference>
191inline constexpr bool isLinearizedAbout<LinearizedAbout<Reference>>{
true};
198template <
typename... Args>
199inline constexpr bool leadsWithReference =
false;
202template <
typename First,
typename... Rest>
203inline constexpr bool leadsWithReference<First, Rest...> = isLinearizedAbout<First>;
215concept IsSpace = Space<T>;
218template <
typename Arg>
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;
236template <UncertainInput Arg>
243 using Scalar =
typename Arg::Scalar;
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;
254template <
typename In>
255struct ArgTraits<CrossMarked<In>> : ArgTraits<In>
257 static constexpr bool hasCross =
true;
261template <
typename... Args>
262inline constexpr std::size_t crossCount = (std::size_t{ArgTraits<Args>::hasCross} + ... + 0);
265template <
typename Arg>
266[[nodiscard]]
auto const &
267argInput(Arg
const & arg)
269 if constexpr (ArgTraits<Arg>::hasCross)
280template <
typename First,
typename... Rest>
284 using type = std::conditional_t<std::is_void_v<First>,
typename FirstNonVoidOf<Rest...>::type, First>;
288template <
typename First>
289struct FirstNonVoidOf<First>
296template <
typename... T>
297using FirstNonVoid =
typename FirstNonVoidOf<T...>::type;
300template <
typename... Args>
303 static constexpr std::size_t count =
sizeof...(Args);
304 static constexpr std::array<bool, count> uncertain{ArgTraits<Args>::isUncertain...};
307 static constexpr std::size_t uncertainCount = (std::size_t{ArgTraits<Args>::isUncertain} + ... + 0);
309 static_assert(uncertainCount > 0,
"transform() needs at least one uncertain input");
310 static_assert(crossCount<Args...> <= 1,
"withCross() marks at most one input");
313 using Scalar = FirstNonVoid<typename ArgTraits<Args>::Scalar...>;
316 ((std::is_void_v<typename ArgTraits<Args>::Scalar> || std::is_same_v<typename ArgTraits<Args>::Scalar, Scalar>)
318 "every uncertain input must have the same scalar type"
322 static constexpr int size = sizeSum<ArgTraits<Args>::size...>;
324 static constexpr int maxSize = sizeSum<ArgTraits<Args>::maxSize...>;
327 static constexpr int differenceSize = sizeSum<ArgTraits<Args>::differenceSize...>;
330 static constexpr int differenceMaxSize = sizeSum<ArgTraits<Args>::differenceMaxSize...>;
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)
338 result[i] = uncertain[i] ? next++ : 0;
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)
364template <
typename F,
typename OutputSpace,
typename... Args>
370 using ArgInfo = TransformArgs<Args...>;
373 using Scalar =
typename ArgInfo::Scalar;
376 using Index = Eigen::Index;
379 using OutputSpaceType = OutputSpace;
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;
391 static constexpr int OutputDifferenceMaxSize =
395 template <
typename T>
396 using InputVector = Eigen::Matrix<T, InputSize, 1, Eigen::ColMajor, InputMaxSize, 1>;
399 template <
typename T>
400 using InputDifferenceVector = Eigen::Matrix<T, InputDifferenceSize, 1, Eigen::ColMajor, InputDifferenceMaxSize, 1>;
403 template <
typename T>
404 using OutputVector = Eigen::Matrix<T, OutputSize, 1, Eigen::ColMajor, OutputMaxSize, 1>;
407 template <
typename T>
408 using OutputDifferenceVector =
409 Eigen::Matrix<T, OutputDifferenceSize, 1, Eigen::ColMajor, OutputDifferenceMaxSize, 1>;
412 using InputMatrix = Eigen::Matrix<
417 InputDifferenceMaxSize,
418 InputDifferenceMaxSize>;
421 using OutputMatrix = Eigen::Matrix<
423 OutputDifferenceSize,
424 OutputDifferenceSize,
426 OutputDifferenceMaxSize,
427 OutputDifferenceMaxSize>;
430 using JacobianMatrix =
431 MatrixOf<Scalar, OutputDifferenceSize, InputDifferenceSize, OutputDifferenceMaxSize, InputDifferenceMaxSize>;
435 MatrixOf<Scalar, InputDifferenceSize, OutputDifferenceSize, InputDifferenceMaxSize, OutputDifferenceMaxSize>;
440 template <
int Po
ints = InputDifferenceSize,
int MaxPo
ints = InputDifferenceMaxSize>
441 using OutputSamples = MatrixOf<Scalar, OutputSize, Points, OutputMaxSize, MaxPoints>;
444 template <
int Po
ints = InputDifferenceSize,
int MaxPo
ints = InputDifferenceMaxSize>
445 using OutputDeviations = MatrixOf<Scalar, OutputDifferenceSize, Points, OutputDifferenceMaxSize, MaxPoints>;
455 OutputVector<Scalar> mean;
457 CrossMatrix whitened;
458 OutputMatrix unexplained;
465 Transform(F & f, OutputSpace
const & outputSpace, Args
const &... args)
467 , m_outputSpace{outputSpace}
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();
477 if constexpr (OutputSize == Eigen::Dynamic &&
requires(Index n) { m_outputSpace.resize(n); })
479 Index
const outputSize = OutputVector<Scalar>{valueOf(call<Scalar>(m_f, mean()))}.size();
482 m_outputSpace.resize(outputSize);
485 else if constexpr (OutputSize == Eigen::Dynamic)
489 OutputVector<Scalar>{valueOf(call<Scalar>(m_f, mean()))}.size() ==
valueDimension(m_outputSpace)
490 &&
"the function's output does not fit the output space"
496 [[nodiscard]] OutputSpace
const &
499 return m_outputSpace;
511 inputDifferenceSize()
const
513 return m_inputDifferenceSize;
518 outputDifferenceSize()
const
524 [[nodiscard]] InputVector<Scalar>
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();
535 [[nodiscard]] InputMatrix
536 covarianceLower()
const
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);
555 template <
typename A = roots::DefaultAxes>
556 [[nodiscard]] InputMatrix
557 root(A
const & axes = {})
const
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(); })
566 return internal::rootValue(internal::rootFromFactor(entry, input.root()));
570 return internal::rootValue(entry.root(covarianceLowerOf(input)));
574 ArgInfo::crossPosition < ArgInfo::count ? I == ArgInfo::crossPosition : ArgInfo::ordinal[I] == 0;
577 result.block(m_differenceOffsets[I], m_differenceOffsets[I], d, d) = take(axes);
581 result.block(m_differenceOffsets[I], m_differenceOffsets[I], d, d) = take(
roots::DefaultAxes{});
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
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(
601 internal::middleRowsOf<Values>(x, m_offsets[I], input.valueDimension()),
602 internal::middleRowsOf<Differences>(d, m_differenceOffsets[I], input.differenceDimension())
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
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());
623 internal::middleRowsOf<Differences>(d, m_differenceOffsets[I], input.differenceDimension());
624 if constexpr (HasPlus<InputSpace>)
628 internal::middleRowsOf<Values>(x, m_offsets[I], input.valueDimension()),
643 template <
typename T>
644 [[nodiscard]] OutputVector<T>
645 evaluate(InputVector<T>
const & x)
const
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");
654 static constexpr bool EvaluatesBatches =
requires {
requires std::remove_cvref_t<F>::EvaluatesBatches; };
657 template <
typename T,
int Po
ints,
int MaxPo
ints>
658 using InputSamples = MatrixOf<T, InputSize, Points, InputMaxSize, MaxPoints>;
666 template <
typename Outputs,
typename Po
intAt>
668 evaluateEach(Index count, PointAt
const & pointAt, Outputs & outputs)
const
670 using T =
typename Outputs::Scalar;
672 if constexpr (EvaluatesBatches)
674 auto x = InputSamples<T, Outputs::ColsAtCompileTime, Outputs::MaxColsAtCompileTime>{};
675 x.resize(m_inputSize, count);
676 for (Index j = 0; j < count; ++j)
678 x.col(j) = pointAt(j);
680 [&]<std::size_t... I>(std::index_sequence<I...>) {
681 m_f.evaluateBatch(outputs, argumentColumns<I>(x)...);
682 }(std::index_sequence_for<Args...>{});
686 for (Index j = 0; j < count; ++j)
688 outputs.col(j) = evaluate<T>(pointAt(j));
699 template <
typename Outputs,
typename Jacobians,
typename Po
intAt>
701 evaluateEachWithAffineJacobians(
703 PointAt
const & pointAt,
705 Jacobians & jacobians
707 requires EvaluatesBatches
710 auto x = InputSamples<Scalar, Outputs::ColsAtCompileTime, Outputs::MaxColsAtCompileTime>{};
711 x.resize(m_inputSize, count);
712 for (Index j = 0; j < count; ++j)
714 x.col(j) = pointAt(j);
716 [&]<std::size_t... I>(std::index_sequence<I...>) {
717 m_f.evaluateBatchWithAffineJacobians(outputs, jacobians, argumentColumns<I>(x)...);
718 }(std::index_sequence_for<Args...>{});
724 template <
typename T>
725 [[nodiscard]]
decltype(
auto)
726 returned(InputVector<T>
const & x)
const
728 return call<T>(m_f, x);
732 template <
typename T,
typename G>
733 [[nodiscard]]
decltype(
auto)
734 call(G & g, InputVector<T>
const & x)
const
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...>{});
742 template <
typename Fn>
744 forEachUncertain(Fn && fn)
const
746 [&]<std::size_t... I>(std::index_sequence<I...>) {
747 (visitIfUncertain<I>(fn), ...);
748 }(std::index_sequence_for<Args...>{});
753 offset(std::size_t i)
const
760 differenceOffset(std::size_t i)
const
762 return m_differenceOffsets[i];
774 template <
typename MethodResult>
776 assemble(MethodResult
const & result)
const
778 constexpr bool Root = std::is_same_v<MethodResult, RootResult>;
779 Index
const m = outputDifferenceSize();
780 if constexpr (ArgInfo::crossPosition == ArgInfo::count)
782 OutputMatrix cov = [&] {
785 OutputMatrix parts = result.unexplained;
786 parts.noalias() += multiply(result.whitened.transpose(), result.whitened);
791 CrossMatrix
const cross = multiply(covarianceLower(), result.jacobian.transpose());
792 return OutputMatrix{multiply(result.jacobian, cross)};
795 cov = (Scalar(0.5) * (cov + cov.transpose())).eval();
796 return GaussianParams<OutputSpace, Scalar>{m_outputSpace, result.mean, std::move(cov)};
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();
810 OutputMatrix inputs = OutputMatrix::Zero(m, m);
811 forEachUncertain([&]<std::size_t I>(
auto const & input) {
812 if constexpr (I != position)
814 using Input = std::remove_cvref_t<
decltype(input)>;
815 using Shared = MatrixOf<
817 Input::DifferenceDimensionAtCompileTime,
818 OutputDifferenceSize,
819 Input::MaxDifferenceDimensionAtCompileTime,
820 OutputDifferenceMaxSize>;
821 Index
const at = m_differenceOffsets[I];
822 Index
const d = input.differenceDimension();
825 Shared
const block = result.whitened.middleRows(
at, d);
826 inputs.noalias() += multiply(
block.transpose(),
block);
831 multiply(symmetricCov(input), result.jacobian.middleCols(
at, d).transpose());
832 inputs.noalias() += multiply(result.jacobian.middleCols(
at, d), shared);
839 typename WithCrossResult::CrossMatrix whitened = result.whitened.middleRows(offset, rows);
840 OutputMatrix explained = multiply(whitened.transpose(), whitened);
842 return WithCrossResult{
845 typename WithCrossResult::RootMatrix{result.root.block(offset, offset, rows, rows)},
847 std::move(explained),
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);
859 return WithCrossResult{
864 std::move(explained),
866 OutputMatrix::Zero(m, m)
875 template <std::
size_t I>
876 using ArgAt = std::tuple_element_t<I, std::tuple<Args...>>;
879 template <
typename Input>
880 [[nodiscard]]
static auto
881 symmetricCov(Input
const & input)
883 using Block = Eigen::Matrix<
885 Input::DifferenceDimensionAtCompileTime,
886 Input::DifferenceDimensionAtCompileTime,
888 Input::MaxDifferenceDimensionAtCompileTime,
889 Input::MaxDifferenceDimensionAtCompileTime>;
890 Block cov = covarianceLowerOf(input);
891 cov.template triangularView<Eigen::StrictlyUpper>() = cov.transpose();
896 template <std::
size_t I,
typename Fn>
898 visitIfUncertain(Fn & fn)
const
900 if constexpr (ArgInfo::uncertain[I])
902 fn.template operator()<I>(argInput(std::get<I>(m_args)));
907 template <
typename T, std::
size_t I>
908 [[nodiscard]]
decltype(
auto)
909 argument(InputVector<T>
const & x)
const
911 auto const & arg = std::get<I>(m_args);
912 if constexpr (ArgInfo::uncertain[I])
914 using Input =
typename ArgTraits<ArgAt<I>>::Input;
915 using Block = Eigen::Matrix<
917 Input::ValueDimensionAtCompileTime,
920 Input::MaxValueDimensionAtCompileTime,
922 return Block{x.segment(m_offsets[I], argInput(arg).
valueDimension())};
933 template <std::
size_t I,
typename X>
934 [[nodiscard]]
decltype(
auto)
935 argumentColumns(X
const & x)
const
937 auto const & arg = std::get<I>(m_args);
938 if constexpr (ArgInfo::uncertain[I])
940 using Input =
typename ArgTraits<ArgAt<I>>::Input;
941 return internal::middleRowsOf<Input::ValueDimensionAtCompileTime>(
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 &>())...
967 template <
typename T>
968 using Returned =
decltype(returnedAt<T>(std::index_sequence_for<Args...>{}));
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;
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...
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
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