| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 <unsupported/Eigen/MatrixFunctions> | ||
| 24 | #include <gcem.hpp> | ||
| 25 | #include <fmt/format.h> | ||
| 26 | |||
| 27 | #include "util/Assert.h" | ||
| 28 | |||
| 29 | namespace NAV::math | ||
| 30 | { | ||
| 31 | |||
| 32 | /// @brief Calculates the factorial of an unsigned integer | ||
| 33 | /// @param[in] n Unsigned integer | ||
| 34 | /// @return The factorial of 'n' | ||
| 35 | uint64_t factorial(uint64_t n); | ||
| 36 | |||
| 37 | /// @brief Round the number to the specified amount of digits | ||
| 38 | /// @param[in] value Value to round | ||
| 39 | /// @param[in] digits Amount of digits | ||
| 40 | /// @return The rounded value | ||
| 41 | template<std::floating_point T> | ||
| 42 | 372806 | constexpr T round(const T& value, size_t digits) | |
| 43 | { | ||
| 44 | 372806 | auto factor = std::pow(10, digits); | |
| 45 | 372806 | return std::round(value * factor) / factor; | |
| 46 | } | ||
| 47 | |||
| 48 | /// @brief Round the number to the specified amount of significant digits | ||
| 49 | /// @param[in] value Value to round | ||
| 50 | /// @param[in] digits Amount of digits | ||
| 51 | /// @return The rounded value | ||
| 52 | template<std::floating_point T> | ||
| 53 | 40 | constexpr T roundSignificantDigits(T value, size_t digits) | |
| 54 | { | ||
| 55 |
3/4✓ Branch 0 taken 39 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 39 times.
|
40 | if (value == T(0) || digits == 0) |
| 56 | { | ||
| 57 | 1 | return T(0); | |
| 58 | } | ||
| 59 | |||
| 60 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 39 times.
|
39 | if (!std::isfinite(value)) |
| 61 | { | ||
| 62 | ✗ | return value; // pass through inf / NaN | |
| 63 | } | ||
| 64 | |||
| 65 | // Determine the magnitude (base-10 exponent) | ||
| 66 | 39 | T absValue = std::fabs(value); | |
| 67 | 39 | T exponent = std::floor(std::log10(absValue)); | |
| 68 | |||
| 69 | // Scale so rounding applies to the requested significant digits | ||
| 70 | 39 | T scale = std::pow(T(10), exponent - T(digits - 1)); | |
| 71 | |||
| 72 | // Round and rescale | ||
| 73 | 39 | return std::round(value / scale) * scale; | |
| 74 | } | ||
| 75 | |||
| 76 | /// @brief Interprets the input integer with certain amount of Bits as Output type. Takes care of sign extension | ||
| 77 | /// @tparam Out Output type | ||
| 78 | /// @tparam Bits Size of the input data | ||
| 79 | /// @tparam In Input data type (needs to be bigger than the amount of Bits) | ||
| 80 | /// @param[in] in Number as two's complement, with the sign bit (+ or -) occupying the MSB | ||
| 81 | /// @return Output type | ||
| 82 | template<std::integral Out, size_t Bits, std::integral In> | ||
| 83 | 3099 | constexpr Out interpretAs(In in) | |
| 84 | { | ||
| 85 | static_assert(Bits < sizeof(In) * 8); | ||
| 86 | static_assert(Bits < sizeof(Out) * 8); | ||
| 87 | |||
| 88 | 3099 | constexpr size_t N = sizeof(Out) * 8 - Bits; | |
| 89 | 3099 | return static_cast<Out>(static_cast<Out>((in & static_cast<In>(gcem::pow(2, Bits) - 1)) << N) >> N); | |
| 90 | } | ||
| 91 | |||
| 92 | /// @brief Calculates the skew symmetric matrix of the given vector. | ||
| 93 | /// This is needed to perform the cross product with a scalar product operation | ||
| 94 | /// @tparam Derived Derived Eigen Type | ||
| 95 | /// @param[in] a The vector | ||
| 96 | /// @return Skew symmetric matrix | ||
| 97 | /// @note See Groves (2013) equation (2.50) | ||
| 98 | template<typename Derived> | ||
| 99 | 2984376 | Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetricMatrix(const Eigen::MatrixBase<Derived>& a) | |
| 100 | { | ||
| 101 |
1/2✓ Branch 1 taken 2929168 times.
✗ Branch 2 not taken.
|
2984376 | INS_ASSERT_USER_ERROR(a.cols() == 1, "Given Eigen Object must be a vector"); |
| 102 |
1/2✓ Branch 1 taken 2930054 times.
✗ Branch 2 not taken.
|
2982794 | INS_ASSERT_USER_ERROR(a.rows() == 3, "Given Vector must have 3 Rows"); |
| 103 | |||
| 104 | 2983680 | Eigen::Matrix<typename Derived::Scalar, 3, 3> skewMat; | |
| 105 |
5/10✓ Branch 1 taken 2926706 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2924305 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2927921 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2926811 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2930476 times.
✗ Branch 14 not taken.
|
2976318 | skewMat << 0, -a(2), a(1), |
| 106 |
5/10✓ Branch 1 taken 2927813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2931927 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2931107 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2928626 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2931491 times.
✗ Branch 14 not taken.
|
2984102 | a(2), 0, -a(0), |
| 107 |
5/10✓ Branch 1 taken 2929357 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2931903 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2929801 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2932847 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2932600 times.
✗ Branch 14 not taken.
|
2985117 | -a(1), a(0), 0; |
| 108 | |||
| 109 | 2985090 | return skewMat; | |
| 110 | } | ||
| 111 | |||
| 112 | /// @brief Calculates the square of a skew symmetric matrix of the given vector. | ||
| 113 | /// @tparam Derived Derived Eigen Type | ||
| 114 | /// @param[in] a The vector | ||
| 115 | /// @return Square of skew symmetric matrix | ||
| 116 | template<typename Derived> | ||
| 117 | ✗ | Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetricMatrixSquared(const Eigen::MatrixBase<Derived>& a) | |
| 118 | { | ||
| 119 | ✗ | INS_ASSERT_USER_ERROR(a.cols() == 1, "Given Eigen Object must be a vector"); | |
| 120 | ✗ | INS_ASSERT_USER_ERROR(a.rows() == 3, "Given Vector must have 3 Rows"); | |
| 121 | |||
| 122 | ✗ | Eigen::Matrix<typename Derived::Scalar, 3, 3> skewMat2; | |
| 123 | ✗ | skewMat2 << std::pow(a(2), 2) + std::pow(a(1), 2), a(0) * a(1), a(0) * a(2), | |
| 124 | ✗ | a(0) * a(1), std::pow(a(2), 2) + std::pow(a(0), 2), a(1) * a(2), | |
| 125 | ✗ | a(0) * a(2), a(1) * a(2), std::pow(a(0), 2) + std::pow(a(1), 2); | |
| 126 | |||
| 127 | ✗ | return skewMat2; | |
| 128 | } | ||
| 129 | |||
| 130 | /// @brief Calculates the matrix exponential map of the given vector. | ||
| 131 | /// @tparam Derived Derived Eigen Type | ||
| 132 | /// @param[in] v The vector | ||
| 133 | /// @return The matrix exponential map | ||
| 134 | template<typename Derived> | ||
| 135 | 4 | Eigen::Matrix<typename Derived::Scalar, 3, 3> expMapMatrix(const Eigen::MatrixBase<Derived>& v) | |
| 136 | { | ||
| 137 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | INS_ASSERT_USER_ERROR(v.cols() == 1, "Given Eigen Object must be a vector"); |
| 138 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | INS_ASSERT_USER_ERROR(v.rows() == 3, "Given Vector must have 3 Rows"); |
| 139 | |||
| 140 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
4 | return math::skewSymmetricMatrix(v).exp(); |
| 141 | } | ||
| 142 | |||
| 143 | /// @brief Calculates the quaternionic exponential map of the given vector. | ||
| 144 | /// @tparam Derived Derived Eigen Type | ||
| 145 | /// @param[in] v The vector | ||
| 146 | /// @return The quaternionic exponential map | ||
| 147 | template<typename Derived> | ||
| 148 | 299986 | Eigen::Quaternion<typename Derived::Scalar> expMapQuat(const Eigen::MatrixBase<Derived>& v) | |
| 149 | { | ||
| 150 |
1/2✓ Branch 1 taken 149995 times.
✗ Branch 2 not taken.
|
299986 | INS_ASSERT_USER_ERROR(v.cols() == 1, "Given Eigen Object must be a vector"); |
| 151 |
1/2✓ Branch 1 taken 149995 times.
✗ Branch 2 not taken.
|
299986 | INS_ASSERT_USER_ERROR(v.rows() == 3, "Given Vector must have 3 Rows"); |
| 152 | |||
| 153 |
2/4✓ Branch 1 taken 149995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149995 times.
✗ Branch 5 not taken.
|
299986 | Eigen::Vector3<typename Derived::Scalar> omega = 0.5 * v; |
| 154 |
1/2✓ Branch 1 taken 149995 times.
✗ Branch 2 not taken.
|
299986 | auto omegaNorm = omega.norm(); |
| 155 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 149995 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
299986 | if (omegaNorm < 1e-9) { return Eigen::Quaternion<typename Derived::Scalar>::Identity(); } |
| 156 |
3/6✓ Branch 1 taken 149995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 149995 times.
✗ Branch 8 not taken.
|
299986 | Eigen::Vector3<typename Derived::Scalar> quatVec = omega / omegaNorm * std::sin(omegaNorm); |
| 157 | |||
| 158 |
4/8✓ Branch 1 taken 149995 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149995 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 149995 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 149995 times.
✗ Branch 11 not taken.
|
299986 | return { std::cos(omegaNorm), quatVec.x(), quatVec.y(), quatVec.z() }; |
| 159 | } | ||
| 160 | |||
| 161 | /// @brief Calculates the right Jacobian of SO(3) which relates additive increments in the tangent space to multiplicative increments applied on the right-hand side | ||
| 162 | /// @param[in] phi Vector applied on the right side | ||
| 163 | /// @return Right Jacobian J_r | ||
| 164 | template<typename Derived> | ||
| 165 | ✗ | [[nodiscard]] Eigen::Matrix3<typename Derived::Scalar> J_r(const Eigen::MatrixBase<Derived>& phi) | |
| 166 | { | ||
| 167 | ✗ | INS_ASSERT_USER_ERROR(phi.cols() == 1, "Given Eigen Object must be a vector"); | |
| 168 | ✗ | INS_ASSERT_USER_ERROR(phi.rows() == 3, "Given Vector must have 3 Rows"); | |
| 169 | |||
| 170 | ✗ | auto phiNorm = phi.norm(); | |
| 171 | ✗ | auto phiNorm2 = phiNorm * phiNorm; | |
| 172 | ✗ | auto phiNorm3 = phiNorm2 * phiNorm; | |
| 173 | return Eigen::Matrix3<typename Derived::Scalar>::Identity() | ||
| 174 | ✗ | - (1.0 - std::cos(phiNorm)) / phiNorm2 * skewSymmetricMatrix(phi) | |
| 175 | ✗ | + (phiNorm - std::sin(phiNorm)) / phiNorm3 * skewSymmetricMatrixSquared(phi); | |
| 176 | } | ||
| 177 | |||
| 178 | /// @brief Calculates the secant of a value (sec(x) = csc(pi/2 - x) = 1 / cos(x)) | ||
| 179 | template<std::floating_point T> | ||
| 180 | ✗ | T sec(const T& x) | |
| 181 | { | ||
| 182 | ✗ | return 1.0 / std::cos(x); | |
| 183 | } | ||
| 184 | |||
| 185 | /// @brief Calculates the cosecant of a value (csc(x) = sec(pi/2 - x) = 1 / sin(x)) | ||
| 186 | template<std::floating_point T> | ||
| 187 | 156269 | T csc(const T& x) | |
| 188 | { | ||
| 189 | 156269 | return 1.0 / std::sin(x); | |
| 190 | } | ||
| 191 | |||
| 192 | /// @brief Returns the sign of the given value | ||
| 193 | /// @param[in] val Value to get the sign from | ||
| 194 | /// @return Sign of the given value | ||
| 195 | template<typename T> | ||
| 196 | 1343753 | int sgn(const T& val) | |
| 197 | { | ||
| 198 | 1343753 | return (T(0) < val) - (val < T(0)); | |
| 199 | } | ||
| 200 | |||
| 201 | /// @brief Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ | ||
| 202 | /// @param[in] X Matrix | ||
| 203 | /// @param[in] order The order of the Taylor polynom to calculate | ||
| 204 | /// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.34, p. 98 | ||
| 205 | template<typename Derived> | ||
| 206 | typename Derived::PlainObject expm(const Eigen::MatrixBase<Derived>& X, size_t order) | ||
| 207 | { | ||
| 208 | INS_ASSERT_USER_ERROR(X.rows() == X.cols(), "Matrix exponential calculation only possible for n x n matrices"); | ||
| 209 | |||
| 210 | typename Derived::PlainObject e_X; | ||
| 211 | |||
| 212 | if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) | ||
| 213 | { | ||
| 214 | e_X = Eigen::MatrixBase<Derived>::Identity(X.rows(), X.cols()); | ||
| 215 | } | ||
| 216 | else | ||
| 217 | { | ||
| 218 | e_X = Eigen::MatrixBase<Derived>::Identity(); | ||
| 219 | } | ||
| 220 | typename Derived::PlainObject Xpower = X; | ||
| 221 | for (size_t i = 1; i <= order; i++) | ||
| 222 | { | ||
| 223 | e_X += Xpower / static_cast<double>(math::factorial(i)); | ||
| 224 | |||
| 225 | if (i < order) | ||
| 226 | { | ||
| 227 | Xpower *= X; | ||
| 228 | } | ||
| 229 | } | ||
| 230 | |||
| 231 | return e_X; | ||
| 232 | } | ||
| 233 | |||
| 234 | /// @brief Find (L^T D L)-decomposition of Q-matrix via outer product method | ||
| 235 | /// @param[in] Qmatrix Symmetric positive definite matrix to be factored | ||
| 236 | /// @return L - Factor matrix (strict lower triangular) | ||
| 237 | /// @return D - Vector with entries of the diagonal matrix | ||
| 238 | /// @note See \cite deJonge1996 de Jonge 1996, Algorithm FMFAC5 | ||
| 239 | /// @attention Consider using NAV::math::LtDLdecomp_choleskyFact() because it is faster by up to a factor 10 | ||
| 240 | template<typename Derived> | ||
| 241 | std::optional<std::pair<Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>, | ||
| 242 | Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>>> | ||
| 243 | 1 | LtDLdecomp_outerProduct(const Eigen::MatrixBase<Derived>& Qmatrix) | |
| 244 | { | ||
| 245 | using Eigen::seq; | ||
| 246 | |||
| 247 | 1 | auto n = Qmatrix.rows(); | |
| 248 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> Q = Qmatrix.template triangularView<Eigen::Lower>(); |
| 249 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> L; |
| 250 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> D; |
| 251 | |||
| 252 | if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) | ||
| 253 | { | ||
| 254 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | L = Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(n, n); |
| 255 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | D.setZero(n); |
| 256 | } | ||
| 257 | else | ||
| 258 | { | ||
| 259 | L = Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>::Zero(); | ||
| 260 | D.setZero(); | ||
| 261 | } | ||
| 262 | |||
| 263 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | for (Eigen::Index i = n - 1; i >= 0; i--) |
| 264 | { | ||
| 265 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | D(i) = Q(i, i); |
| 266 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
|
3 | if (Q(i, i) <= 0.0) { return {}; } |
| 267 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
|
3 | L(i, seq(0, i)) = Q(i, seq(0, i)) / std::sqrt(Q(i, i)); |
| 268 | |||
| 269 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
|
6 | for (Eigen::Index j = 0; j <= i - 1; j++) |
| 270 | { | ||
| 271 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
|
3 | Q(j, seq(0, j)) -= L(i, seq(0, j)) * L(i, j); |
| 272 | } | ||
| 273 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
|
3 | L(i, seq(0, i)) /= L(i, i); |
| 274 | } | ||
| 275 | |||
| 276 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | return std::make_pair(L, D); |
| 277 | 1 | } | |
| 278 | |||
| 279 | /// @brief Find (L^T D L)-decomposition of Q-matrix via a backward Cholesky factorization in a bordering method formulation | ||
| 280 | /// @param[in] Q Symmetric positive definite matrix to be factored | ||
| 281 | /// @return L - Factor matrix (strict lower triangular) | ||
| 282 | /// @return D - Vector with entries of the diagonal matrix | ||
| 283 | /// @note See \cite deJonge1996 de Jonge 1996, Algorithm FMFAC6 | ||
| 284 | template<typename Derived> | ||
| 285 | std::optional<std::pair<Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>, | ||
| 286 | Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>>> | ||
| 287 | 2423 | LtDLdecomp_choleskyFact(const Eigen::MatrixBase<Derived>& Q) | |
| 288 | { | ||
| 289 | using Eigen::seq; | ||
| 290 | |||
| 291 | 2423 | auto n = Q.rows(); | |
| 292 |
2/4✓ Branch 1 taken 1213 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1213 times.
✗ Branch 5 not taken.
|
2423 | typename Derived::PlainObject L = Q.template triangularView<Eigen::Lower>(); |
| 293 |
1/2✓ Branch 1 taken 1213 times.
✗ Branch 2 not taken.
|
2423 | Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> D; |
| 294 | 2423 | double cmin = 1; | |
| 295 | |||
| 296 |
1/2✓ Branch 1 taken 1206 times.
✗ Branch 2 not taken.
|
2409 | if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) { D.setZero(n); } |
| 297 | else | ||
| 298 | { | ||
| 299 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | D.setZero(); |
| 300 | } | ||
| 301 | |||
| 302 |
2/2✓ Branch 0 taken 32377 times.
✓ Branch 1 taken 1213 times.
|
67060 | for (Eigen::Index j = n - 1; j >= 0; j--) |
| 303 | { | ||
| 304 |
2/2✓ Branch 0 taken 448444 times.
✓ Branch 1 taken 32377 times.
|
959299 | for (Eigen::Index i = n - 1; i >= j + 1; i--) |
| 305 | { | ||
| 306 |
8/16✓ Branch 1 taken 448444 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 448444 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 448444 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 448444 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 448444 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 448444 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 448444 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 448444 times.
✗ Branch 23 not taken.
|
894662 | L(i, j) = (L(i, j) - L(seq(i + 1, n - 1), j).dot(L(seq(i + 1, n - 1), i))) / L(i, i); |
| 307 | } | ||
| 308 |
6/12✓ Branch 1 taken 32377 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32377 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32377 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32377 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32377 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 32377 times.
✗ Branch 17 not taken.
|
64637 | double t = L(j, j) - L(seq(j + 1, n - 1), j).dot(L(seq(j + 1, n - 1), j)); |
| 309 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 32377 times.
|
64637 | if (t <= 0.0) { return {}; } |
| 310 |
1/2✓ Branch 1 taken 32377 times.
✗ Branch 2 not taken.
|
64637 | double c = t / L(j, j); |
| 311 | 64637 | cmin = std::min(c, cmin); | |
| 312 |
1/2✓ Branch 1 taken 32377 times.
✗ Branch 2 not taken.
|
64637 | L(j, j) = std::sqrt(t); |
| 313 | } | ||
| 314 |
2/2✓ Branch 0 taken 32377 times.
✓ Branch 1 taken 1213 times.
|
67060 | for (Eigen::Index i = 0; i < n; i++) |
| 315 | { | ||
| 316 |
4/8✓ Branch 1 taken 32377 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32377 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32377 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32377 times.
✗ Branch 11 not taken.
|
64637 | L.row(i).leftCols(i) /= L(i, i); |
| 317 |
2/4✓ Branch 1 taken 32377 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32377 times.
✗ Branch 5 not taken.
|
64637 | D(i) = std::pow(L(i, i), 2.0); |
| 318 |
1/2✓ Branch 1 taken 32377 times.
✗ Branch 2 not taken.
|
64637 | L(i, i) = 1; |
| 319 | } | ||
| 320 | |||
| 321 |
1/2✓ Branch 1 taken 1213 times.
✗ Branch 2 not taken.
|
2423 | return std::make_pair(L, D); |
| 322 | 2409 | } | |
| 323 | |||
| 324 | /// @brief Calculates the squared norm of the vector and matrix | ||
| 325 | /// | ||
| 326 | /// \anchor eq-squaredNorm \f{equation}{ \label{eq:eq-squaredNorm} | ||
| 327 | /// ||\mathbf{\dots}||^2_{\mathbf{Q}} = (\dots)^T \mathbf{Q}^{-1} (\dots) | ||
| 328 | /// \f} | ||
| 329 | /// @param a Vector | ||
| 330 | /// @param Q Covariance matrix of the vector | ||
| 331 | /// @return Squared norm | ||
| 332 | template<typename DerivedA, typename DerivedQ> | ||
| 333 | 8 | typename DerivedA::Scalar squaredNormVectorMatrix(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Q) | |
| 334 | { | ||
| 335 | static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1); | ||
| 336 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | INS_ASSERT_USER_ERROR(a.cols() == 1, "Parameter 'a' has to be a vector"); |
| 337 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | INS_ASSERT_USER_ERROR(a.rows() == Q.rows(), "Parameter 'a' and 'Q' need to have same size"); |
| 338 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
8 | INS_ASSERT_USER_ERROR(Q.cols() == Q.rows(), "Parameter 'Q' needs to be quadratic"); |
| 339 | |||
| 340 |
5/10✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
|
8 | return a.transpose() * Q.inverse() * a; |
| 341 | } | ||
| 342 | |||
| 343 | /// @brief Calculates the cumulative distribution function (CDF) of the standard normal distribution | ||
| 344 | /// | ||
| 345 | /// \anchor eq-normalDistCDF \f{equation}{ \label{eq:eq-normalDistCDF} | ||
| 346 | /// \Phi(x) = \int\displaylimits_{-\infty}^x \frac{1}{\sqrt{2\pi}} \exp{\left(-\frac{1}{2} v^2\right)} \text{d}v | ||
| 347 | /// \f} | ||
| 348 | /// which can be expressed with the error function | ||
| 349 | /// \anchor eq-normalDistCDF-erf \f{equation}{ \label{eq:eq-normalDistCDF-erf} | ||
| 350 | /// \Phi(x) = \frac{1}{2} \left[ 1 + \text{erf}{\left(\frac{x}{\sqrt{2}}\right)} \right] | ||
| 351 | /// \f} | ||
| 352 | /// Using the property | ||
| 353 | /// \anchor eq-erf-minus \f{equation}{ \label{eq:eq-erf-minus} | ||
| 354 | /// \text{erf}{\left( -x \right)} = -\text{erf}{\left( x \right)} | ||
| 355 | /// \f} | ||
| 356 | /// and the complementary error function | ||
| 357 | /// \anchor eq-erfc \f{equation}{ \label{eq:eq-erfc} | ||
| 358 | /// \text{erfc}{\left( x \right)} = 1 - \text{erf}{\left( x \right)} | ||
| 359 | /// \f} | ||
| 360 | /// we can simplify equation \eqref{eq-normalDistCDF-erf} to | ||
| 361 | /// \anchor eq-normalDistCDF-erfc \f{equation}{ \label{eq:eq-normalDistCDF-erfc} | ||
| 362 | /// \begin{aligned} | ||
| 363 | /// \Phi(x) &= \frac{1}{2} \left[ 1 - \text{erf}{\left(-\frac{x}{\sqrt{2}}\right)} \right] \\ | ||
| 364 | /// &= \frac{1}{2} \text{erfc}{\left(-\frac{x}{\sqrt{2}}\right)} | ||
| 365 | /// \end{aligned} | ||
| 366 | /// \f} | ||
| 367 | /// | ||
| 368 | /// @param value Value to calculate the CDF for | ||
| 369 | double normalCDF(double value); | ||
| 370 | |||
| 371 | /// @brief Returns the inverse square root of a matrix | ||
| 372 | /// @param matrix Matrix to use | ||
| 373 | template<typename Derived> | ||
| 374 | [[nodiscard]] typename Derived::PlainObject inverseSqrt(const Eigen::MatrixBase<Derived>& matrix) | ||
| 375 | { | ||
| 376 | INS_ASSERT_USER_ERROR(matrix.rows() == matrix.cols(), "Only square matrix supported"); | ||
| 377 | if constexpr (std::is_floating_point_v<typename Derived::Scalar>) | ||
| 378 | { | ||
| 379 | return matrix.inverse().sqrt(); // Eigen::SelfAdjointEigenSolver<Eigen::MatrixX<T>>{ covMatrix }.operatorInverseSqrt(); | ||
| 380 | } | ||
| 381 | else // Eigen gets problems with ceres::Jet in the .sqrt() function | ||
| 382 | { | ||
| 383 | Eigen::JacobiSVD<Eigen::MatrixX<typename Derived::Scalar>> svd(matrix.inverse(), Eigen::ComputeFullV); | ||
| 384 | typename Derived::PlainObject sqrtInverse = svd.matrixV() * svd.singularValues().cwiseSqrt().asDiagonal() * svd.matrixV().transpose(); | ||
| 385 | INS_ASSERT_USER_ERROR(!sqrtInverse.hasNaN(), "The matrix is not invertible"); | ||
| 386 | return sqrtInverse; | ||
| 387 | } | ||
| 388 | } | ||
| 389 | |||
| 390 | /// @brief Change the sign of x according to the value of y | ||
| 391 | /// @param[in] x input value | ||
| 392 | /// @param[in] y input value | ||
| 393 | /// @return -x or +x | ||
| 394 | template<typename T> | ||
| 395 | 2612 | T sign(const T& x, const T& y) | |
| 396 | { | ||
| 397 | // similar function 'sign' in fortran | ||
| 398 |
2/2✓ Branch 0 taken 498 times.
✓ Branch 1 taken 2114 times.
|
2612 | if (y >= 0.0) |
| 399 | { | ||
| 400 | 498 | return fabs(x); | |
| 401 | } | ||
| 402 | 2114 | return -1.0 * fabs(x); | |
| 403 | } | ||
| 404 | |||
| 405 | /// @brief Linear interpolation between vectors | ||
| 406 | /// @param a Left value | ||
| 407 | /// @param b Right value | ||
| 408 | /// @param t Multiplier. [0, 1] for interpolation | ||
| 409 | /// @return a + t * (b - a) | ||
| 410 | template<typename Derived> | ||
| 411 | ✗ | typename Derived::PlainObject lerp(const Eigen::MatrixBase<Derived>& a, const Eigen::MatrixBase<Derived>& b, auto t) | |
| 412 | { | ||
| 413 | ✗ | return a + t * (b - a); | |
| 414 | } | ||
| 415 | |||
| 416 | /// Lerp Search Result | ||
| 417 | struct LerpSearchResult | ||
| 418 | { | ||
| 419 | size_t l; ///< Lower bound index | ||
| 420 | size_t u; ///< Upper bound index (l + 1) | ||
| 421 | double t; ///< [0, 1] for Interpolation, otherwise Extrapolation | ||
| 422 | }; | ||
| 423 | |||
| 424 | /// @brief Searches the value in the data container | ||
| 425 | /// @param[in] data Data container | ||
| 426 | /// @param[in] value Value to search | ||
| 427 | 3 | LerpSearchResult lerpSearch(const auto& data, const auto& value) | |
| 428 | { | ||
| 429 | 3 | auto i = static_cast<size_t>(std::distance(data.begin(), std::upper_bound(data.begin(), data.end(), value))); | |
| 430 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (i > 0) { i--; } |
| 431 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (i == data.size() - 1) { i--; } |
| 432 | 3 | const auto& lb = data.at(i); | |
| 433 | 3 | const auto& ub = data.at(i + 1); | |
| 434 | 3 | double t = (value - lb) / (ub - lb); | |
| 435 | |||
| 436 | 3 | return { .l = i, .u = i + 1, .t = t }; | |
| 437 | } | ||
| 438 | |||
| 439 | /// @brief Bilinear interpolation | ||
| 440 | /// @param[in] tx Distance in x component to interpolate [0, 1] | ||
| 441 | /// @param[in] ty Distance in y component to interpolate [0, 1] | ||
| 442 | /// @param[in] c00 Value for tx = ty = 0 | ||
| 443 | /// @param[in] c10 Value for tx = 1 and ty = 0 | ||
| 444 | /// @param[in] c01 Value for tx = 0 and ty = 1 | ||
| 445 | /// @param[in] c11 Value for tx = ty = 1 | ||
| 446 | /// | ||
| 447 | /// c01 ------ c11 | ||
| 448 | /// | | | ||
| 449 | /// | | | ||
| 450 | /// | | | ||
| 451 | /// c00 ------ c10 | ||
| 452 | /// | ||
| 453 | /// @note See https://www.scratchapixel.com/lessons/mathematics-physics-for-computer-graphics/interpolation/bilinear-filtering.html | ||
| 454 | 9 | auto bilinearInterpolation(const auto& tx, const auto& ty, | |
| 455 | const auto& c00, const auto& c10, | ||
| 456 | const auto& c01, const auto& c11) | ||
| 457 | { | ||
| 458 | 9 | auto a = c00 * (1.0 - tx) + c10 * tx; | |
| 459 | 9 | auto b = c01 * (1.0 - tx) + c11 * tx; | |
| 460 | 9 | return a * (1.0 - ty) + b * ty; | |
| 461 | // Alternative implementation: | ||
| 462 | // return (1.0 - tx) * (1.0 - ty) * c00 + tx * (1.0 - ty) * c10 + (1.0 - tx) * ty * c01 + tx * ty * c11; | ||
| 463 | } | ||
| 464 | |||
| 465 | /// @brief Calculates the incomplete elliptical integral of the second kind | ||
| 466 | /// @param[in] phi Interval bound the integration uses from 0 to phi | ||
| 467 | /// @param[in] m Function parameter that is integrated 1-m*sin(t)^2 | ||
| 468 | /// @return Incomplete elliptical integral of the second kind | ||
| 469 | /// @note See http://www2.iap.fr/users/pichon/doc/html_xref/elliptic-es.html | ||
| 470 | double calcEllipticalIntegral(double phi, double m); | ||
| 471 | |||
| 472 | } // namespace NAV::math | ||
| 473 |