INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/Sort.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 86 91 94.5%
Functions: 20 20 100.0%
Branches: 60 96 62.5%
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 Sort.hpp
10 /// @brief Sorting algorithms
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-09-15
13
14 #pragma once
15
16 #include <vector>
17 #include <algorithm>
18 #include <numeric>
19 #include "util/Eigen.hpp"
20
21 namespace NAV
22 {
23
24 /// @brief Gives the permutation to sort the std::vector
25 /// @param vec Vector to get the sorting permutation for
26 /// @param compare Comparison operator
27 /// @return Permutation vector
28 /// @note Taken from https://stackoverflow.com/a/17074810
29 template<typename T, typename Compare>
30 1 std::vector<size_t> sort_permutation(const std::vector<T>& vec, Compare compare)
31 {
32
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::vector<size_t> p(vec.size());
33 1 std::iota(p.begin(), p.end(), 0); // NOLINT(boost-use-ranges,modernize-use-ranges) // ranges::iota is C++23 and not supported yet
34
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 11 std::ranges::sort(p, [&](size_t i, size_t j) { return compare(vec[i], vec[j]); });
35 1 return p;
36 ✗ }
37
38 /// @brief Gives the permutation to sort the vector
39 /// @param vec Vector to get the sorting permutation for
40 /// @param compare Comparison operator
41 /// @return Permutation vector
42 /// @note Taken from https://stackoverflow.com/a/17074810, adapted for Eigen
43 template<typename Derived, typename Compare>
44 218 std::vector<size_t> sort_permutation(const Eigen::MatrixBase<Derived>& vec, Compare compare)
45 {
46
1/2
✓ Branch 2 taken 218 times.
✗ Branch 3 not taken.
 218 std::vector<size_t> p(static_cast<size_t>(vec.rows()));
47 218 std::iota(p.begin(), p.end(), 0); // NOLINT(boost-use-ranges,modernize-use-ranges) // ranges::iota is C++23 and not supported yet
48
1/2
✓ Branch 1 taken 218 times.
✗ Branch 2 not taken.
 580 std::ranges::sort(p, [&](size_t i, size_t j) { return compare(vec(static_cast<Eigen::Index>(i)), vec(static_cast<Eigen::Index>(j))); });
49 218 return p;
50 ✗ }
51
52 /// @brief Sort a std::vector with a permutation
53 /// @param vec Vector to sort
54 /// @param p Permutation to sort by
55 /// @return Sorted vector
56 /// @note Taken from https://stackoverflow.com/a/17074810
57 template<typename T>
58 2 std::vector<T> apply_permutation(const std::vector<T>& vec, const std::vector<size_t>& p)
59 {
60
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 std::vector<T> sorted_vec(vec.size());
61 2 std::transform(p.begin(), p.end(), sorted_vec.begin(),
62 10 [&](size_t i) { return vec[i]; });
63 2 return sorted_vec;
64 }
65
66 /// @brief Sort a vector with a permutation
67 /// @param vec Vector to sort
68 /// @param p Permutation to sort by
69 /// @return Sorted vector
70 /// @note Taken from https://stackoverflow.com/a/17074810, adapted for Eigen
71 template<typename Scalar, int Size>
72 1 Eigen::Vector<Scalar, Size> apply_permutation(const Eigen::Vector<Scalar, Size>& vec, const std::vector<size_t>& p)
73 {
74 1 Eigen::Vector<Scalar, Size> sorted = vec;
75
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::transform(p.begin(), p.end(), sorted.begin(),
76 5 [&](size_t i) { return vec(static_cast<Eigen::Index>(i)); });
77 1 return sorted;
78 ✗ }
79
80 /// @brief Sort a matrix row-wise with a permutation
81 /// @param m Matrix to sort
82 /// @param p Permutation to sort by
83 /// @return Sorted matrix
84 /// @note Taken from https://stackoverflow.com/a/17074810, adapted for Eigen
85 template<typename Derived>
86 1 typename Derived::PlainObject apply_permutation_rowwise(const Eigen::MatrixBase<Derived>& m, const std::vector<size_t>& p)
87 {
88 1 typename Derived::PlainObject sorted = m;
89
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 std::transform(p.begin(), p.end(), sorted.rowwise().begin(),
90 5 [&](size_t i) { return m.row(static_cast<Eigen::Index>(i)); });
91 1 return sorted;
92 ✗ }
93
94 /// @brief Sort a matrix col-wise with a permutation
95 /// @param m Matrix to sort
96 /// @param p Permutation to sort by
97 /// @return Sorted matrix
98 /// @note Taken from https://stackoverflow.com/a/17074810, adapted for Eigen
99 template<typename Derived>
100 1 typename Derived::PlainObject apply_permutation_colwise(const Eigen::MatrixBase<Derived>& m, const std::vector<size_t>& p)
101 {
102 1 typename Derived::PlainObject sorted = m;
103
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 std::transform(p.begin(), p.end(), sorted.colwise().begin(),
104 5 [&](size_t i) { return m.col(static_cast<Eigen::Index>(i)); });
105 1 return sorted;
106 ✗ }
107
108 /// @brief Sort a std::vector with a permutation
109 /// @param vec Vector to sort
110 /// @param p Permutation to sort by
111 template<typename T>
112 2 void apply_permutation_in_place(std::vector<T>& vec, const std::vector<size_t>& p)
113 {
114
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 std::vector<bool> done(p.size());
115
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 2 times.
 12 for (size_t i = 0; i < p.size(); ++i)
116 {
117
3/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✓ Branch 5 taken 2 times.
 10 if (done[i])
118 {
119 8 continue;
120 }
121
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 done[i] = true;
122 2 size_t prev_j = i;
123 2 size_t j = p[i];
124
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 2 times.
 10 while (i != j)
125 {
126 8 std::swap(vec[prev_j], vec[j]);
127
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 done[j] = true;
128 8 prev_j = j;
129 8 j = p[j];
130 }
131 }
132 2 }
133
134 /// @brief Sort a vector with a permutation
135 /// @param vec Vector to sort
136 /// @param p Permutation to sort by
137 /// @note Taken from https://stackoverflow.com/a/17074810
138 template<typename Scalar, int Size>
139 218 void apply_permutation_in_place(Eigen::Vector<Scalar, Size>& vec, const std::vector<size_t>& p)
140 {
141
1/2
✓ Branch 2 taken 218 times.
✗ Branch 3 not taken.
 218 std::vector<bool> done(p.size());
142
2/2
✓ Branch 1 taken 449 times.
✓ Branch 2 taken 218 times.
 667 for (size_t i = 0; i < p.size(); ++i)
143 {
144
3/4
✓ Branch 1 taken 449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✓ Branch 5 taken 333 times.
 449 if (done[i])
145 {
146 116 continue;
147 }
148
1/2
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
 333 done[i] = true;
149 333 size_t prev_j = i;
150 333 size_t j = p[i];
151
2/2
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 333 times.
 449 while (i != j)
152 {
153
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 std::swap(vec(static_cast<Eigen::Index>(prev_j)), vec(static_cast<Eigen::Index>(j)));
154
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 done[j] = true;
155 116 prev_j = j;
156 116 j = p[j];
157 }
158 }
159 218 }
160
161 /// @brief Sort a matrix row-wise with a permutation
162 /// @param m Matrix to sort
163 /// @param p Permutation to sort by
164 /// @note Taken from https://stackoverflow.com/a/17074810
165 template<typename Scalar, int Rows, int Cols>
166 1 void apply_permutation_rowwise_in_place(Eigen::Matrix<Scalar, Rows, Cols>& m, const std::vector<size_t>& p)
167 {
168
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::vector<bool> done(p.size());
169
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 for (size_t i = 0; i < p.size(); ++i)
170 {
171
3/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 1 times.
 5 if (done[i])
172 {
173 4 continue;
174 }
175
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 done[i] = true;
176 1 size_t prev_j = i;
177 1 size_t j = p[i];
178
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
 5 while (i != j)
179 {
180
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 m.row(static_cast<Eigen::Index>(prev_j)).swap(m.row(static_cast<Eigen::Index>(j)));
181
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 done[j] = true;
182 4 prev_j = j;
183 4 j = p[j];
184 }
185 }
186 1 }
187
188 /// @brief Sort a matrix col-wise with a permutation
189 /// @param m Matrix to sort
190 /// @param p Permutation to sort by
191 /// @note Taken from https://stackoverflow.com/a/17074810
192 template<typename Scalar, int Rows, int Cols>
193 218 void apply_permutation_colwise_in_place(Eigen::Matrix<Scalar, Rows, Cols>& m, const std::vector<size_t>& p)
194 {
195
1/2
✓ Branch 2 taken 218 times.
✗ Branch 3 not taken.
 218 std::vector<bool> done(p.size());
196
2/2
✓ Branch 1 taken 449 times.
✓ Branch 2 taken 218 times.
 667 for (size_t i = 0; i < p.size(); ++i)
197 {
198
3/4
✓ Branch 1 taken 449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✓ Branch 5 taken 333 times.
 449 if (done[i])
199 {
200 116 continue;
201 }
202
1/2
✓ Branch 1 taken 333 times.
✗ Branch 2 not taken.
 333 done[i] = true;
203 333 size_t prev_j = i;
204 333 size_t j = p[i];
205
2/2
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 333 times.
 449 while (i != j)
206 {
207
3/6
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 116 times.
✗ Branch 8 not taken.
 116 m.col(static_cast<Eigen::Index>(prev_j)).swap(m.col(static_cast<Eigen::Index>(j)));
208
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 done[j] = true;
209 116 prev_j = j;
210 116 j = p[j];
211 }
212 }
213 218 }
214
215 } // namespace NAV
216