0.4.1
Loading...
Searching...
No Matches
Math.hpp
Go to the documentation of this file.
1// This file is part of INSTINCT, the INS Toolkit for Integrated
2// Navigation Concepts and Training by the Institute of Navigation of
3// the University of Stuttgart, Germany.
4//
5// This Source Code Form is subject to the terms of the Mozilla Public
6// License, v. 2.0. If a copy of the MPL was not distributed with this
7// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9/// @file Math.hpp
10/// @brief Simple Math functions
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @author N. Stahl (HiWi: Elliptical integral)
13/// @date 2023-07-04
14
15#pragma once
16
17#include <concepts>
18#include <cstdint>
19#include <optional>
20#include <type_traits>
21#include <Eigen/Core>
22#include <Eigen/Dense>
23#include <gcem.hpp>
24#include <fmt/format.h>
25
26#include "util/Assert.h"
27
28namespace NAV::math
29{
30
31/// @brief Calculates the factorial of an unsigned integer
32/// @param[in] n Unsigned integer
33/// @return The factorial of 'n'
34uint64_t factorial(uint64_t n);
35
36/// @brief Round the number to the specified amount of digits
37/// @param[in] value Value to round
38/// @param[in] digits Amount of digits
39/// @return The rounded value
40template<std::floating_point T>
41constexpr T round(const T& value, size_t digits)
42{
43 auto factor = std::pow(10, digits);
44 return std::round(value * factor) / factor;
45}
46
47/// @brief Round the number to the specified amount of significant digits
48/// @param[in] value Value to round
49/// @param[in] digits Amount of digits
50/// @return The rounded value
51template<std::floating_point T>
52constexpr T roundSignificantDigits(T value, size_t digits)
53{
54 if (value == 0.0) { return 0.0; }
55 // LOG_DEBUG("value = {:.13e} --> Round to {} digits", value, digits);
56 auto absVal = gcem::abs(value);
57 auto log10 = static_cast<int32_t>(gcem::log10(absVal));
58 auto exp = log10 + (log10 > 0 || (log10 == 0 && absVal >= 1.0));
59 auto fac = static_cast<T>(digits) - static_cast<T>(exp);
60 // LOG_DEBUG(" log10 = {}, exp = {}, fac = {}", log10, exp, fac);
61 auto factor = static_cast<T>(gcem::pow(10.0, fac));
62 // LOG_DEBUG(" factor = {:.0e} --> value * factor = {}", factor, value * factor);
63 // LOG_DEBUG(" round = {} --> ... / factor = {}", gcem::round(value * factor), gcem::round(value * factor) / factor);
64 return static_cast<T>(gcem::round(value * factor) / factor);
65}
66
67/// @brief Interprets the input integer with certain amount of Bits as Output type. Takes care of sign extension
68/// @tparam Out Output type
69/// @tparam Bits Size of the input data
70/// @tparam In Input data type (needs to be bigger than the amount of Bits)
71/// @param[in] in Number as two's complement, with the sign bit (+ or -) occupying the MSB
72/// @return Output type
73template<std::integral Out, size_t Bits, std::integral In>
74constexpr Out interpretAs(In in)
75{
76 static_assert(Bits < sizeof(In) * 8);
77 static_assert(Bits < sizeof(Out) * 8);
78
79 constexpr size_t N = sizeof(Out) * 8 - Bits;
80 return static_cast<Out>(static_cast<Out>((in & static_cast<In>(gcem::pow(2, Bits) - 1)) << N) >> N);
81}
82
83/// @brief Calculates the skew symmetric matrix of the given vector.
84/// This is needed to perform the cross product with a scalar product operation
85/// @tparam Derived Derived Eigen Type
86/// @param[in] a The vector
87/// @return Skew symmetric matrix
88/// @note See Groves (2013) equation (2.50)
89template<typename Derived>
90Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetricMatrix(const Eigen::MatrixBase<Derived>& a)
91{
92 INS_ASSERT_USER_ERROR(a.cols() == 1, "Given Eigen Object must be a vector");
93 INS_ASSERT_USER_ERROR(a.rows() == 3, "Given Vector must have 3 Rows");
94
95 Eigen::Matrix<typename Derived::Scalar, 3, 3> skewMat;
96 skewMat << 0, -a(2), a(1),
97 a(2), 0, -a(0),
98 -a(1), a(0), 0;
99
100 return skewMat;
101}
102
103/// @brief Calculates the square of a skew symmetric matrix of the given vector.
104/// @tparam Derived Derived Eigen Type
105/// @param[in] a The vector
106/// @return Square of skew symmetric matrix
107template<typename Derived>
108Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetricMatrixSquared(const Eigen::MatrixBase<Derived>& a)
109{
110 INS_ASSERT_USER_ERROR(a.cols() == 1, "Given Eigen Object must be a vector");
111 INS_ASSERT_USER_ERROR(a.rows() == 3, "Given Vector must have 3 Rows");
112
113 Eigen::Matrix<typename Derived::Scalar, 3, 3> skewMat2;
114 skewMat2 << std::pow(a(2), 2) + std::pow(a(1), 2), a(0) * a(1), a(0) * a(2),
115 a(0) * a(1), std::pow(a(2), 2) + std::pow(a(0), 2), a(1) * a(2),
116 a(0) * a(2), a(1) * a(2), std::pow(a(0), 2) + std::pow(a(1), 2);
117
118 return skewMat2;
119}
120
121/// @brief Calculates the secant of a value (sec(x) = csc(pi/2 - x) = 1 / cos(x))
122template<std::floating_point T>
123T sec(const T& x)
124{
125 return 1.0 / std::cos(x);
126}
127
128/// @brief Calculates the cosecant of a value (csc(x) = sec(pi/2 - x) = 1 / sin(x))
129template<std::floating_point T>
130T csc(const T& x)
131{
132 return 1.0 / std::sin(x);
133}
134
135/// @brief Returns the sign of the given value
136/// @param[in] val Value to get the sign from
137/// @return Sign of the given value
138template<typename T>
139int sgn(const T& val)
140{
141 return (T(0) < val) - (val < T(0));
142}
143
144/// @brief Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ
145/// @param[in] X Matrix
146/// @param[in] order The order of the Taylor polynom to calculate
147/// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.34, p. 98
148template<typename Derived>
149typename Derived::PlainObject expm(const Eigen::MatrixBase<Derived>& X, size_t order)
150{
151 INS_ASSERT_USER_ERROR(X.rows() == X.cols(), "Matrix exponential calculation only possible for n x n matrices");
152
153 typename Derived::PlainObject e_X;
154
155 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic)
156 {
157 e_X = Eigen::MatrixBase<Derived>::Identity(X.rows(), X.cols());
158 }
159 else
160 {
161 e_X = Eigen::MatrixBase<Derived>::Identity();
162 }
163 typename Derived::PlainObject Xpower = X;
164 for (size_t i = 1; i <= order; i++)
165 {
166 e_X += Xpower / static_cast<double>(math::factorial(i));
167
168 if (i < order)
169 {
170 Xpower *= X;
171 }
172 }
173
174 return e_X;
175}
176
177/// @brief Find (L^T D L)-decomposition of Q-matrix via outer product method
178/// @param[in] Qmatrix Symmetric positive definite matrix to be factored
179/// @return L - Factor matrix (strict lower triangular)
180/// @return D - Vector with entries of the diagonal matrix
181/// @note See \cite deJonge1996 de Jonge 1996, Algorithm FMFAC5
182/// @attention Consider using NAV::math::LtDLdecomp_choleskyFact() because it is faster by up to a factor 10
183template<typename Derived>
184std::optional<std::pair<Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>,
185 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>>>
186 LtDLdecomp_outerProduct(const Eigen::MatrixBase<Derived>& Qmatrix)
187{
188 using Eigen::seq;
189
190 auto n = Qmatrix.rows();
191 Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> Q = Qmatrix.template triangularView<Eigen::Lower>();
192 Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> L;
193 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> D;
194
195 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic)
196 {
197 L = Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(n, n);
198 D.setZero(n);
199 }
200 else
201 {
202 L = Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>::Zero();
203 D.setZero();
204 }
205
206 for (Eigen::Index i = n - 1; i >= 0; i--)
207 {
208 D(i) = Q(i, i);
209 if (Q(i, i) <= 0.0) { return {}; }
210 L(i, seq(0, i)) = Q(i, seq(0, i)) / std::sqrt(Q(i, i));
211
212 for (Eigen::Index j = 0; j <= i - 1; j++)
213 {
214 Q(j, seq(0, j)) -= L(i, seq(0, j)) * L(i, j);
215 }
216 L(i, seq(0, i)) /= L(i, i);
217 }
218
219 return std::make_pair(L, D);
220}
221
222/// @brief Find (L^T D L)-decomposition of Q-matrix via a backward Cholesky factorization in a bordering method formulation
223/// @param[in] Q Symmetric positive definite matrix to be factored
224/// @return L - Factor matrix (strict lower triangular)
225/// @return D - Vector with entries of the diagonal matrix
226/// @note See \cite deJonge1996 de Jonge 1996, Algorithm FMFAC6
227template<typename Derived>
228std::optional<std::pair<Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>,
229 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>>>
230 LtDLdecomp_choleskyFact(const Eigen::MatrixBase<Derived>& Q)
231{
232 using Eigen::seq;
233
234 auto n = Q.rows();
235 typename Derived::PlainObject L = Q.template triangularView<Eigen::Lower>();
236 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> D;
237 double cmin = 1;
238
239 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) { D.setZero(n); }
240 else { D.setZero(); }
241
242 for (Eigen::Index j = n - 1; j >= 0; j--)
243 {
244 for (Eigen::Index i = n - 1; i >= j + 1; i--)
245 {
246 L(i, j) = (L(i, j) - L(seq(i + 1, n - 1), j).dot(L(seq(i + 1, n - 1), i))) / L(i, i);
247 }
248 double t = L(j, j) - L(seq(j + 1, n - 1), j).dot(L(seq(j + 1, n - 1), j));
249 if (t <= 0.0) { return {}; }
250 double c = t / L(j, j);
251 cmin = std::min(c, cmin);
252 L(j, j) = std::sqrt(t);
253 }
254 for (Eigen::Index i = 0; i < n; i++)
255 {
256 L.row(i).leftCols(i) /= L(i, i);
257 D(i) = std::pow(L(i, i), 2.0);
258 L(i, i) = 1;
259 }
260
261 return std::make_pair(L, D);
262}
263
264/// @brief Calculates the squared norm of the vector and matrix
265///
266/// \anchor eq-squaredNorm \f{equation}{ \label{eq:eq-squaredNorm}
267/// ||\mathbf{\dots}||^2_{\mathbf{Q}} = (\dots)^T \mathbf{Q}^{-1} (\dots)
268/// \f}
269/// @param a Vector
270/// @param Q Covariance matrix of the vector
271/// @return Squared norm
272template<typename DerivedA, typename DerivedQ>
273typename DerivedA::Scalar squaredNormVectorMatrix(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Q)
274{
275 static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1);
276 INS_ASSERT_USER_ERROR(a.cols() == 1, "Parameter 'a' has to be a vector");
277 INS_ASSERT_USER_ERROR(a.rows() == Q.rows(), "Parameter 'a' and 'Q' need to have same size");
278 INS_ASSERT_USER_ERROR(Q.cols() == Q.rows(), "Parameter 'Q' needs to be quadratic");
279
280 return a.transpose() * Q.inverse() * a;
281}
282
283/// @brief Calculates the cumulative distribution function (CDF) of the standard normal distribution
284///
285/// \anchor eq-normalDistCDF \f{equation}{ \label{eq:eq-normalDistCDF}
286/// \Phi(x) = \int\displaylimits_{-\infty}^x \frac{1}{\sqrt{2\pi}} \exp{\left(-\frac{1}{2} v^2\right)} \text{d}v
287/// \f}
288/// which can be expressed with the error function
289/// \anchor eq-normalDistCDF-erf \f{equation}{ \label{eq:eq-normalDistCDF-erf}
290/// \Phi(x) = \frac{1}{2} \left[ 1 + \text{erf}{\left(\frac{x}{\sqrt{2}}\right)} \right]
291/// \f}
292/// Using the property
293/// \anchor eq-erf-minus \f{equation}{ \label{eq:eq-erf-minus}
294/// \text{erf}{\left( -x \right)} = -\text{erf}{\left( x \right)}
295/// \f}
296/// and the complementary error function
297/// \anchor eq-erfc \f{equation}{ \label{eq:eq-erfc}
298/// \text{erfc}{\left( x \right)} = 1 - \text{erf}{\left( x \right)}
299/// \f}
300/// we can simplify equation \eqref{eq-normalDistCDF-erf} to
301/// \anchor eq-normalDistCDF-erfc \f{equation}{ \label{eq:eq-normalDistCDF-erfc}
302/// \begin{aligned}
303/// \Phi(x) &= \frac{1}{2} \left[ 1 - \text{erf}{\left(-\frac{x}{\sqrt{2}}\right)} \right] \\
304/// &= \frac{1}{2} \text{erfc}{\left(-\frac{x}{\sqrt{2}}\right)}
305/// \end{aligned}
306/// \f}
307///
308/// @param value Value to calculate the CDF for
309double normalCDF(double value);
310
311/// @brief Returns the inverse square root of a matrix
312/// @param matrix Matrix to use
313template<typename Derived>
314[[nodiscard]] typename Derived::PlainObject inverseSqrt(const Eigen::MatrixBase<Derived>& matrix)
315{
316 INS_ASSERT_USER_ERROR(matrix.rows() == matrix.cols(), "Only square matrix supported");
317 if constexpr (std::is_floating_point_v<typename Derived::Scalar>)
318 {
319 return matrix.inverse().sqrt(); // Eigen::SelfAdjointEigenSolver<Eigen::MatrixX<T>>{ covMatrix }.operatorInverseSqrt();
320 }
321 else // Eigen gets problems with ceres::Jet in the .sqrt() function
322 {
323 Eigen::JacobiSVD<Eigen::MatrixX<typename Derived::Scalar>> svd(matrix.inverse(), Eigen::ComputeFullV);
324 typename Derived::PlainObject sqrtInverse = svd.matrixV() * svd.singularValues().cwiseSqrt().asDiagonal() * svd.matrixV().transpose();
325 INS_ASSERT_USER_ERROR(!sqrtInverse.hasNaN(), "The matrix is not invertible");
326 return sqrtInverse;
327 }
328}
329
330/// @brief Change the sign of x according to the value of y
331/// @param[in] x input value
332/// @param[in] y input value
333/// @return -x or +x
334template<typename T>
335T sign(const T& x, const T& y)
336{
337 // similar function 'sign' in fortran
338 if (y >= 0.0)
339 {
340 return fabs(x);
341 }
342 return -1.0 * fabs(x);
343}
344
345/// @brief Linear interpolation between vectors
346/// @param a Left value
347/// @param b Right value
348/// @param t Multiplier. [0, 1] for interpolation
349/// @return a + t * (b - a)
350template<typename Derived>
351typename Derived::PlainObject lerp(const Eigen::MatrixBase<Derived>& a, const Eigen::MatrixBase<Derived>& b, auto t)
352{
353 return a + t * (b - a);
354}
355
356/// Lerp Search Result
358{
359 size_t l; ///< Lower bound index
360 size_t u; ///< Upper bound index (l + 1)
361 double t; ///< [0, 1] for Interpolation, otherwise Extrapolation
362};
363
364/// @brief Searches the value in the data container
365/// @param[in] data Data container
366/// @param[in] value Value to search
367LerpSearchResult lerpSearch(const auto& data, const auto& value)
368{
369 auto i = static_cast<size_t>(std::distance(data.begin(), std::upper_bound(data.begin(), data.end(), value)));
370 if (i > 0) { i--; }
371 if (i == data.size() - 1) { i--; }
372 const auto& lb = data.at(i);
373 const auto& ub = data.at(i + 1);
374 double t = (value - lb) / (ub - lb);
375
376 return { .l = i, .u = i + 1, .t = t };
377}
378
379/// @brief Bilinear interpolation
380/// @param[in] tx Distance in x component to interpolate [0, 1]
381/// @param[in] ty Distance in y component to interpolate [0, 1]
382/// @param[in] c00 Value for tx = ty = 0
383/// @param[in] c10 Value for tx = 1 and ty = 0
384/// @param[in] c01 Value for tx = 0 and ty = 1
385/// @param[in] c11 Value for tx = ty = 1
386///
387/// c01 ------ c11
388/// | |
389/// | |
390/// | |
391/// c00 ------ c10
392///
393/// @note See https://www.scratchapixel.com/lessons/mathematics-physics-for-computer-graphics/interpolation/bilinear-filtering.html
394auto bilinearInterpolation(const auto& tx, const auto& ty,
395 const auto& c00, const auto& c10,
396 const auto& c01, const auto& c11)
397{
398 auto a = c00 * (1.0 - tx) + c10 * tx;
399 auto b = c01 * (1.0 - tx) + c11 * tx;
400 return a * (1.0 - ty) + b * ty;
401 // Alternative implementation:
402 // return (1.0 - tx) * (1.0 - ty) * c00 + tx * (1.0 - ty) * c10 + (1.0 - tx) * ty * c01 + tx * ty * c11;
403}
404
405/// @brief Calculates the incomplete elliptical integral of the second kind
406/// @param[in] phi Interval bound the integration uses from 0 to phi
407/// @param[in] m Function parameter that is integrated 1-m*sin(t)^2
408/// @return Incomplete elliptical integral of the second kind
409/// @note See http://www2.iap.fr/users/pichon/doc/html_xref/elliptic-es.html
410double calcEllipticalIntegral(double phi, double m);
411
412} // namespace NAV::math
Assertion helpers.
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
Definition Assert.h:21
T sec(const T &x)
Calculates the secant of a value (sec(x) = csc(pi/2 - x) = 1 / cos(x))
Definition Math.hpp:123
LerpSearchResult lerpSearch(const auto &data, const auto &value)
Searches the value in the data container.
Definition Math.hpp:367
T csc(const T &x)
Calculates the cosecant of a value (csc(x) = sec(pi/2 - x) = 1 / sin(x))
Definition Math.hpp:130
DerivedA::Scalar squaredNormVectorMatrix(const Eigen::MatrixBase< DerivedA > &a, const Eigen::MatrixBase< DerivedQ > &Q)
Calculates the squared norm of the vector and matrix.
Definition Math.hpp:273
Derived::PlainObject expm(const Eigen::MatrixBase< Derived > &X, size_t order)
Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ
Definition Math.hpp:149
int sgn(const T &val)
Returns the sign of the given value.
Definition Math.hpp:139
std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > > LtDLdecomp_outerProduct(const Eigen::MatrixBase< Derived > &Qmatrix)
Find (L^T D L)-decomposition of Q-matrix via outer product method.
Definition Math.hpp:186
T sign(const T &x, const T &y)
Change the sign of x according to the value of y.
Definition Math.hpp:335
Eigen::Matrix< typename Derived::Scalar, 3, 3 > skewSymmetricMatrixSquared(const Eigen::MatrixBase< Derived > &a)
Calculates the square of a skew symmetric matrix of the given vector.
Definition Math.hpp:108
auto bilinearInterpolation(const auto &tx, const auto &ty, const auto &c00, const auto &c10, const auto &c01, const auto &c11)
Bilinear interpolation.
Definition Math.hpp:394
constexpr T roundSignificantDigits(T value, size_t digits)
Round the number to the specified amount of significant digits.
Definition Math.hpp:52
Derived::PlainObject lerp(const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &b, auto t)
Linear interpolation between vectors.
Definition Math.hpp:351
double calcEllipticalIntegral(double phi, double m)
Calculates the incomplete elliptical integral of the second kind.
Definition Math.cpp:53
Eigen::Matrix< typename Derived::Scalar, 3, 3 > skewSymmetricMatrix(const Eigen::MatrixBase< Derived > &a)
Calculates the skew symmetric matrix of the given vector. This is needed to perform the cross product...
Definition Math.hpp:90
uint64_t factorial(uint64_t n)
Calculates the factorial of an unsigned integer.
Definition Math.cpp:14
double normalCDF(double value)
Calculates the cumulative distribution function (CDF) of the standard normal distribution.
Definition Math.cpp:48
constexpr T round(const T &value, size_t digits)
Round the number to the specified amount of digits.
Definition Math.hpp:41
constexpr Out interpretAs(In in)
Interprets the input integer with certain amount of Bits as Output type. Takes care of sign extension...
Definition Math.hpp:74
Derived::PlainObject inverseSqrt(const Eigen::MatrixBase< Derived > &matrix)
Returns the inverse square root of a matrix.
Definition Math.hpp:314
std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > > LtDLdecomp_choleskyFact(const Eigen::MatrixBase< Derived > &Q)
Find (L^T D L)-decomposition of Q-matrix via a backward Cholesky factorization in a bordering method ...
Definition Math.hpp:230
Lerp Search Result.
Definition Math.hpp:358
size_t u
Upper bound index (l + 1)
Definition Math.hpp:360
double t
[0, 1] for Interpolation, otherwise Extrapolation
Definition Math.hpp:361
size_t l
Lower bound index.
Definition Math.hpp:359