OpenKalman
redux.hpp
Go to the documentation of this file.
1 /* This file is part of OpenKalman, a header-only C++ library for
2  * Kalman filters and other recursive filters.
3  *
4  * Copyright (c) 2023 Christopher Lee Ogden <ogden@gatech.edu>
5  *
6  * This Source Code Form is subject to the terms of the Mozilla Public
7  * License, v. 2.0. If a copy of the MPL was not distributed with this
8  * file, You can obtain one at https://mozilla.org/MPL/2.0/.
9  */
10 
16 #ifndef OPENKALMAN_EIGEN_TRAITS_FUNCTORS_REDUX_HPP
17 #define OPENKALMAN_EIGEN_TRAITS_FUNCTORS_REDUX_HPP
18 
19 #include <type_traits>
20 
21 namespace OpenKalman::Eigen3
22 {
23 
24  // Default, if MemberOp is not handled below.
25  template<typename MemberOp, std::size_t direction>
26  struct ReduxTraits
27  {
28  template<typename C, typename Factor, typename Dim>
29  static constexpr auto get_constant(const C&, const Factor& factor, const Dim& dim)
30  {
31  return std::monostate{};
32  }
33 
34  template<bool at_least_square, typename C, typename Factor, typename Dim>
35  static constexpr auto get_constant_diagonal(const C&, const Factor& factor, const Dim& dim)
36  {
37  return std::monostate{};
38  }
39  };
40 
41 
42 #ifndef __cpp_concepts
43  namespace detail
44  {
45  template<typename C, typename = void>
46  struct const_is_zero : std::false_type {};
47 
48  template<typename C>
49  struct const_is_zero<C, std::enable_if_t<values::to_value_type(C{}) == 0>> : std::true_type {};
50  }
51 #endif
52 
53 
55  // Norms //
57 
58 #if EIGEN_VERSION_AT_LEAST(3,4,0)
59  template<int p, typename ResultType, typename Scalar, std::size_t direction>
60  struct ReduxTraits<Eigen::internal::member_lpnorm<p, ResultType, Scalar>, direction>
61 #else
62  template<int p, typename ResultType, std::size_t direction>
63  struct ReduxTraits<Eigen::internal::member_lpnorm<p, ResultType>, direction>
64 #endif
65  {
66  private:
67 
68  struct Op
69  {
70  template<typename X>
71  constexpr Scalar operator()(X x, std::size_t dim) const
72  {
73  auto abs_x = values::abs(x);
74  if constexpr (p == 1) return static_cast<Scalar>(dim) * abs_x;
75  else if constexpr (p == 2) return values::sqrt(static_cast<Scalar>(dim)) * abs_x;
76  else return values::pow(static_cast<Scalar>(dim), static_cast<Scalar>(1)/p) * abs_x;
77  }
78  };
79 
80 
81  struct OpInf
82  {
83  template<typename X>
84  constexpr Scalar operator()(X x) const { return values::abs(x); }
85  };
86 
87 
88  template<bool diag, bool at_least_square, typename C, typename Factor, typename Dim>
89  static constexpr auto get_constant_impl(const C& c, const Factor& factor, const Dim& dim)
90  {
91  if constexpr (p == 0)
92  {
93  // Note: Eigen does not specifically define the l0 norm, so if p==0 the result is infinity.
94  if constexpr (std::numeric_limits<ResultType>::has_infinity) return std::numeric_limits<ResultType>::infinity();
95  else throw std::domain_error {"Domain error in lpnorm<0>: result is infinity"};
96  }
97 #ifdef __cpp_concepts
98  else if constexpr (requires { requires values::to_value_type(C{}) == 0; })
99 #else
100  else if constexpr (detail::const_is_zero<C>::value)
101 #endif
102  {
104  }
105  else if constexpr (not at_least_square)
106  {
107  return std::monostate{};
108  }
109  else if constexpr (p == Eigen::Infinity)
110  {
111  return values::operation(OpInf{}, c);
112  }
113  else if constexpr (diag)
114  {
115  return values::operation(Op{}, c, factor);
116  }
117  else
118  {
119  return values::operation(Op{}, c, values::operation(std::multiplies<std::size_t>{}, factor, dim));
120  }
121  }
122 
123  public:
124 
125  template<typename C, typename Factor, typename Dim>
126  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
127  {
128  return get_constant_impl<false, true>(c, factor, dim);
129  }
130 
131 
132  template<bool at_least_square, typename C, typename Factor, typename Dim>
133  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
134  {
135  return get_constant_impl<true, at_least_square>(c, factor, dim);
136  }
137  };
138 
139 
140 #if EIGEN_VERSION_AT_LEAST(3,4,0)
141  template<typename ResultType, typename Scalar, std::size_t direction>
142  struct ReduxTraits<Eigen::internal::member_stableNorm<ResultType, Scalar>, direction>
143  : ReduxTraits<Eigen::internal::member_lpnorm<2, ResultType, Scalar>, direction> {};
144 #else
145  template<typename ResultType, std::size_t direction>
146  struct ReduxTraits<Eigen::internal::member_stableNorm<ResultType>, direction>
147  : ReduxTraits<Eigen::internal::member_lpnorm<2, ResultType>, direction> {};
148 #endif
149 
150 
151 #if EIGEN_VERSION_AT_LEAST(3,4,0)
152  template<typename ResultType, typename Scalar, std::size_t direction>
153  struct ReduxTraits<Eigen::internal::member_hypotNorm<ResultType, Scalar>, direction>
154  : ReduxTraits<Eigen::internal::member_lpnorm<2, ResultType, Scalar>, direction> {};
155 #else
156  template<typename ResultType, std::size_t direction>
157  struct ReduxTraits<Eigen::internal::member_hypotNorm<ResultType>, direction>
158  : ReduxTraits<Eigen::internal::member_lpnorm<2, ResultType>, direction> {};
159 #endif
160 
161 
162 # if not EIGEN_VERSION_AT_LEAST(3,4,0)
163  template<typename...Args, std::size_t direction>
164  struct ReduxTraits<Eigen::internal::member_squaredNorm<Args...>, direction>
165  {
166  struct Op
167  {
168  template<typename Scalar>
169  constexpr Scalar operator()(Scalar x, std::size_t dim) const
170  {
171  if constexpr (values::complex<Scalar>)
172  {
173  auto r = values::real(x);
174  auto i = values::imag(x);
175  if constexpr (constant_diagonal_matrix<XprType>) return r * r + i * i;
176  else return dim * (r * r + i * i);
177  }
178  else return dim * x * x;
179  }
180  };
181 
182 
183  template<typename C, typename Factor, typename Dim>
184  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
185  {
186 #ifdef __cpp_concepts
187  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
188 #else
189  if constexpr (detail::const_is_zero<C>::value)
190 #endif
192  else
193  return values::operation(Op{}, c, values::operation(std::multiplies<std::size_t>{}, factor, dim));
194  }
195 
196 
197  template<bool at_least_square, typename C, typename Factor>
198  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim&)
199  {
200 #ifdef __cpp_concepts
201  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
202 #else
203  if constexpr (detail::const_is_zero<C>::value)
204 #endif
206  else if constexpr (at_least_square)
207  return values::operation(Op{}, c, factor);
208  else
209  return std::monostate{};
210  }
211  };
212 
213 
214  template<typename...Args, std::size_t direction>
215  struct ReduxTraits<Eigen::internal::member_norm<Args...>, direction>
216  {
217  struct Op
218  {
219  template<typename Scalar>
220  constexpr Scalar operator()(Scalar x, std::size_t dim) const
221  {
222  if constexpr (values::complex<Scalar>)
223  {
224  auto r = values::real(x);
225  auto i = values::imag(x);
226  if constexpr (constant_diagonal_matrix<XprType>) return r * r + i * i;
227  return values::sqrt(dim * (r * r + i * i));
228  }
229  else
230  {
231  auto arg = values::abs(x);
232  return values::sqrt(static_cast<Scalar>(dim)) * arg;
233  }
234  }
235  };
236 
237  template<typename XprType, typename Factor, typename Dim>
238  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
239  {
240 #ifdef __cpp_concepts
241  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
242 #else
243  if constexpr (detail::const_is_zero<C>::value)
244 #endif
246  else
247  return values::operation(Op{}, c, values::operation(std::multiplies<std::size_t>{}, factor, dim));
248  }
249 
250 
251  template<bool at_least_square, typename C, typename Factor, typename Dim>
252  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
253  {
254  if constexpr (at_least_square)
255  return values::operation(Op{}, c, factor);
256  else
257  return std::monostate{};
258  }
259  };
260 
261 
262  template<typename...Args, std::size_t direction>
263  struct ReduxTraits<Eigen::internal::member_mean<Args...>, direction>
264  {
265  template<typename C, typename Factor, typename Dim>
266  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim&)
267  {
268  return c;
269  }
270 
271 
272  template<bool at_least_square, typename C, typename Factor, typename Dim>
273  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
274  {
275 #ifdef __cpp_concepts
276  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
277 #else
278  if constexpr (detail::const_is_zero<C>::value)
279 #endif
281  else if constexpr (at_least_square)
282  return (c * factor) / dim;
283  else
284  return std::monostate{};
285  }
286  };
287 # endif
288 
289 
291  // sum //
293 
294  template<typename T, typename Scalar, std::size_t direction>
295  struct ReduxTraits<Eigen::internal::member_redux<std::plus<T>, Scalar>, direction>
296  {
297  template<typename C, typename Factor, typename Dim>
298  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
299  {
300  #ifdef __cpp_concepts
301  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
302  #else
303  if constexpr (detail::const_is_zero<C>::value)
304  #endif
306  else
307  return values::operation(std::multiplies<Scalar>{}, c, values::operation(std::multiplies<Scalar>{}, factor, dim));
308  }
309 
310 
311  template<bool at_least_square, typename C, typename Factor, typename Dim>
312  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
313  {
314 #ifdef __cpp_concepts
315  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
316 #else
317  if constexpr (detail::const_is_zero<C>::value)
318 #endif
320  else if constexpr (at_least_square)
321  return values::operation(std::multiplies<Scalar>{}, c, factor);
322  else
323  return std::monostate{};
324  }
325  };
326 
327 
328 #if EIGEN_VERSION_AT_LEAST(3,4,0)
329  template<typename ResultType, typename Scalar, std::size_t direction>
330  struct ReduxTraits<Eigen::internal::member_sum<ResultType, Scalar>, direction>
331 #else
332  template<typename ResultType, std::size_t direction>
333  struct ReduxTraits<Eigen::internal::member_sum<ResultType>, direction>
334 #endif
335  : ReduxTraits<Eigen::internal::member_redux<std::plus<Scalar>, Scalar>, direction> {};
336 
337 
338  template<typename LhsScalar, typename RhsScalar, typename Scalar, std::size_t direction>
339  struct ReduxTraits<Eigen::internal::member_redux<Eigen::internal::scalar_sum_op<LhsScalar, RhsScalar>, Scalar>, direction>
340  : ReduxTraits<Eigen::internal::member_redux<std::plus<Scalar>, Scalar>, direction> {};
341 
342 
344  // min //
346 
347 #if EIGEN_VERSION_AT_LEAST(3,4,0)
348  template<typename ResultType, typename Scalar, std::size_t direction>
349  struct ReduxTraits<Eigen::internal::member_minCoeff<ResultType, Scalar>, direction>
350 #else
351  template<typename ResultType, std::size_t direction>
352  struct ReduxTraits<Eigen::internal::member_minCoeff<ResultType>, direction>
353 #endif
354  {
355  struct Op
356  {
357  template<typename X, typename Dim>
358  constexpr auto operator()(X x, Dim dim) const
359  {
360  if (dim > 1) return std::min<ResultType>(x, 0);
361  else return x;
362  }
363  };
364 
365 
366  template<typename C, typename Factor, typename Dim>
367  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
368  {
369  return c;
370  }
371 
372 
373  template<bool at_least_square, typename C, typename Factor, typename Dim>
374  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
375  {
376  if constexpr (at_least_square)
377  {
378  return values::operation(Op{}, c, dim);
379  }
380  else if constexpr (values::fixed<C>)
381  {
382  if constexpr (C::value < 0) return std::monostate{};
384  }
385  else return std::monostate{};
386  }
387  };
388 
389 
390  template<typename LhsScalar, typename RhsScalar, typename Scalar, std::size_t direction>
391  struct ReduxTraits<Eigen::internal::member_redux<Eigen::internal::scalar_min_op<LhsScalar, RhsScalar>, Scalar>, direction>
392 #if EIGEN_VERSION_AT_LEAST(3,4,0)
393  : ReduxTraits<Eigen::internal::member_minCoeff<Scalar, Scalar>, direction> {};
394 #else
396 #endif
397 
398 
400  // max //
402 
403 #if EIGEN_VERSION_AT_LEAST(3,4,0)
404  template<typename ResultType, typename Scalar, std::size_t direction>
405  struct ReduxTraits<Eigen::internal::member_maxCoeff<ResultType, Scalar>, direction>
406 #else
407  template<typename ResultType, std::size_t direction>
408  struct ReduxTraits<Eigen::internal::member_maxCoeff<ResultType>, direction>
409 #endif
410  {
411  struct Op
412  {
413  template<typename X, typename Dim>
414  constexpr auto operator()(X x, Dim dim) const
415  {
416  if (dim > 1) return std::max<ResultType>(x, 0);
417  else return x;
418  }
419  };
420 
421 
422  template<typename C, typename Factor, typename Dim>
423  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
424  {
425  return c;
426  }
427 
428 
429  template<bool at_least_square, typename C, typename Factor, typename Dim>
430  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
431  {
432  if constexpr (at_least_square)
433  {
434  return values::operation(Op{}, c, dim);
435  }
436  else if constexpr (values::fixed<C>)
437  {
438  if constexpr (C::value > 0) return std::monostate{};
440  }
441  else return std::monostate{};
442  }
443  };
444 
445 
446  template<typename LhsScalar, typename RhsScalar, typename Scalar, std::size_t direction>
447  struct ReduxTraits<Eigen::internal::member_redux<Eigen::internal::scalar_max_op<LhsScalar, RhsScalar>, Scalar>, direction>
448 #if EIGEN_VERSION_AT_LEAST(3,4,0)
449  : ReduxTraits<Eigen::internal::member_maxCoeff<Scalar, Scalar>, direction> {};
450 #else
452 #endif
453 
454 
456  // and //
458 
459 #if EIGEN_VERSION_AT_LEAST(3,4,0)
460  template<typename ResultType, typename Scalar, std::size_t direction>
461  struct ReduxTraits<Eigen::internal::member_all<ResultType, Scalar>, direction>
462 #else
463  template<typename ResultType, std::size_t direction>
464  struct ReduxTraits<Eigen::internal::member_all<ResultType>, direction>
465 #endif
466  {
467  struct Op
468  {
469  template<typename X>
470  constexpr bool operator()(X x) const { return static_cast<bool>(x); }
471  };
472 
473 
474  template<typename C, typename Factor, typename Dim>
475  static constexpr auto get_constant(const C& c, const Factor&, const Dim&)
476  {
477  return values::operation(Op{}, c);
478  }
479 
480 
481  template<bool at_least_square, typename C, typename Factor, typename Dim>
482  static constexpr auto get_constant_diagonal(const C&, const Factor&, const Dim&)
483  {
484  return std::false_type{};
485  }
486  };
487 
488 
489  template<typename T, typename Scalar, std::size_t direction>
490  struct ReduxTraits<Eigen::internal::member_redux<std::logical_and<T>, Scalar>, direction>
491 #if EIGEN_VERSION_AT_LEAST(3,4,0)
492  : ReduxTraits<Eigen::internal::member_all<bool, Scalar>, direction> {};
493 #else
495 #endif
496 
497 
498  template<typename Scalar, std::size_t direction>
499  struct ReduxTraits<Eigen::internal::member_redux<Eigen::internal::scalar_boolean_and_op, Scalar>, direction>
500 #if EIGEN_VERSION_AT_LEAST(3,4,0)
501  : ReduxTraits<Eigen::internal::member_all<bool, Scalar>, direction> {};
502 #else
504 #endif
505 
506 
508  // or //
510 
511 #if EIGEN_VERSION_AT_LEAST(3,4,0)
512  template<typename ResultType, typename Scalar, std::size_t direction>
513  struct ReduxTraits<Eigen::internal::member_any<ResultType, Scalar>, direction>
514 #else
515  template<typename ResultType, std::size_t direction>
516  struct ReduxTraits<Eigen::internal::member_any<ResultType>, direction>
517 #endif
518  {
519  struct Op
520  {
521  template<typename X>
522  constexpr bool operator()(X x) const { return static_cast<bool>(x); }
523  };
524 
525 
526  template<typename C, typename Factor, typename Dim>
527  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
528  {
529  return values::operation(Op{}, c);
530  }
531 
532 
533  template<bool at_least_square, typename C, typename Factor, typename Dim>
534  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
535  {
536  if constexpr (at_least_square)
537  return values::operation(Op{}, c);
538  else
539  return std::monostate{};
540  }
541  };
542 
543 
544  template<typename T, typename Scalar, std::size_t direction>
545  struct ReduxTraits<Eigen::internal::member_redux<std::logical_or<T>, Scalar>, direction>
546 #if EIGEN_VERSION_AT_LEAST(3,4,0)
547  : ReduxTraits<Eigen::internal::member_any<bool, Scalar>, direction> {};
548 #else
550 #endif
551 
552 
553  template<typename Scalar, std::size_t direction>
554  struct ReduxTraits<Eigen::internal::member_redux<Eigen::internal::scalar_boolean_or_op, Scalar>, direction>
555 #if EIGEN_VERSION_AT_LEAST(3,4,0)
556  : ReduxTraits<Eigen::internal::member_any<bool, Scalar>, direction> {};
557 #else
559 #endif
560 
561 
563  // count //
565 
566 #if EIGEN_VERSION_AT_LEAST(3,4,0)
567  template<typename ResultType, typename Scalar, std::size_t direction>
568  struct ReduxTraits<Eigen::internal::member_count<ResultType, Scalar>, direction>
569 #else
570  template<typename ResultType, std::size_t direction>
571  struct ReduxTraits<Eigen::internal::member_count<ResultType>, direction>
572 #endif
573  {
574  struct Op
575  {
576  template<typename X>
577  constexpr Eigen::Index operator()(X x, std::size_t dim) const
578  {
579  return static_cast<bool>(x) ? static_cast<Eigen::Index>(dim) : Eigen::Index{0};
580  }
581  };
582 
583 
584  template<typename C, typename Factor, typename Dim>
585  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
586  {
587  return values::operation(Op{}, c, values::operation(std::multiplies<std::size_t>{}, dim, factor));
588  }
589 
590 
591  template<bool at_least_square, typename C, typename Factor, typename Dim>
592  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim&)
593  {
594  if constexpr (at_least_square)
595  return values::operation(Op{}, c, factor);
596  else
597  return std::monostate{};
598  }
599  };
600 
601 
603  // product //
605 
606 #if EIGEN_VERSION_AT_LEAST(3,4,0)
607  template<typename ResultType, typename Scalar, std::size_t direction>
608  struct ReduxTraits<Eigen::internal::member_prod<ResultType, Scalar>, direction>
609 #else
610  template<typename ResultType, std::size_t direction>
611  struct ReduxTraits<Eigen::internal::member_prod<ResultType>, direction>
612 #endif
613  {
614  struct Op
615  {
616  template<typename X>
617  constexpr Scalar operator()(X x, std::size_t dim) const { return values::pow(x, dim); }
618  };
619 
620 
621  template<typename C, typename Factor, typename Dim>
622  static constexpr auto get_constant(const C& c, const Factor& factor, const Dim& dim)
623  {
624 #ifdef __cpp_concepts
625  if constexpr (requires { requires values::to_value_type(C{}) == 0; })
626 #else
627  if constexpr (detail::const_is_zero<C>::value)
628 #endif
630  else
631  return values::operation(Op{}, c, values::operation(std::multiplies<std::size_t>{}, factor, dim));
632  }
633 
634 
635  template<bool at_least_square, typename C, typename Factor, typename Dim>
636  static constexpr auto get_constant_diagonal(const C& c, const Factor& factor, const Dim& dim)
637  {
639  }
640  };
641 
642 
643  template<typename LhsScalar, typename RhsScalar, typename Scalar, std::size_t direction>
644  struct ReduxTraits<Eigen::internal::member_redux<Eigen::internal::scalar_product_op<LhsScalar, RhsScalar>, Scalar>, direction>
645 #if EIGEN_VERSION_AT_LEAST(3,4,0)
646  : ReduxTraits<Eigen::internal::member_prod<Scalar, Scalar>, direction> {};
647 #else
649 #endif
650 
651 
652  template<typename T, typename Scalar, std::size_t direction>
653  struct ReduxTraits<Eigen::internal::member_redux<std::multiplies<T>, Scalar>, direction>
654 #if EIGEN_VERSION_AT_LEAST(3,4,0)
655  : ReduxTraits<Eigen::internal::member_prod<Scalar, Scalar>, direction> {};
656 #else
658 #endif
659 
660 }
661 
662 #endif
Definition: fixed_value.hpp:41
decltype(auto) constexpr to_value_type(Arg &&arg)
Convert, if necessary, a fixed or dynamic value to its underlying base type.
Definition: to_value_type.hpp:28
constexpr bool value
T is a fixed or dynamic value that is reducible to a number.
Definition: value.hpp:45
constexpr auto imag(const Arg &arg)
A constexpr function to obtain the imaginary part of a (complex) number.
Definition: imag.hpp:40
constexpr auto sqrt(const Arg &arg)
A constexpr alternative to std::sqrt.
Definition: sqrt.hpp:46
Definition: eigen-forward-declarations.hpp:22
Definition: redux.hpp:26
constexpr auto real(const Arg &arg)
A constexpr function to obtain the real part of a (complex) number.
Definition: real.hpp:40
constexpr auto abs(const Arg &arg)
A constexpr alternative to std::abs.
Definition: abs.hpp:38
constexpr auto operation(Operation &&op, Args &&...args)
A potentially constant-evaluated operation involving some number of values.
Definition: operation.hpp:98