INSTINCT Code Coverage Report


Directory: src/
File: Navigation/INS/SensorCombiner/BsplineKF/QuadraticBsplines.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 63 67 94.0%
Functions: 1 1 100.0%
Branches: 105 178 59.0%

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 #include "QuadraticBsplines.hpp"
10
11 #include <cmath>
12 #include <array>
13
14 #include "util/Logger.hpp"
15
16 14964 std::array<double, 3> NAV::BsplineKF::quadraticBsplines(const double& ti, const double& splineSpacing)
17 {
18 14964 std::array<double, 3> qBsplines{ 0.0, 0.0, 0.0 }; // Stacked B-spline, which consists of three single B-splines
19 14964 std::array<double, 4> knots{}; // Knot vector for a single quadratic B-spline
20
21
2/2
✓ Branch 0 taken 44892 times.
✓ Branch 1 taken 14964 times.
59856 for (size_t splineIterator = 0; splineIterator < 3; splineIterator++)
22 {
23
2/2
✓ Branch 0 taken 43404 times.
✓ Branch 1 taken 1488 times.
44892 if (ti >= 2.0 * splineSpacing)
24 {
25
1/2
✓ Branch 1 taken 43404 times.
✗ Branch 2 not taken.
43404 knots.at(3) = ti - std::fmod(ti, splineSpacing) + splineSpacing * static_cast<double>(splineIterator + 1); // t_k+1
26
2/4
✓ Branch 1 taken 43404 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43404 times.
✗ Branch 5 not taken.
43404 knots.at(2) = knots.at(3) - splineSpacing; // t_k
27
2/4
✓ Branch 1 taken 43404 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43404 times.
✗ Branch 5 not taken.
43404 knots.at(1) = knots.at(3) - 2.0 * splineSpacing; // t_k-1
28
2/4
✓ Branch 1 taken 43404 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43404 times.
✗ Branch 5 not taken.
43404 knots.at(0) = knots.at(3) - 3.0 * splineSpacing; // t_k-2
29 }
30
3/4
✓ Branch 0 taken 744 times.
✓ Branch 1 taken 744 times.
✓ Branch 2 taken 744 times.
✗ Branch 3 not taken.
1488 else if ((ti >= splineSpacing) && (ti < 2.0 * splineSpacing))
31 {
32
3/4
✓ Branch 0 taken 248 times.
✓ Branch 1 taken 248 times.
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
744 switch (splineIterator)
33 {
34 248 case 0:
35 248 knots = { 0.0, 0.0, 1.0 * splineSpacing, 2.0 * splineSpacing };
36 248 break;
37 248 case 1:
38 248 knots = { 0.0, 1.0 * splineSpacing, 2.0 * splineSpacing, 3.0 * splineSpacing };
39 248 break;
40 248 case 2:
41 248 knots = { 1.0 * splineSpacing, 2.0 * splineSpacing, 3.0 * splineSpacing, 4.0 * splineSpacing };
42 248 break;
43
44 default:
45 break;
46 }
47 }
48
1/2
✓ Branch 0 taken 744 times.
✗ Branch 1 not taken.
744 else if (ti < 1.0 * splineSpacing)
49 {
50
3/4
✓ Branch 0 taken 248 times.
✓ Branch 1 taken 248 times.
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
744 switch (splineIterator)
51 {
52 248 case 0:
53 248 knots = { 0.0, 0.0, 0.0, 1.0 * splineSpacing };
54 248 break;
55 248 case 1:
56 248 knots = { 0.0, 0.0, 1.0 * splineSpacing, 2.0 * splineSpacing };
57 248 break;
58 248 case 2:
59 248 knots = { 0.0, 1.0 * splineSpacing, 2.0 * splineSpacing, 3.0 * splineSpacing };
60 248 break;
61
62 default:
63 break;
64 }
65 }
66
67 LOG_DATA("ti = {}, B-spline '{}', knots = {}, {}, {}, {}", ti, splineIterator, knots.at(0), knots.at(1), knots.at(2), knots.at(3));
68
69 // B-spline of degree 0
70 44892 std::array<double, 3> N0{};
71
7/10
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44892 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 44892 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 14964 times.
✓ Branch 9 taken 29928 times.
✓ Branch 10 taken 14964 times.
✓ Branch 11 taken 29928 times.
44892 if ((knots.at(0) <= ti) && (ti < knots.at(1)))
72 {
73
1/2
✓ Branch 1 taken 14964 times.
✗ Branch 2 not taken.
14964 N0.at(0) = 1.0;
74 }
75
7/10
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29928 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 29928 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 14964 times.
✓ Branch 9 taken 14964 times.
✓ Branch 10 taken 14964 times.
✓ Branch 11 taken 14964 times.
29928 else if ((knots.at(1) <= ti) && (ti < knots.at(2)))
76 {
77
1/2
✓ Branch 1 taken 14964 times.
✗ Branch 2 not taken.
14964 N0.at(1) = 1.0;
78 }
79
5/10
✓ Branch 1 taken 14964 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14964 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14964 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 14964 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 14964 times.
✗ Branch 11 not taken.
14964 else if ((knots.at(2) <= ti) && (ti < knots.at(3)))
80 {
81
1/2
✓ Branch 1 taken 14964 times.
✗ Branch 2 not taken.
14964 N0.at(2) = 1.0;
82 }
83
84 // B-spline of degree 1
85 44892 std::array<double, 2> N1{};
86 44892 double N11_factor1{};
87 44892 double N11_factor2{};
88 44892 double N12_factor1{};
89 44892 double N12_factor2{};
90
91
7/10
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44892 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 44892 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 29928 times.
✓ Branch 9 taken 14964 times.
✓ Branch 10 taken 29928 times.
✓ Branch 11 taken 14964 times.
44892 if ((knots.at(0) <= ti) && (ti < knots.at(2)))
92 {
93
4/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 29680 times.
✓ Branch 7 taken 248 times.
29928 if (knots.at(1) - knots.at(0) != 0.0)
94 {
95
3/6
✓ Branch 1 taken 29680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29680 times.
✗ Branch 8 not taken.
29680 N11_factor1 = (ti - knots.at(0)) / (knots.at(1) - knots.at(0));
96 }
97
3/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 29928 times.
✗ Branch 7 not taken.
29928 if (knots.at(2) - knots.at(1) != 0.0)
98 {
99
3/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29928 times.
✗ Branch 8 not taken.
29928 N11_factor2 = (knots.at(2) - ti) / (knots.at(2) - knots.at(1));
100 }
101
3/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29928 times.
✗ Branch 8 not taken.
29928 N1.at(0) = N11_factor1 * N0.at(0) + N11_factor2 * N0.at(1);
102 }
103
7/10
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29928 times.
✓ Branch 4 taken 14964 times.
✓ Branch 6 taken 29928 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 29928 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 29928 times.
✓ Branch 11 taken 14964 times.
44892 if ((knots.at(1) <= ti) && (ti < knots.at(3)))
104 {
105
4/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 29680 times.
✓ Branch 7 taken 248 times.
29928 if (knots.at(2) - knots.at(1) != 0.0)
106 {
107
3/6
✓ Branch 1 taken 29680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29680 times.
✗ Branch 8 not taken.
29680 N12_factor1 = (ti - knots.at(1)) / (knots.at(2) - knots.at(1));
108 }
109
3/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 29928 times.
✗ Branch 7 not taken.
29928 if (knots.at(3) - knots.at(2) != 0.0)
110 {
111
3/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29928 times.
✗ Branch 8 not taken.
29928 N12_factor2 = (knots.at(3) - ti) / (knots.at(3) - knots.at(2));
112 }
113
3/6
✓ Branch 1 taken 29928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29928 times.
✗ Branch 8 not taken.
29928 N1.at(1) = N12_factor1 * N0.at(1) + N12_factor2 * N0.at(2);
114 }
115
116 // B-spline of degree 2 (i.e. quadratic)
117 44892 double N2_factor1{};
118 44892 double N2_factor2{};
119
120
4/6
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44892 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 44644 times.
✓ Branch 7 taken 248 times.
44892 if (knots.at(2) - knots.at(0) != 0.0)
121 {
122
3/6
✓ Branch 1 taken 44644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 44644 times.
✗ Branch 8 not taken.
44644 N2_factor1 = (ti - knots.at(0)) / (knots.at(2) - knots.at(0));
123 }
124
3/6
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44892 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 44892 times.
✗ Branch 7 not taken.
44892 if (knots.at(3) - knots.at(1) != 0.0)
125 {
126
3/6
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44892 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 44892 times.
✗ Branch 8 not taken.
44892 N2_factor2 = (knots.at(3) - ti) / (knots.at(3) - knots.at(1));
127 }
128
3/6
✓ Branch 1 taken 44892 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44892 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 44892 times.
✗ Branch 8 not taken.
44892 qBsplines.at(splineIterator) = N2_factor1 * N1.at(0) + N2_factor2 * N1.at(1);
129 }
130
131 29928 return qBsplines;
132 }
133